机器人运动估计系列（一）——基础介绍

机器人运动估计系列（一）——基础介绍

作为航空航天类专业毕业，马上要入机器人坑、直博坑的小本科生，要来写这样一个系列的文章，我感到诚惶诚恐。不过，人还是得有一些追求的，写这样一系列文章很难，但是相信对我自己的提升也会不少。当然，作为一名资深小白，出点错误、理解得不透彻的情况很有可能会在这个系列的文章中屡屡出现，到时候还望各位看官不吝赐教。当然，我也将会在未来的学习过程不断勘误。

1 什么是机器人的运动估计？

这是一个相当关键的问题，因为这个问题的结果如果不符合我的预期，很有可能导致我这个系列的文章沦落为各位的笑柄。

因为我是这样理解运动估计：

机器人的运动估计就是估计机器人的位置、速度以及姿态。

• 1
• 2

暂先不论这个定义是不是严谨科学吧！显而易见，我写这个系列的文章的目的就是要说明白估计机器人的位置、速度以及姿态是怎么一回事。
哲学上说，世界上的一切物体都是运动的，运动也都是可以被估计的。那么机器人的运动估计有什么不同之处呢？

其中最重要的一点就是，大多数移动机器人或是四旋翼都属于近地、慢速的运动物体，另外也不会配置很高精度的惯性传感器，因此在考虑其运动学方程的时候往往不会考虑地球自转的影响，而直接可以把导航坐标系就当作惯性系。

2 运动估计有什么用？

只能说运动估计的作用超出你的想象，其中最主要的用途有两项，一是用作控制反馈，二是用于导航。有了解过四旋翼或者是调过小车底盘的童鞋肯定都知道，要想稳定的控制，开环肯定是不行的，必须要有反馈的信号。四旋翼控制程序里最关键的一环就是姿态解算，姿态估计准了才能把飞机控稳。小车底盘的控制也必须要有速度反馈，只控制电机的话，想让小车走条直线都成问题。而对于导航，要将运动体从A地导引到B地，定位和定向技术就是基础中的基础。除此之外，在AR、VR以及可穿戴设备等等领域内，运动估计都有很大作用。

3 运动估计怎么做？

运动估计的目标是估计出物体的位置、速度、姿态等运动参数。具体应该怎么操作呢，我总结出了以下五步大法：

-第一步，弄明白我们怎么表示物体的位置、速度和姿态。

-第二步，我们需要了解位置、速度以及姿态这三个量之间的关系，也就是常常被提到的运动学方程。

-第三步，了解能提供位置、速度或姿态测量的传感器以及对应的数学模型。就是说，明白了三者之间的关系还不够，总得先测出他们其中的一个量，才能推出其他量吧。

-第四步，掌握多传感器数据融合的算法。一个高精度传感器测量难免有误差，更何况是用这么便宜的传感器。所以没钱的话，好歹多用几个便宜的不同类型的传感器互相配合吧，那么数据融合算法就值得你拥有了。

-第五步，进阶教程之传感器故障诊断。温度太高、气流太快、干扰太强，造成传感器难免有崩溃的时候，崩溃了的传感器只好让他一边凉快会儿去啦。当然，这部分我还不懂，不敢造次。

上面所说的五个步骤实际上就是本系列文章的大纲，通过以上步骤就能够对机器人的运动状态进行一个初步的估计了。

4 本系列文章结构

本系列文章主要分为六篇：

-（一）基础介绍：介绍运动估计的基本情况以及坐标系定义、旋转的表示。

-（二）运动学方程：以四麦轮小车以及四旋翼无人机为例，介绍如何建立机器人的运动学方程。

-（三）常用传感器：介绍在机器人的运动估计中常用的传感器，如：加速度计、陀螺仪、磁力计、码盘（里程计）、GPS等。

-（四）多传感器数据融合之卡尔曼滤波器：以四麦轮小车为例，介绍如何根据运动学方程以及传感器数据模型建立状态方程以及观测方程，介绍了卡尔曼滤波器的基本原理以及我对它的一点微不足道的见解。

-（五）多传感器数据融合之互补滤波器：以四旋翼飞机为例，介绍了互补滤波器在无人机姿态估计上的应用。

-（六）进阶教程之联邦卡尔曼滤波以及故障检测与隔离：狂抄一些书上的公式，以及加入一些我的理解。

接下来，让我们正式进入，理论介绍的第一部分吧！

5 基础知识——坐标系定义

要对机器人的速度、位置、姿态进行估计，首先要清楚，速度、位置和姿态在数学中是怎么表示的。位置、速度的表示看起来比较简单，定义一个笛卡尔坐标系，位置、速度都可以表示成坐标系下的一个向量。但是，问题是坐标系该怎么选取呢？

