我们的身体每时每刻都在做着各种各样的动作，比如站着、坐着、躺着等等，这里的“站着”“坐着”“躺着”在我们看来是很自然的事情，但大家有没有注意到一件事情——我们如何知道自己是在“站着”、“坐着”还是“躺着”呢？
其实在我们的身体上有一个很不起眼但极其重要的器官，位觉感受器，它是身体能够正常运动保持平衡的关键。试想一下，我们原地转很多圈以后是不感觉晕晕乎乎的，严重的甚至走不了路？尽管这个时候我们的视觉、听觉、触觉等都在正常工作着，但位觉感受器其实已经出现了问题，因此身体感受不到自己当前的姿态，就无法保持平衡了。
同样地，在机器人的身上也存在这么一个位觉感受器——IMU，它用来测量机器人的运动及姿态信息，其工作流程也如同位觉感受器一般，将测量结果告诉机器人的运动控制模块(小脑)，运动控制模块根据这些信息做出决策后给执行末端(手、腿、脚)下发指令，以此控制机器人的运动。
狭义上，IMU指惯性测量单元(Inertial measurement unit)，它可以测量物体在三维空间中运动的三轴加速度及三轴角速度，再通过数据融合等算法得到物体的姿态；而在实际使用中，IMU一般泛指测量被控物体运动及姿态的传感器，包括狭义的IMU、VRU、AHRS等。对机器人而言，IMU的数据质量直接决定了机器人的运动性能。事实上，不止在机器人领域，IMU在很多领域都有应用，大到飞机、导弹、航天器，小到手机、电子手环等都有IMU的身影。
▍姿态是什么
姿态是物体相对于某一坐标系各轴的旋转。一般我们说姿态，指的是物体相对于自身坐标系的旋转，但我们可以通过旋转变换计算得到它相对于其他坐标系的姿态。比如图中两个人看到地上的物体，甲认为是6，乙认为是9，为了解决这样的事，我们给甲乙和地上的物体建立了坐标系，规定地上的物体就是6，x的初始方向指向6的勾，甲乙的坐标系x方向与物体x方向平行，y垂直向上，这样通过坐标转换，在甲看来地上的物体就是6，在乙看来地上的物体也是6，只不过是“6翻了”。
图1 物体的相对姿态
我们用数学的语言描述以上内容，以二维坐标系为例，使用两个角度值表示各物体的姿态以及相对姿态(单位：deg)，首先甲乙以及地上的物体相对于自身的坐标系的姿态均为：
上述表示姿态的方法其实就是姿态表示中最直观的欧拉角表示法。在三维笛卡尔空间中，使用物体绕三轴的旋转角度表示物体的姿态：
通常定义绕x轴旋转的角度为横滚角(roll)，绕y轴旋转的角度为俯仰角(pitch)，绕z轴旋转的角度为偏航角(yaw)，如图2所示。
图片来源：https://www.youtube.com/watch?v=UpSMNYTVqQI
欧拉角虽然直观，但用来表示姿态有一定的局限性，旋转顺序的定义会影响旋转结果，比如图2中的物体6先绕x旋转90度再绕y旋转90度，和先绕y旋转90度再绕x旋转90度得到的结果是不一样的；再比如在旋转时产生的万向节死锁问题也是由于旋转顺序引起的。
在现实中，大多数刚体的旋转都是一次完成的。比如我们要自拍，头要朝斜向上45度的方向拍出来才好看，头在转动时应该是绕某一轴转动某一角度直接到达了斜向上45度的姿态，而不应该是先水平转45度，再朝上转45度。因此，姿态还可以用轴角或四元数这种“直接到达某一姿态”的方式表示。
用轴角表示姿态，轴即旋转轴，用一个三维向量表示，角即绕旋转轴旋转的角度，因此轴角可以表示如下：
四元数定义在四维空间，用来表示物体的姿态时有诸多良好的性质，但不太直观。四元数的表示如下：
其中 
即旋转角， 
即旋转轴，这样我们可以从侧面感受一下四元数是怎么表示姿态的。
此外，姿态的表示方法还有方向余弦矩阵、李群李代数等，这里就不多做介绍了。
▍IMU测量姿态的原理
以惯性测量单元(MEMS IMU)为例，它由一个三轴加速度计和一个三轴陀螺仪组成，可以测量物体在三维空间中运动的加速度以及角速度，那么我们怎么用加速度或角速度得到物体的姿态呢？
图片来源：https://www.zhihu.com/question/19769131/answer/886359013
MEMS加速度计测量加速度的原理其实就是牛顿第二定律，如图3所示，有质量的物体运动时产生惯性会导致两侧受力件发生形变，在MEMS中使用变化的电压测量这种形变，即两侧有两块固定的电容板，中间是一块活动的电容板，相当于带质量的物体，这样就可以将形变反应到两侧电容的变化上，通过模数转换等方式就可以得到物体运动的加速度。
IMU在静置时，加速度计测量得到的加速度是重力加速度，利用这一性质，定义世界坐标系的z轴平行于重力加速度朝上，根据力的合成与分解，便可以求得物体相对于世界坐标系的姿态。如图4所示，物体在旋转一个角度时，加速度计便可以测到重力加速度在x轴的分量，从而计算得到夹角 ，即物体绕y轴的旋转角度，同理也可以得到物体绕x轴的旋转角度，这就是加速度测姿态的基本原理。但是实际中的MEMS加速度计存在高频噪声，短时间内由加速度计算得到的姿态是不准的。
传统的机械陀螺仪利用了角动量守恒原理，即高速旋转的陀螺在不受外力的作用时其旋转轴指向不变，这就是陀螺仪的定轴性，而进动性其本质上也是由于定轴性。显而易见，这样的方式如果应用在MEMS上是不太现实的，因此MEMS陀螺仪的实现采用了另外一种方式。
图片来源：https://www.youtube.com/watch?v=n_6p-1J551Y
MEMS陀螺仪测量角速度的原理是旋转物体在有径向运动时会产生科氏力，其方向垂直于径向速度方向与旋转角速度方向所成平面，且大小为：
m是物体的质量， w是物体旋转的角速度， v是物体径向运动的速度。
在MEMS中，有径向及切向相正交的电容板，径向的电容板施加振荡的电压驱动质量块运动，当物体旋转产生角速度时，由于科氏力的作用物体产生切向位移，导致切向电容板的电压发生变化，再通过模数转换等方法得到科氏加速度的大小，根据上述公式便可求出物体的角速度。
图6 MEMS陀螺仪的工作原理
图片来源：基础丨MEMS陀螺仪是如何工作的？- 中国自动化学会机器人专委会
微积分的原理告诉我们，角速度的积分就是角度，因此我们对陀螺仪测量得到三轴角速度直接进行积分也可以得到物体绕三轴旋转的角度。但是实际中的MEMS陀螺仪由于工艺、温度、震动等原因，会有各种各样的误差，这些误差随着积分会使陀螺仪得到的角度越來越不准，最后的结果就是明明物体在“站着”，但IMU告诉它是在“躺着”，有什么解决方法呢？
▍IMU姿态解算
我们发现，加速度得到的姿态虽然短期不准，但是不存在积分误差，长期是可以相信它的；角速度得到的姿态虽然长期存在积分误差，但是短期内是可以相信它的。那如果我们两边都相信一点，不就可以得到一个长期的稳定的准确的姿态了？由此我们发现了最简单的数据融合方法：
当然，这个简单的融合方法在实际中是不可以直接用于IMU的，由于传感器的噪声特性等，很难调节参数使其收敛，这里只是举个例子。不过数据融合方法就是基于这个朴素的思想，这里相信一点，那里相信一点，但我们要用科学的方法去实现“这里相信一点，那里相信一点”这件事。
IMU的数据融合，也叫姿态解算，常用的算法有互补滤波(Complementary Filter)、贝叶斯滤波(Bayesian Filter)等。互补滤波中应用比较多的有Mahony算法，其互补原理与pid类似；还有基于梯度下降优化的Madgwick算法等。贝叶斯滤波中经典的算法就是卡尔曼滤波(Kalman Filter，KF)及其衍生算法，如扩展卡尔曼滤波(Extended Kalman Filter，EKF)、误差卡尔曼滤波(Error State Kalman Filter，ESKF)、自适应卡尔曼滤波(Adaptive Extended Kalman Filter，AEKF)等。相较于互补滤波的方法，卡尔曼滤波基于贝叶斯模型对数据进行融合，可以得到目标的无偏估计。
图7 基于扩展卡尔曼滤波的IMU姿态解算[1]
如图7所示，以扩展卡尔曼滤波进行姿态解算为例，对四元数进行闭环迭代。卡尔曼滤波主要分为预测和更新两部分，我们分别对两部分进行简单的介绍。
首先是预测部分，对姿态
及其协方差矩阵
进行预测，这里的预测就是使用陀螺仪测得的角速度进行一次积分预测，其状态转移方程即图中的
，通过四元数微分方程可以得到。
之后就是更新部分，在更新时要对预测值和观测值进行比较作差。这里的预测值就是预测部分得到的姿态 ，观测值就是用加速度计测量得到的重力加速度在三轴的分量，怎么对姿态和加速度比较作差呢？我们想到了在上面的分析中可以用加速度求得姿态，这样姿态与姿态之间不就可以作差了；同样，我们也可以把姿态转换成加速度，这样也可以作差。量测矩阵
的作用就在这里，它可以把预测得到的向量由预测空间转换到观测空间。由于预测空间到观测空间的转换一般是非线性关系，不满足卡尔曼滤波高斯分布的假设，因此在EKF中，使用转换方程的一阶泰勒展开来近似这种线性关系，量测矩阵 即为转换方程的雅可比矩阵。
这里预测空间到观测空间的转换方程就是图7中 ，
它包含两层含义，一是将预测的姿态转换为预测的加速度，二是将预测的加速度转换至观测的加速度所在的坐标系。假如预测的姿态没有误差，那么使用预测的姿态将重力加速度转换至观测坐标系时，应该与观测的加速度完全重合，因为观测的加速度就是在观测坐标系下的重力加速度，正是由于预测的姿态有误差，因此最终导致预测加速度与观测加速度有偏差，而卡尔曼滤波所做的事情就是对这两者进行加权平均，得到一个加速度的无偏估计，之后再利用量测矩阵的逆 将加速度转换成姿态，这样就得到了姿态的无偏估计。这就是EKF进行IMU姿态解算的基本原理。
▍AHRS在干什么
在上述IMU测量姿态的过程中我们发现，重力加速度只在xz平面、yz平面有投影，而在xy平面，不管怎么旋转，重力加速度的投影只是一个点，无法计算绕Z轴旋转的角度，因此IMU对于绕z轴的旋转只能依靠角速度积分去测量，而我们前面也说过，角速度积分会有长期的积分误差，那绕z轴的旋转怎么纠正呢？答案是用磁力计。
我们前面提到，广义的IMU包括有AHRS。AHRS指的是航姿参考系统(Attitude and heading reference system)，包括基于MEMS的三轴陀螺仪，加速度计和磁力计，内部集成数据融合算法，可直接输出自身姿态信息，十轴的AHRS还有气压计去测量高度。
图片来源：https://www.youtube.com/watch?v=qXVLXrOB8Ag&feature=youtu.be
磁力计可以测量地磁场的大小及方向，大多数时候地磁场方向都与重力方向有一定的夹角，因此可以通过地磁场在xy平面的投影去计算物体绕z轴的旋转角度，其原理与重力加速度测姿态相同，之后便可以与角速度积分得到的角度进行数据融合。通过九轴的AHRS，可以得到物体长期、稳定的姿态测量，这也是现在多数机器人控制所使用的IMU。不过在地球两极的时候，磁场方向接近与重力平行，通过磁场解算姿态的方法也失效了，AHRS可以看作是退化成了六轴。此外，由于地磁场强度较弱，在遇到强磁干扰的环境中，使用磁力计的方法也会失效。
此外还有惯性导航系统(Inertial Navigation System，INS)，其中集成了GPS，在感知自身姿态的同时，也提供了稳定的位置、速度等信息，其功能更加强大。
IMU在足式机器人中最重要的应用就是做控制闭环。足式机器人运动控制的目标就是控制机器人运动到达指定位姿，比如机器人在奔跑时，控制的首要目标就是让它保持平衡不摔倒，在此基础上才能实现跑步这一动作，就像人一样，摔倒那肯定就不能跑了，具体一点就是通过IMU反馈的数据，对机器人机身姿态进行控制，以保证它的roll、pitch、yaw角保持在我们期望的位置。
图片来源：https://www.mi.com/cyberdog
前面我们提到了数据融合，只是介绍了IMU内部的数据融合。在机器人上通常会有很多传感器，比如视觉传感器、激光雷达、超声、TOF等等，多种传感器共同组成了机器人的感知系统，就如同我们人的视觉、听觉、触觉等感受器官。与IMU相似，各种各样的传感器都有其局限性，比如视觉 SLAM 容易受图像遮挡，光照变化，运动物体干扰，弱纹理场景等影响，快速运动时易丢失，激光SLAM缺少语义信息等，因此需要多种传感器进行数据融合。
IMU可以实现快速运动时的姿态估计，与其他传感器进行数据融合，为其他传感器提供准确的先验信息。比如IMU与视觉传感器融合，可以得到视觉传感器的初始姿态，减小因感知源运动造成的误差；同时也可以利用IMU对视觉图像进行插值平滑等。反过来，视觉传感器自身有不漂移、直接测量平移旋转等特性，可以用来校准IMU的漂移，提高IMU的精度。通过“优势互补”，多传感器融合为机器人提供稳定可靠的感知系统，是智能机器人系统不可或缺的一部分。
我们这里以世界上行业技术领先的几家做IMU的企业为例，介绍一下IMU在其他领域的一些应用。
>>>> Xsens
Xsens是荷兰的一家公司，专注于惯性传感器、惯性导航系统和运动捕捉设备的研发和生产。其产品主要应用于运动分析、姿态测量、虚拟现实、人机交互、医疗康复等领域。Xsens的产品主要分为三大类：惯性传感器(AHRS、IMU)、惯性导航系统(INS)以及运动捕捉设备(MVN)。其中，惯性传感器主要用于测量物体的加速度、角速度和磁场强度等物理量，可以用于姿态测量、运动分析和导航等方面；惯性导航系统则是在惯性传感器的基础上，结合地磁、气压、GPS等传感器，实现高精度的姿态测量和导航；运动捕捉设备则是用于捕捉人体或物体的运动轨迹，可以应用于虚拟现实、动作捕捉等领域。
图10 Xsens公司的MTi AHRS设备
图片来源：https://www.movella.com/
传统的光学动捕设备需要专门搭建一个动捕场地，利用分布在四周的高速摄像机捕捉贴在人体上的反光点，之后经过后处理去还原人体的动作，光学动捕多用于需要高精度运动数据的领域，比如游戏制作、电影拍摄等。这类动捕系统虽然精度高，但造价昂贵且不方便，因此现在一些领域比如VR、医疗恢复等多采用惯性动捕系统，利用IMU反馈的姿态数据去实时的计算并且还原人体动作。
Xsens MVN Link就是Xsens的动捕系列产品，可以实现无需外部设备的运动捕捉，只需要穿戴在身体上的传感器就可以获取数据，不需要安装摄像头等外部设备。这其中的关键技术之一就是使用IMU实时的感知人体或其他物体的姿态数据，并且回传到设备端，去回放动作或做一些其他的后处理。
>>>> Microstrain
Microstrain是美国的一家公司，专注于惯性传感器和惯性导航系统的研发和生产，其产品主要应用于航空航天、国防、自动化控制、运动分析等领域。Microstrain的产品以其高精度、高稳定性和高可靠性而闻名，被广泛应用于各种领域，其客户包括了很多知名的公司和机构，如波音、洛克希德马丁、美国国防部、NASA等。
Microstrain公司的RTK产品是基于全球定位系统(GPS)技术的实时动态定位解决方案，它们使用高精度GPS接收器和IMU来提供非常准确的定位和导航信息，目前在土地测量和地图制作、建筑物监测、交通运输、农业等方面都有应用。
RTK系统从应用上大致可以分为几种：无人机RTK系统：该系统可以为无人机提供高精度的定位能力，支持多种飞控系统，并且具有轻巧、易于安装和操作的特点；固定式RTK系统：该系统可以为固定式设备提供高精度的定位能力，例如建筑物监测、桥梁监测和地质勘探等领域；移动式RTK系统：该系统可以为移动设备提供高精度的定位能力，例如车辆导航、船舶导航和机器人导航等领域。
图11 Microstrain公司的3DM RTK设备
图片来源：https://www.microstrain.com/
本文简单介绍了机器人感知系统中的位姿传感器及其工作原理，虽然它看起来毫不起眼，但发挥的作用却是十分重要的。现如今机器人、自动驾驶等可以实现自主移动的智能设备发展越来越快，控制算法也越来越复杂，感知系统也随之发展的越來越强大。IMU作为感知系统重要的一环，小到民用电子，大到航空航天都能看到它的身影，其价格也是从民用级的几块钱到战略级的几十上百万不等，IMU也在朝着低成本高精度的方向发展。
当然了，单传感器的能力是有限的，多传感器融合才能将各自的作用最大化。多传感器融合感知已经成为机器人、自动驾驶等领域的热点方向之一，传统的数据融合算法加上深度学习等方法，感知系统的精度也在逐步提高。相信未来机器人可以走进千家万户，依靠自身强大的感知能力，与人进行交互，成为社会生产主要的参与者之一，为人类生活带来便利。
参考文献：
[1] Sabatelli S , Galgani M , Fanucci L ,et al.A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU[J].IEEE Transactions on Instrumentation and Measurement, 2013, 62(3):590-598.
