SLAM+语音机器人DIY系列:(三)感知与大脑——2.带自校准九轴数据融合IMU惯性传感器

 温馨提示

本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》,感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习,购买链接如下:

摘要                                              

在我的想象中机器人首先应该能自由的走来走去,然后应该能流利的与主人对话。朝着这个理想,我准备设计一个能自由行走,并且可以与人语音对话的机器人。实现的关键是让机器人能通过传感器感知周围环境,并通过机器人大脑处理并输出反馈和执行动作。本章节涉及到的传感器有激光雷达、IMU、轮式里程计、麦克风、音响、摄像头,和用于处理信息的嵌入式主板。关于传感器的ROS驱动程序开发和在机器人上的使用在后面的章节会展开,本章节重点对机器人传感器和嵌入式主板进行讲解,主要内容:

1.ydlidar-x4激光雷达

2.带自校准九轴数据融合IMU惯性传感器

3.轮式里程计与运动控制

4.音响麦克风与摄像头

5.机器人大脑嵌入式主板性能对比

6.做一个能走路和对话的机器人



2.带自校准九轴数据融合IMU惯性传感器

IMU是惯性测量单元的简称,用于测量物体的三轴姿态角(rollpitchyaw)、三轴加速度(acc_xacc_yacc_z)、三轴角速度(w_xw_yw_z)等。IMU惯性测量单元在制造过程中,由于物理因素,导致IMU惯性测量单元实际的坐标轴与理想的坐标轴之间会有一定的偏差,同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。这里提到的自校准就是要通过给的补偿值来减小或消除坐标轴的偏差及原始值的固定偏差,也就是所谓的IMU内部标定。如果将IMU安装到机器人或摄像头上后,需要知道IMU与机器人或摄像头的相对位置,这个时候进行的标定就是所谓的IMU外部标定。特此说明,这里提到的自校准指IMU内部标定。这里提到的九轴数据融合,是指通过三轴加速度、三轴角速度数据融合得到更加精准的三轴加速度、三轴角速度,同时通过三轴加速度、三轴角速度、三轴磁力计数据融合得到测量物体的三轴姿态角。选用一款带自校准九轴数据融合的IMU,能很好的提升机器人的SLAM建图与导航性能,同时降低机器人上软件的开发难度。出于这一点,我们选用了一款基于MPU9250.带自校准九轴数据融合IMU,如图6

 

(图6)带自校准九轴数据融合IMU惯性传感器

2.1.自校准                  

IMU误差模型:

误差主要来自三部分:噪声(bias and noise)、尺度误差(scale error)、轴偏差(axis misalignment)。通过对这些误差的补偿来实现IMU测量数据的校准,校准的数学表达如图7

(图7校准的数学表达

噪声部分考虑零偏Bias和高斯白噪声noise。零偏Bias也叫随机游走,一般是由传感器内部构造、温度变换多方面综合影响的结果;高斯白噪声noise,一般是由于AD转换引起的一种外部噪声。

尺度误差部分,来自于AD转换中量化过程,比如采样电压值1V对应acc(x)轴的1.4g,同样采样电压值1V对应acc(y)轴的1.6g,也就是说不通的轴上AD转换量化是不同的。

轴偏差部分,三轴加速度、三轴角速度、三轴磁力计的坐标轴严格上都不是正交坐标系。但是最后我们期望的使用值默认是在正交坐标系下测量的,所以就需要将在非正交坐标系测量的原始值变换到正交坐标系中。

加速度校准:

校准过程中需要判断传感器是否处于静止状态,其实很简单,在时间t内(t一般取50s),分别计算acc(x)acc(y)acc(z)三轴数据的方差var[acc(x)]var[acc(y)]var[acc(z)],如果var[acc(x)]+var[acc(y)]+var[acc(z)]小于阈值HH为经验值,需要通过实验法确定),就判断传感器静止。

在静止状态下,加速度计测量值的二范数等于当地重力加速度g。在这一约束条件下,利用最小二乘法进行优化问题求解,由于加速度采样值取得是一个小窗口采样区间的平均值,所以可以忽略白噪声Na的影响,便可以求解出待标定参数TaSaBa。在静止状态取一串加速度采样值共M个,构建代价函数如图8,对代价函数进行最优化求解即可,可以选用ceresg2o等优化工具来完成具体的优化计算过程。