这就得回归到我们进行运动估计的目的来看了，估计位置通常都是为了进行导航，必须知道我现在在地球上的哪个角落，这样才能拿出世界地图看看我和目的地有多远，现在要往哪个方向去走，所以位置通常要表示在世界系下。世界系其实也可以说成导航系，以世界上的一个固定不动的点（暂时不考虑自转啦！！！）为原点，构成的一个右手系。那么方向呢？通常，导航里都是以北向、东向、天向、地向来定义导航系的方向的，比较常见的导航系有东北天系（X轴指向东，Y轴指向北，Z轴指向天）以及北东地系（X轴指向北，Y轴指向东，Z轴指向地）。不过假如机器人没用磁力计，也没用GPS，导航系的轴向事实上也没有必要遵循这个规定了。而速度估计通常是为了给控制系统反馈的，所以速度通常表示在物体的体坐标系下。当然，用作导航用途的时候，也可以把速度表示在世界系下。无论如何，在这里我们都需要定义两个坐标系，也就是世界系（导航系）以及体坐标系。

从下面这幅图可以看看东北天坐标系是如何定义的：

图中形象地表示了地球坐标系（XYZ）和东北天坐标系（ENU）。东北天系原点可以是地球表面上任意一个固定的点，N轴与地球表面相切指向北极，E轴与地球表面相切指向东，U轴垂直地球表面指向天空。

从下面这幅图可以看看体坐标系如何定义，并且与东北天系的关系如何？

体坐标系的原点通常是运动物体的几何中心或是质心，X轴或Y轴指向物体的头部，Z轴垂直物体或向上或向下。体坐标系与东北天坐标系之间除了有平移关系，通常存在一定的旋转关系，旋转如何表示呢，继续往下看！

6 基础知识——旋转的表示

位置和速度的表示方法很容易，但是姿态的表示却要复杂得多。事实上，物体的姿态就是以体坐标系相对与导航坐标系的旋转来表示的。通常来说，旋转的表示方法有三种，分别是方向余弦矩阵、欧拉角以及四元数。下面对这几种表示方式逐一介绍。

6.1 方向余弦矩阵

方向余弦矩阵可以用来表示两个坐标系之间的旋转，同样也可以用来表示一个向量绕相同坐标系中某个轴的旋转。讨论一下当它表达两个坐标系之间的选择时的定义方式，如下：
假设两组坐标系的基底，分别为：

v⃗ =[i0→,j0→,k0→]

u⃗ =[i1→,j1→,k1→]

另外，假设有一个向量 a⃗ ，那么 a⃗ 在这两组基底下的投影为：

a⃗ =v⃗ ⋅av=u⃗ ⋅au

其中， av和 au是三维列向量，是 a⃗ 分别在两个坐标系下的投影，不相等但是存在一定的关系：

av=v⃗ T⋅u⃗ ⋅au

C=v⃗ T⋅u⃗ =⎡⎣⎢⎢⎢⎢i0→⋅i1→j0→⋅i1→k0→⋅i1→i0→⋅j1→j0→⋅j1→k0→⋅j1→i0→⋅k1→j0→⋅k1→k0→⋅k1→⎤⎦⎥⎥⎥⎥

所以，有av=C⋅au

，这里的C代表的就是一个方向余弦矩阵，代表了U和V两个坐标系之间的旋转，可见下图：

那么，怎么用方向余弦表示向量绕相同坐标系中某个轴的旋转呢？其实很简单。举个例子，要表示一个向量绕当前坐标系的Z轴旋转30度，不也可以看作向量不动，当前坐标系绕Z轴旋转了-30度吗。所以，只需要算出当前坐标系绕这个轴的方向余弦矩阵，然后取逆矩阵就可以了。

6.2 欧拉角

欧拉角适合用于表示两个坐标系之间的旋转。欧拉角方法根据一切旋转都能分解为三次绕空间中不同轴的旋转的原理，表明了一切坐标系的取向，都可以用三个欧拉角来表示。由于欧拉角表示方法非常直观，因此在四旋翼以及移动机器人领域里，这种表示姿态的方法广受使用。

欧拉角是怎么表示旋转的呢？看看下面这幅图：

可以看到最初的蓝色坐标系，经过围绕三个不同轴的三个欧拉角旋转，最后成为了红色坐标系。旋转过程如下：

先绕z

轴旋转 α角度，再绕旋转后得到的 x′轴旋转 β角度，最后绕旋转之后的 z″轴转 γ

角度，最后得到了一个新的坐标系。可以看出，三次围绕旋转的轴在空间上是不重合的，三次旋转中的第一次和第三次旋转围绕的是同一个轴，这就会产生欧拉角中所谓的Gimbal Lock问题，当然这是另一个话题啦。

