机器人基础原理
《机器人基础原理》是为机器人工程专业开设的一门专业基础课程。学习对象是大学二年级以上本科生及其他对机器人技术感兴趣的学习人员。
本课程为机器人专业基础课程，涉及到机器人机构组成原理、坐标变换基础、位姿运动学、速度建模与分析、动力学建模与分析、驱动系统、位姿轨迹控制、力控制、智能控制等内容。
本课程的内容涉及面较宽，视频和课件主要是提纲性的，单纯依赖视频学习存在一定的局限性，建议结合本课程推荐的如下参考书同步进行学习。
课程主要参考书：
（1）约翰·克莱格（JohnCraig），机器人学导论(原书第三版或第四版)，机械工业出版社。（2）蔡自兴，谢斌，机器人学（第三版），清华大学出版社。
机器人运动学及动力学建模是本课程的重点讲授内容，学生通过本课程学习可了解掌握机器人的基本原理和建模方法，建立系统完整的基础理论体系，为后续深入学习机器人智能控制等课程打下必要的知识基础。
第1至第8章涵盖了机器人学的基础内容，各章节知识点之间存在一定的先后顺序，建议按章节顺序安排学习。第9章和第10章具有一定的拓展性，可根据自身基础情况来安排。
第11章是Matlab仿真，与第1-8章的理论知识相对应，建议结合理论内容逐步进行。
课程团队教师均为东北大学机器人科学与工程学院的专任教师，主讲教师长期从事机器人领域研究和教学工作，了解掌握机器人领域的历史背景和发展趋势，对机器人技术具有深入的理解和深厚的研究积累，可为课程教学提供有力的支撑。《机器人基础原理》课程可为有志于在机器人领域开展学习并成长为高水平人才的学生提供入门指导。
东北大学机器人科学与工程学院成立于2015年9月，系985高校首个机器人学院。本课程从2016级开始为机器人工程专业本科生讲授，在大二的下学期开设，理论学时32学时，实验学时8学时。
课程团队全体人员欢迎广大学生踊跃参与本课程的学习，并希望所有学习人员学有所成，在未来的机器人热潮中快速成长为栋梁人才。
SLAM+语音机器人DIY系列：（三）感知与大脑——2带自校准九轴数据融合IMU惯性传感器
温馨提示本篇文章已经收录在我最新出版的书籍《机器人SLAM导航核心技术与实战》，感兴趣的读者可以购买纸质书籍来进行更加深入和系统性的学习，购买链接如下：点这里购买：《机器人SLAM导航核心技术与实战》购买链接点这里观看视频讲解：《机器人SLAM导航核心技术与实战》书籍配套教学视频摘要在我的想象中机器人首先应该能自由的走来走去，然后应该能流利的与主人对话。朝着这个理想，我准备设计一个能自由行走，并且可以与人语音对话的机器人。实现的关键是让机器人能通过传感器感知周围环境，并通过机器人大脑处理并输出反馈和执行动作。本章节涉及到的传感器有激光雷达、IMU、轮式里程计、麦克风、音响、摄像头，和用于处理信息的嵌入式主板。关于传感器的ROS驱动程序开发和在机器人上的使用在后面的章节会展开，本章节重点对机器人传感器和嵌入式主板进行讲解，主要内容：
1.ydlidar-x4激光雷达
2.带自校准九轴数据融合IMU惯性传感器
3.轮式里程计与运动控制
4.音响麦克风与摄像头
5.机器人大脑嵌入式主板性能对比
6.做一个能走路和对话的机器人
2.带自校准九轴数据融合IMU惯性传感器IMU是惯性测量单元的简称，用于测量物体的三轴姿态角（roll、pitch、yaw）、三轴加速度（acc_x、acc_y、acc_z）、三轴角速度（w_x、w_y、w_z）等。IMU惯性测量单元在制造过程中，由于物理因素，导致IMU惯性测量单元实际的坐标轴与理想的坐标轴之间会有一定的偏差，同时三轴加速度、三轴角速度、三轴磁力计的原始值会与真实值有一个固定的偏差等。这里提到的自校准就是要通过给的补偿值来减小或消除坐标轴的偏差及原始值的固定偏差，也就是所谓的IMU内部标定。如果将IMU安装到机器人或摄像头上后，需要知道IMU与机器人或摄像头的相对位置，这个时候进行的标定就是所谓的IMU外部标定。特此说明，这里提到的自校准指IMU内部标定。这里提到的九轴数据融合，是指通过三轴加速度、三轴角速度数据融合得到更加精准的三轴加速度、三轴角速度，同时通过三轴加速度、三轴角速度、三轴磁力计数据融合得到测量物体的三轴姿态角。选用一款带自校准九轴数据融合的IMU，能很好的提升机器人的SLAM建图与导航性能，同时降低机器人上软件的开发难度。出于这一点，我们选用了一款基于MPU9250的.带自校准九轴数据融合IMU，如图6。
（图6）带自校准九轴数据融合IMU惯性传感器
2.1.自校准IMU误差模型：
误差主要来自三部分：噪声（biasandnoise）、尺度误差（scaleerror）、轴偏差（axismisalignment）。通过对这些误差的补偿来实现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)]小于阈值H（H为经验值，需要通过实验法确定），就判断传感器静止。
在静止状态下，加速度计测量值的二范数等于当地重力加速度g。在这一约束条件下，利用最小二乘法进行优化问题求解，由于加速度采样值取得是一个小窗口采样区间的平均值，所以可以忽略白噪声Na的影响，便可以求解出待标定参数Ta、Sa、Ba。在静止状态取一串加速度采样值共M个，构建代价函数如图8，对代价函数进行最优化求解即可，可以选用ceres或g2o等优化工具来完成具体的优化计算过程。
（图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的影响。
（图9）用Allan方差校准零偏Bias
角速度的零偏Bw校准完后，将零偏值Bw带入，接着校准尺度误差Sw和轴偏差Tw。挑选加速度校准过程中两静止状态夹杂的动态片段，这样经过校准后的静态值的平均值可作为acc_calib向量的起始值acc_calib(k-1)和旋转完成后的结束值acc_calib(k)。
通过图10的步骤便可以求出角速度的尺度误差Sw和轴偏差Tw。
（图10）求角速度的尺度误差Sw和轴偏差Tw
磁力计校准：
与加速度校准比较类似，磁力计测量值的二范数等于当地磁场强度m，不同的是磁力的测量不需要在静止状态。在这一约束条件下，利用最小二乘法进行优化问题求解，由于磁力采样值取得是一个小窗口采样区间的平均值，所以可以忽略白噪声Nm的影响，便可以求解出待标定参数Tm、Sm、Bm。构建代价函数如图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进行姿态解算。常用的九轴数据融合算法包括：高低通互补滤波、扩展卡尔曼滤波EKF、Mahony滤波。在实际情况中根据不同的用途进行选择，这里对这三种滤波方法进行原理性的介绍。
高低通互补滤波：
（图15）高低通互补滤波的过程
高低通互补滤波的过程如图15,通过加权系数融合陀螺仪估计姿态和加速度/磁力计融合姿态，达到对陀螺仪低通滤波、对加速度/磁力计高通滤波的效果。
扩展卡尔曼滤波EKF：
和高低通互补滤波过程类似，也是分为估计、校正。
（图16）EKF滤波的过程
EKF滤波的过程如图16，想要了解EKF滤波的详细推导请直接参考wiki百科：https://en.wikipedia.org/wiki/Extended_Kalman_filter。估计过程中，利用陀螺仪积分推算出估计姿态；在校正过程中，计算估计姿态下的加速度/磁力计，再用在实际姿态下测量的加速度/磁力计按照EKF校正方程进行校正。
Mahony滤波：
同样，Mahony滤波也分为估计、校正。主要使用了PI控制思想。
（图17）Mahony滤波的过程
Mahony滤波的过程如图17。在估计过程中，同样利用陀螺仪积分推算出估计姿态；在校正过程中，计算估计姿态下的加速度/磁力计，测量在实际姿态下的加速度/磁力计，这两个向量叉乘便可以得到姿态误差e，姿态误差e通过PI控制过程可以实现对陀螺仪测量角速度的补偿，利用补偿后的角速度重新估计出来的姿态，就是滤波后的姿态了。
（图18）MPU9250模块九轴数据融合后的效果
MPU9250模块九轴数据融合后的效果如图18，图中左侧显示的分别是加速度、角速度、磁力计的实时数据，图中右侧显示的是融合后的姿态实时数据，可以看出融合后的姿态还是非常平稳的。
2.3.在机器人中使用IMU（图19）在机器人中使用MPU9250-IMU模块
MPU9250-IMU模块通过串口与机器人相连接，机器人中通过运行IMU的ROS驱动，来实现读取串口的IMU数据和将IMU数据发布到/imu这个主题，这样机器人上的其它节点就可以通过订阅/imu主题来获取IMU数据了。关于IMU的ROS驱动在机器人上的具体使用，将在后面的章节中具体讲解。
后记------SLAM+语音机器人DIY系列【目录】快速导览------
第1章：Linux基础
1.Linux简介
2.安装Linux发行版ubuntu系统
3.Linux命令行基础操作
第2章：ROS入门
1.ROS是什么
2.ROS系统整体架构
3.在ubuntu16.04中安装ROSkinetic
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.1ROS简介21.1.1ROS的性能特色21.1.2ROS的发行版本31.1.3ROS的学习方法31.2ROS开发环境的搭建31.2.1ROS的安装41.2.2ROS文件的组织方式41.2.3ROS网络通信配置51.2.4集成开发工具51.3ROS系统架构51.3.1从计算图视角理解ROS架构61.3.2从文件系统视角理解ROS架构71.3.3从开源社区视角理解ROS架构81.4ROS调试工具81.4.1命令行工具91.4.2可视化工具91.5ROS节点通信101.5.1话题通信方式121.5.2服务通信方式151.5.3动作通信方式191.6ROS的其他重要概念251.7ROS2.0展望281.8本章小结28第2章C++编程范式2.1C++工程的组织结构292.1.1C++工程的一般组织结构292.1.2C++工程在机器人中的组织结构292.2C++代码的编译方法302.2.1使用g++编译代码312.2.2使用make编译代码322.2.3使用CMake编译代码322.3C++编程风格指南332.4本章小结34第3章OpenCV图像处理3.1认识图像数据353.1.1获取图像数据353.1.2访问图像数据363.2图像滤波373.2.1线性滤波373.2.2非线性滤波383.2.3形态学滤波393.3图像变换403.3.1射影变换403.3.2霍夫变换423.3.3边缘检测423.3.4直方图均衡433.4图像特征点提取443.4.1SIFT特征点443.4.2SURF特征点503.4.3ORB特征点523.5本章小结54硬件基础篇第4章机器人传感器4.1惯性测量单元564.1.1工作原理564.1.2原始数据采集604.1.3参数标定654.1.4数据滤波734.1.5姿态融合754.2激光雷达914.2.1工作原理924.2.2性能参数944.2.3数据处理964.3相机1004.3.1单目相机1014.3.2双目相机1074.3.3RGB-D相机1094.4带编码器的减速电机1114.4.1电机1114.4.2电机驱动电路1124.4.3电机控制主板1134.4.4轮式里程计1174.5本章小结118第5章机器人主机5.1X86与ARM主机对比1195.2ARM主机树莓派3B+1205.2.1安装UbuntuMATE18.041205.2.2安装ROSmelodic1225.2.3装机软件与系统设置1225.3ARM主机RK33991275.4ARM主机Jetson-tx21285.5分布式架构主机1295.5.1ROS网络通信1305.5.2机器人程序的远程开发1305.6本章小结131第6章机器人底盘6.1底盘运动学模型1326.1.1两轮差速模型1326.1.2四轮差速模型1366.1.3阿克曼模型1406.1.4全向模型1446.1.5其他模型1486.2底盘性能指标1486.2.1载重能力1486.2.2动力性能1486.2.3控制精度1506.2.4里程计精度1506.3典型机器人底盘搭建1516.3.1底盘运动学模型选择1526.3.2传感器选择1526.3.3主机选择1536.4本章小结155SLAM篇第7章SLAM中的数学基础7.1SLAM发展简史1587.1.1数据关联、收敛和一致性1607.1.2SLAM的基本理论1617.2SLAM中的概率理论1637.2.1状态估计问题1647.2.2概率运动模型1667.2.3概率观测模型1717.2.4概率图模型1737.3估计理论1827.3.1估计量的性质1827.3.2估计量的构建1837.3.3各估计量对比1907.4基于贝叶斯网络的状态估计1937.4.1贝叶斯估计1947.4.2参数化实现1967.4.3非参数化实现2027.5基于因子图的状态估计2067.5.1非线性最小二乘估计2067.5.2直接求解方法2067.5.3优化方法2087.5.4各优化方法对比2187.5.5常用优化工具2197.6典型SLAM算法2217.7本章小结221第8章激光SLAM系统8.1Gmapping算法2238.1.1原理分析2238.1.2源码解读2288.1.3安装与运行2338.2Cartographer算法2408.2.1原理分析2408.2.2源码解读2478.2.3安装与运行2588.3LOAM算法2668.3.1原理分析2668.3.2源码解读2678.3.3安装与运行2708.4本章小结270第9章视觉SLAM系统9.1ORB-SLAM2算法2749.1.1原理分析2749.1.2源码解读3109.1.3安装与运行3199.1.4拓展3279.2LSD-SLAM算法3299.2.1原理分析3299.2.2源码解读3349.2.3安装与运行3379.3SVO算法3389.3.1原理分析3389.3.2源码解读3419.4本章小结341第10章其他SLAM系统10.1RTABMAP算法34410.1.1原理分析34410.1.2源码解读35110.1.3安装与运行35710.2VINS算法36210.2.1原理分析36410.2.2源码解读37310.2.3安装与运行37610.3机器学习与SLAM37910.3.1机器学习37910.3.2CNN-SLAM算法41110.3.3DeepVO算法41310.4本章小结414自主导航篇第11章自主导航中的数学基础11.1自主导航41811.2环境感知42011.2.1实时定位42011.2.2环境建模42111.2.3语义理解42211.3路径规划42211.3.1常见的路径规划算法42311.3.2带约束的路径规划算法43011.3.3覆盖的路径规划算法43411.4运动控制43511.4.1基于PID的运动控制43711.4.2基于MPC的运动控制43811.4.3基于强化学习的运动控制44111.5强化学习与自主导航44211.5.1强化学习44311.5.2基于强化学习的自主导航46511.6本章小结467第12章典型自主导航系统12.1ros-navigation导航系统47012.1.1原理分析47012.1.2源码解读47512.1.3安装与运行47912.1.4路径规划改进49212.1.5环境探索49612.2riskrrt导航系统49812.3autoware导航系统49912.4导航系统面临的一些挑战50012.5本章小结500第13章机器人SLAM导航综合实战13.1运行机器人上的传感器50213.1.1运行底盘的ROS驱动50313.1.2运行激光雷达的ROS驱动50313.1.3运行IMU的ROS驱动50413.1.4运行相机的ROS驱动50413.1.5运行底盘的urdf模型50513.1.6传感器一键启动50613.2运行SLAM建图功能50613.2.1运行激光SLAM建图功能50713.2.2运行视觉SLAM建图功能50813.2.3运行激光与视觉联合建图功能50813.3运行自主导航50913.4基于自主导航的应用51013.5本章小结511附录ALinux与SLAM性能优化的探讨附录B习题