(图8)代价函数

角速度校准:

角速度校准分为两部分:用Allan方差校准零偏Bias、用最优化方法求解尺度误差Sw和轴偏差Tw。加速度校准中需要用到加速度校准信息,所以加速度校准好坏影响整个IMU校准。

同加速度校准一样,也需要在静止状态下采集角速度,简便的做法是在采集加速度的同时也采集角速度。利用角速度采样值计算Allan方差,Allan方差计算比较繁琐就不展开,请直接参考wiki百科:https://en.wikipedia.org/wiki/Allan_variance

Allan方差中共有5个噪声参数:量化噪声Q、角度随机游走N、零偏B、角速度随机游走K、角速度斜坡R。通过图9的步骤便可以求出角速度的零偏Bw,由于角速度采样时是一个小窗口采样区间的平均值,所以可以忽略白噪声Nw的影响。

(图9Allan方差校准零偏Bias

角速度的零偏Bw校准完后,将零偏值Bw带入,接着校准尺度误差Sw和轴偏差Tw。挑选加速度校准过程中两静止状态夹杂的动态片段,这样经过校准后的静态值的平均值可作为acc_calib向量的起始值acc_calib(k-1)和旋转完成后的结束值acc_calib(k)

通过图10的步骤便可以求出角速度的尺度误差Sw和轴偏差Tw

(图10求角速度的尺度误差Sw和轴偏差Tw

磁力计校准:

与加速度校准比较类似,磁力计测量值的二范数等于当地磁场强度m,不同的是磁力的测量不需要在静止状态。在这一约束条件下,利用最小二乘法进行优化问题求解,由于磁力采样值取得是一个小窗口采样区间的平均值,所以可以忽略白噪声Nm的影响,便可以求解出待标定参数TmSmBm。构建代价函数如图11,对代价函数进行最优化求解即可。

(图11代价函数

由于需要用加速度/磁力计进行融合,需要将磁力计测量值变换到加速度坐标系。变换矩阵Tm2a求解比较简单,通过多组关联的加速度acc_calib和磁力计mag_calib,通过最小二次法优化求解,可以求出变换矩阵Tm2a。这里基于一个假设,磁力向量与加速度向量成固定夹角,也就是两向量的点乘为一个常数C,利用这个约束可以构建代价函数如图12,对代价函数进行最优化求解即可。

(图12代价函数

IMU校准软件流程图:

(图13代价函数

IMU手动校准与自校准对比:

通常的校准过程是,将IMU接入PC端,手动将IMU置不同的状态,PC端通过采集这些数据,完成校准。另一种校准过程是,将校准算法内置在IMU模块上的MCU中,在IMU工作的过程中实时采集数据,并自动完成校准,不需要人为的干预。本文介绍的MPU9250模块就属于自校准这一方式。手动校准与自校准对比如图14

(图14手动校准与自校准对比

2.2.九轴数据融合             

IMU的校准完成后,就可以利用校准后的九轴数据进行数据融合,得到IMU的空间姿态了。

MPU9250模块包括三轴加速度计、三轴陀螺仪和三轴磁力计。因为加速度/磁力计具有高频噪声,也就是说它们的瞬时值不够精确,解算出来的姿态会震荡,但长期来看姿态方向是对的。而陀螺仪具有低频噪声,即每个时刻的得到的角速度是比较精确的,使用积分就能得到旋转角度(姿态),但是积分会累积误差,因此积分到后面姿态就不对了,也就是漂移现象。 加速度/磁力计和陀螺仪在频域上的特性互补,可以融合这三种传感器的数据,提高精度和系统的动态特性。

由于欧拉角在求解姿态时存在万向节锁,不能用于全姿态解算;故采用四元数Quaternion进行姿态解算。常用的九轴数据融合算法包括:高低通互补滤波、扩展卡尔曼滤波EKFMahony滤波。在实际情况中根据不同的用途进行选择,这里对这三种滤波方法进行原理性的介绍。

高低通互补滤波:

(图15)高低通互补滤波的过程

高低通互补滤波的过程如图15,通过加权系数融合陀螺仪估计姿态和加速度/磁力计融合姿态,达到对陀螺仪低通滤波、对加速度/磁力计高通滤波的效果。

扩展卡尔曼滤波EKF

和高低通互补滤波过程类似,也是分为估计、校正。

(图16EKF滤波的过程

EKF滤波的过程如图16,想要了解EKF滤波的详细推导请直接参考wiki百科:https://en.wikipedia.org/wiki/Extended_Kalman_filter。估计过程中,利用陀螺仪积分推算出估计姿态;在校正过程中,计算估计姿态下的加速度/磁力计,再用在实际姿态下测量的加速度/磁力计按照EKF校正方程进行校正。

Mahony滤波:

同样,Mahony滤波也分为估计、校正。主要使用了PI控制思想。

(图17Mahony滤波的过程

Mahony滤波的过程如图17。在估计过程中,同样利用陀螺仪积分推算出估计姿态;在校正过程中,计算估计姿态下的加速度/磁力计,测量在实际姿态下的加速度/磁力计,这两个向量叉乘便可以得到姿态误差e,姿态误差e通过PI控制过程可以实现对陀螺仪测量角速度的补偿,利用补偿后的角速度重新估计出来的姿态,就是滤波后的姿态了。

(图18MPU9250模块九轴数据融合后的效果

MPU9250模块九轴数据融合后的效果如图18,图中左侧显示的分别是加速度、角速度、磁力计的实时数据,图中右侧显示的是融合后的姿态实时数据,可以看出融合后的姿态还是非常平稳的。

2.3.在机器人中使用IMU                                

19在机器人中使用MPU9250-IMU模块

MPU9250-IMU模块通过串口与机器人相连接,机器人中通过运行IMUROS驱动,来实现读取串口的IMU数据和将IMU数据发布到/imu这个主题,这样机器人上的其它节点就可以通过订阅/imu主题来获取IMU数据了。关于IMUROS驱动在机器人上的具体使用,将在后面的章节中具体讲解。

后记               

------SLAM+语音机器人DIY系列【目录】快速导览------

第1章:Linux基础

1.Linux简介

2.安装Linux发行版ubuntu系统

3.Linux命令行基础操作

第2章:ROS入门

1.ROS是什么

2.ROS系统整体架构

3.在ubuntu16.04中安装ROS kinetic

4.如何编写ROS的第一个程序hello_world

5.编写简单的消息发布器和订阅器

6.编写简单的service和client

7.理解tf的原理

8.理解roslaunch在大型项目中的作用

9.熟练使用rviz

10.在实际机器人上运行ROS高级功能预览

第3章:感知与大脑

1.ydlidar-x4激光雷达

2.带自校准九轴数据融合IMU惯性传感器

3.轮式里程计与运动控制

4.音响麦克风与摄像头

5.机器人大脑嵌入式主板性能对比

6.做一个能走路和对话的机器人

第4章:差分底盘设计

1.stm32主控硬件设计

2.stm32主控软件设计

3.底盘通信协议

4.底盘ROS驱动开发

5.底盘PID控制参数整定

6.底盘里程计标

第5章:树莓派3开发环境搭建

1.安装系统ubuntu_mate_16.04

2.安装ros-kinetic

3.装机后一些实用软件安装和系统设置

4.PC端与robot端ROS网络通信

5.Android手机端与robot端ROS网络通信

6.树莓派USB与tty串口号绑定

7.开机自启动ROS节点

第6章:SLAM建图与自主避障导航

1.在机器人上使用传感器

2.google-cartographer机器人SLAM建图

3.ros-navigation机器人自主避障导航

4.多目标点导航及任务调度

5.机器人巡航与现场监控

第7章:语音交互与自然语言处理

1.语音交互相关技术

2.机器人语音交互实现

3.自然语言处理云计算引擎

第8章:高阶拓展

1.miiboo机器人安卓手机APP开发

2.centos7下部署Django(nginx+uwsgi+django+python3)

 


 

参考文献

 

[1] 张虎,机器人SLAM导航核心技术与实战[M]. 机械工业出版社,2022.

 

 

 

 

前言
编程基础篇
第1章 ROS入门必备知识
1.1 ROS简介 2
1.1.1 ROS的性能特色 2
1.1.2 ROS的发行版本 3
1.1.3 ROS的学习方法 3
1.2 ROS开发环境的搭建 3
1.2.1 ROS的安装 4
1.2.2 ROS文件的组织方式 4
1.2.3 ROS网络通信配置 5
1.2.4 集成开发工具 5
1.3 ROS系统架构 5
1.3.1 从计算图视角理解ROS架构 6
1.3.2 从文件系统视角理解ROS架构 7
1.3.3 从开源社区视角理解ROS架构 8
1.4 ROS调试工具 8
1.4.1 命令行工具 9
1.4.2 可视化工具 9
1.5 ROS节点通信 10
1.5.1 话题通信方式 12
1.5.2 服务通信方式 15
1.5.3 动作通信方式 19
1.6 ROS的其他重要概念 25
1.7 ROS 2.0展望 28
1.8 本章小结 28
第2章 C++编程范式
2.1 C++工程的组织结构 29
2.1.1 C++工程的一般组织结构 29
2.1.2 C++工程在机器人中的组织结构 29
2.2 C++代码的编译方法 30
2.2.1 使用g++编译代码 31
2.2.2 使用make编译代码 32
2.2.3 使用CMake编译代码 32
2.3 C++编程风格指南 33
2.4 本章小结 34
第3章 OpenCV图像处理
3.1 认识图像数据 35
3.1.1 获取图像数据 35
3.1.2 访问图像数据 36
3.2 图像滤波 37
3.2.1 线性滤波 37
3.2.2 非线性滤波 38
3.2.3 形态学滤波 39
3.3 图像变换 40
3.3.1 射影变换 40
3.3.2 霍夫变换 42
3.3.3 边缘检测 42
3.3.4 直方图均衡 43
3.4 图像特征点提取 44
3.4.1 SIFT特征点 44
3.4.2 SURF特征点 50
3.4.3 ORB特征点 52
3.5 本章小结 54
硬件基础篇
第4章 机器人传感器
4.1 惯性测量单元 56
4.1.1 工作原理 56
4.1.2 原始数据采集 60
4.1.3 参数标定 65
4.1.4 数据滤波 73
4.1.5 姿态融合 75
4.2 激光雷达 91
4.2.1 工作原理 92
4.2.2 性能参数 94
4.2.3 数据处理 96
4.3 相机 100
4.3.1 单目相机 101
4.3.2 双目相机 107
4.3.3 RGB-D相机 109
4.4 带编码器的减速电机 111
4.4.1 电机 111
4.4.2 电机驱动电路 112
4.4.3 电机控制主板 113
4.4.4 轮式里程计 117
4.5 本章小结 118
第5章 机器人主机
5.1 X86与ARM主机对比 119
5.2 ARM主机树莓派3B+ 120
5.2.1 安装Ubuntu MATE 18.04 120
5.2.2 安装ROS melodic 122
5.2.3 装机软件与系统设置 122
5.3 ARM主机RK3399 127
5.4 ARM主机Jetson-tx2 128
5.5 分布式架构主机 129
5.5.1 ROS网络通信 130
5.5.2 机器人程序的远程开发 130
5.6 本章小结 131
第6章 机器人底盘
6.1 底盘运动学模型 132
6.1.1 两轮差速模型 132
6.1.2 四轮差速模型 136
6.1.3 阿克曼模型 140
6.1.4 全向模型 144
6.1.5 其他模型 148
6.2 底盘性能指标 148
6.2.1 载重能力 148
6.2.2 动力性能 148
6.2.3 控制精度 150
6.2.4 里程计精度 150
6.3 典型机器人底盘搭建 151
6.3.1 底盘运动学模型选择 152
6.3.2 传感器选择 152
6.3.3 主机选择 153
6.4 本章小结 155
SLAM篇
第7章 SLAM中的数学基础
7.1 SLAM发展简史 158
7.1.1 数据关联、收敛和一致性 160
7.1.2 SLAM的基本理论 161
7.2 SLAM中的概率理论 163
7.2.1 状态估计问题 164
7.2.2 概率运动模型 166
7.2.3 概率观测模型 171
7.2.4 概率图模型 173
7.3 估计理论 182
7.3.1 估计量的性质 182
7.3.2 估计量的构建 183
7.3.3 各估计量对比 190
7.4 基于贝叶斯网络的状态估计 193
7.4.1 贝叶斯估计 194
7.4.2 参数化实现 196
7.4.3 非参数化实现 202
7.5 基于因子图的状态估计 206
7.5.1 非线性最小二乘估计 206
7.5.2 直接求解方法 206
7.5.3 优化方法 208
7.5.4 各优化方法对比 218
7.5.5 常用优化工具 219
7.6 典型SLAM算法 221
7.7 本章小结 221
第8章 激光SLAM系统
8.1 Gmapping算法 223
8.1.1 原理分析 223
8.1.2 源码解读 228
8.1.3 安装与运行 233
8.2 Cartographer算法 240
8.2.1 原理分析 240
8.2.2 源码解读 247
8.2.3 安装与运行 258
8.3 LOAM算法 266
8.3.1 原理分析 266
8.3.2 源码解读 267
8.3.3 安装与运行 270
8.4 本章小结 270
第9章 视觉SLAM系统
9.1 ORB-SLAM2算法 274
9.1.1 原理分析 274
9.1.2 源码解读 310
9.1.3 安装与运行 319
9.1.4 拓展 327
9.2 LSD-SLAM算法 329
9.2.1 原理分析 329
9.2.2 源码解读 334
9.2.3 安装与运行 337
9.3 SVO算法 338
9.3.1 原理分析 338
9.3.2 源码解读 341
9.4 本章小结 341
第10章 其他SLAM系统
10.1 RTABMAP算法 344
10.1.1 原理分析 344
10.1.2 源码解读 351
10.1.3 安装与运行 357
10.2 VINS算法 362
10.2.1 原理分析 364
10.2.2 源码解读 373
10.2.3 安装与运行 376
10.3 机器学习与SLAM 379
10.3.1 机器学习 379
10.3.2 CNN-SLAM算法 411
10.3.3 DeepVO算法 413
10.4 本章小结 414
自主导航篇
第11章 自主导航中的数学基础
11.1 自主导航 418
11.2 环境感知 420
11.2.1 实时定位 420
11.2.2 环境建模 421
11.2.3 语义理解 422
11.3 路径规划 422
11.3.1 常见的路径规划算法 423
11.3.2 带约束的路径规划算法 430
11.3.3 覆盖的路径规划算法 434
11.4 运动控制 435
11.4.1 基于PID的运动控制 437
11.4.2 基于MPC的运动控制 438
11.4.3 基于强化学习的运动控制 441
11.5 强化学习与自主导航 442
11.5.1 强化学习 443
11.5.2 基于强化学习的自主导航 465
11.6 本章小结 467
第12章 典型自主导航系统
12.1 ros-navigation导航系统 470
12.1.1 原理分析 470
12.1.2 源码解读 475
12.1.3 安装与运行 479
12.1.4 路径规划改进 492
12.1.5 环境探索 496
12.2 riskrrt导航系统 498
12.3 autoware导航系统 499
12.4 导航系统面临的一些挑战 500
12.5 本章小结 500
第13章 机器人SLAM导航综合实战
13.1 运行机器人上的传感器 502
13.1.1 运行底盘的ROS驱动 503
13.1.2 运行激光雷达的ROS驱动 503
13.1.3 运行IMU的ROS驱动 504
13.1.4 运行相机的ROS驱动 504
13.1.5 运行底盘的urdf模型 505
13.1.6 传感器一键启动 506
13.2 运行SLAM建图功能 506
13.2.1 运行激光SLAM建图功能 507
13.2.2 运行视觉SLAM建图功能 508
13.2.3 运行激光与视觉联合建图功能 508
13.3 运行自主导航 509
13.4 基于自主导航的应用 510
13.5 本章小结 511
附录A Linux与SLAM性能优化的探讨
附录B 习题
posted @ 2019-02-19 02:43  小虎哥哥爱学习  阅读(10007)  评论(0编辑  收藏  举报