另外，在上面的描述中，欧拉角的三次旋转中有两次围绕的是z轴。事实上，欧拉角法可以分为两类，一类是依次旋转三个不同的轴，称为Tait-Bryan angles，因此可选顺序有：X-Y-Z，X-Z-Y，Y-X-Z，Y-Z-X，Z-X-Y，Z-Y-X；另一类是相邻两次旋转不同的轴，也就是上文介绍的那一类，称为Euler angles，可选顺序有：X-Y-X，X-Z-X，Y-X-Y，Y-Z-Y，Z-X-Z，Z-Y-Z。由于绕不同的轴旋转最后得到的欧拉角是不同的，因此在有用到欧拉角的场合必须指明旋转的顺序。

事实上，欧拉角表示方法中其实还存在外在旋转和内在旋转的区别，前者是指每次围绕的旋转轴是原始坐标系的轴，后者则是围绕旋转后得到的坐标系的轴，这二者还是有些许区别的。

但是，在四旋翼或是移动机器人的应用中貌似比较偏好于选择第一类方法中的Z-Y-X旋转顺序，并且都采用内在旋转，因此在后续的介绍中，我们都将采用这样的配置。

6.3 四元数

相对于方向余弦矩阵以及欧拉角，四元数能够很方便地表示一个向量围绕另一个向量的旋转。另外，四元数具有相对与前二者非常优越的特点，后面会具体说到。那么四元数是怎么表示旋转的呢？

这里只介绍其定义，关于四元数和旋转之间关系的具体证明可以参考这篇文章。
四元数是一个高阶复数，由一个实部，三个虚部构成，表示如下：

q=(w,x,y,z)=w+xi+yj+zk

也可以表示为：

q=w+v⃗ =(w,(x,y,z))

其中， w为实数， v⃗ 代表向量。

那么，比如让一个三维向量u⃗ 

绕另一个三维向量 v⃗ 旋转 θ角度得到向量 uq→

该怎么用四元数表示呢？
其实很简单：

uq→=q∗u⃗ ∗q−1

其中， ∗代表了四元数乘法， uq→和 u⃗ 都是扩展的四维向量，即

u⃗ =[0,u⃗ ]

q=cosθ2+sinθ2⋅v⃗ 

在上面的式子中，要求围绕的旋转轴v⃗ 

是单位向量，因此整个旋转四元数的也是模值为1的单位四元数。需要注意，表达一次旋转是需要两个四元数参与的，一个是它本身，另一个是它的逆。有关于四元数的诸多运算法则由于篇幅有限，也不在这里多做展示了，各位可以参考其他资料。

6.4 三种旋转表示方法的优劣对比

这里借鉴这篇博客中的相关评价，如下：

1、方向余弦矩阵 优点

旋转轴可以是任意向量；

是架在欧拉角和四元数之前的桥梁（后面说三种表达的相互转换时会提到）；

缺点

旋转其实只需要知道1个三维向量、1个角度，一共4个量的信息，但矩阵法却使用了16个元素，运算量大，造成存储空间以及运算能力的浪费；

2、欧拉角 优点

很容易理解，形象直观；

表示更方便，只需要3个值（分别对应x、y、z轴的旋转角度）；

缺点

之前提到过这种方法是要按照一个固定的坐标轴的顺序旋转的，因此不同的顺序会造成不同的结果；

会造成万向节锁（Gimbal Lock）的现象;（我的理解是，在一般情况下一组欧拉角都能单独对应坐标系的一种姿态，但是出现万向节锁时，会出现多组欧拉角对应坐标系的一种姿态，比如（30,90,0）和（10,90,20）其实对应的都是一个姿态）

由于万向节锁的存在，欧拉旋转无法实现球面平滑插值；

3、四元数旋转 优点

可以避免万向节锁现象（一个四元数总是唯一对应坐标系的一个姿态）；

只需要一个4维的四元数就可以执行绕任意过原点的向量的旋转，方便快捷，在某些实现下比旋转矩阵效率更高；

可以提供平滑插值；

缺点

比欧拉旋转稍微复杂了一点点，因为多了一个维度；

理解更困难，不直观；

综合以上各种旋转方式的优缺点，在我们的运动估计系列中：为了表示直观，统统采用欧拉角表示姿态，但是在更新运算过程中为了运算效率，使用四元数进行运算。

不过事实上，也离不开方向余弦矩阵（以后称旋转矩阵）的帮助，因为在四元数与欧拉角的互相转换中，必须借助旋转矩阵来搭起桥梁。

结语

本篇文章中介绍了运动估计是什么，为什么要运动估计，运动估计的具体步骤以及坐标系定义、旋转的表达方法，内容已经足够多了。有关于旋转矩阵、欧拉角与四元数的三者之间的转换方法，将会在下一章节中继续介绍。另外，下一章节中将会详细介绍，运动学模型的推导过程！一起继续往前吧！