机器人运动估计系列（二）——运动学方程（上）
前言
在上一篇文章中，我们了解了用于表示机器人位置、速度的坐标系的定义，学习了如何表示姿态，也就是旋转的三种表达方式：旋转矩阵、欧拉角以及四元数。在这一节的第一部分中，我们将继续讨论在三种旋转表达之间互相转换的方法。之后，我们将以四麦轮小车以及四旋翼为例，推导二者的运动学模型，并分享一些我在学习过程中的一些心得体会。
1 各旋转表达方式的转换
1.1 欧拉角 - - > 旋转矩阵
在上一篇文章中，我们定义了在这个系列里欧拉角的旋转顺序与方式，即Z-Y-X，并且是内在旋转。下面，我们来推导由欧拉角到旋转矩阵的转换关系。
1、绕Z轴旋转 ψ <script id="MathJax-Element-146" type="math/tex">\psi</script> 角度（从n系到1系）
C1n=⎡⎣⎢cosψ−sinψ0sinψcosψ0001⎤⎦⎥
<script id="MathJax-Element-147" type="math/tex; mode=display">C_{n}^{1} = \left[ \begin{matrix} \cos{\psi} & \sin{\psi} & 0 \\ -\sin{\psi} & \cos{\psi} & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right]</script>
2、绕Y轴旋转 θ <script id="MathJax-Element-148" type="math/tex">\theta</script> 角度（从1系到2系）
C21=⎡⎣⎢cosθ0sinθ010−sinθ0cosθ⎤⎦⎥
<script id="MathJax-Element-149" type="math/tex; mode=display">C_{1}^{2} = \left[ \begin{matrix} \cos{\theta} & 0 & -\sin{\theta} \\ 0 & 1 & 0 \\ \sin{\theta} & 0 & \cos{\theta} \\ \end{matrix} \right]</script>
3、绕X轴旋转 γ <script id="MathJax-Element-150" type="math/tex">\gamma</script> 角度（从2系到b系）
Cb2=⎡⎣⎢1000cosγ−sinγ0sinγcosγ⎤⎦⎥
<script id="MathJax-Element-151" type="math/tex; mode=display">C_{2}^{b} = \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos{\gamma} & \sin{\gamma} \\ 0 & -\sin{\gamma} & \cos{\gamma} \\ \end{matrix} \right]</script>
4、结合三次旋转推导出完整的旋转矩阵
Cbn=Cb2⋅C21⋅C1n
<script id="MathJax-Element-152" type="math/tex; mode=display">C_{n}^{b} = C_{2}^{b} \cdot C_{1}^{2} \cdot C_{n}^{1} </script>
Cbn=⎡⎣⎢cosθcosψsinθsinγcosψ−cosγsinψsinθcosγcosψ+sinγsinψcosθsinψsinθsinγsinψ+cosγcosψsinθcosγsinψ−sinγcosψ−sinθcosθsinγcosθcosγ⎤⎦⎥
<script id="MathJax-Element-153" type="math/tex; mode=display">C_{n}^{b} = \left [ \begin{matrix} \cos{\theta} \cos{\psi} & \cos{\theta} \sin{\psi} & -\sin{\theta} \\ \sin{\theta} \sin{\gamma} \cos{\psi} - \cos{\gamma} \sin{\psi} & \sin{\theta} \sin{\gamma} \sin{\psi} + \cos{\gamma} \cos{\psi} & \cos{\theta} \sin{\gamma} \\ \sin{\theta} \cos{\gamma} \cos{\psi} + \sin{\gamma} \sin{\psi} & \sin{\theta} \cos{\gamma} \sin{\psi} - \sin{\gamma} \cos{\psi} & \cos{\theta} \cos{\gamma} \end{matrix} \right ] </script>
以上便定义了由欧拉角到旋转矩阵的转换关系。但是注意一点，这个旋转矩阵的含义是指：将一个在n系下表示的向量，投影到b系下进行表示。这里，n系指的是世界系或者说是导航系，b系指的是体坐标系。
1.2 四元数 - - > 旋转矩阵
根据上一篇文章的描述，要利用四元数表示由一个三维向量 un <script id="MathJax-Element-9" type="math/tex">\boldsymbol{u^{n}}</script> 得到向量 ub <script id="MathJax-Element-10" type="math/tex">\boldsymbol{u^{b}}</script> ，方法如下：
ub=q∗un∗q−1
<script id="MathJax-Element-11" type="math/tex; mode=display"> \boldsymbol{u^{b}}=q* \boldsymbol{u^{n}}*q^{-1}</script>
其中，
q=[q0 q1 q2 q3] <script id="MathJax-Element-12" type="math/tex">q=[q_{0} \ q_{1} \ q_{2} \ q_{3}]</script>。
根据四元数乘法法则，有：
ub=M(q)⋅M′(q−1)⋅un
<script id="MathJax-Element-13" type="math/tex; mode=display"> \boldsymbol{u^{b}}=M(q) \cdot M^{'}(q^{-1})\cdot \boldsymbol{u^{n}}</script>
其中，
M(q) <script id="MathJax-Element-14" type="math/tex">M(q)</script>代表的是四元数的左乘矩阵，
M′(q−1) <script id="MathJax-Element-15" type="math/tex">M^{'}(q^{-1})</script>代表的是四元数的右乘矩阵。两个矩阵的定义在这里就不细说了，最终可以得到的结果就是：
M(q)⋅M′(q−1)=⎡⎣⎢⎢⎢⎢××××0q21+q20−q23−q222(q1q2+q0q3)2(q1q3−q0q2)02(q1q2−q0q3)q22−q23+q20−q212(q2q3+q0q1)02(q1q3+q0q2)2(q2q3−q0q1)q23−q22−q21+q20⎤⎦⎥⎥⎥⎥
<script id="MathJax-Element-16" type="math/tex; mode=display">M(q) \cdot M^{'}(q^{-1})=\left [ \begin{matrix} \times & 0 & 0 & 0 \\ \times & q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2} & 2(q_{1}q_{2}-q_{0}q_{3}) & 2(q_{1}q_{3}+q_{0}q_{2}) \\ \times & 2(q_{1}q_{2}+q_{0}q_{3}) & q_{2}^{2}-q_{3}^{2}+q_{0}^{2}-q_{1}^{2} & 2(q_{2}q_{3}-q_{0}q_{1}) \\ \times & 2(q_{1}q_{3}-q_{0}q_{2}) & 2(q_{2}q_{3}+q_{0}q_{1}) & q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2}\\ \end{matrix} \right ]</script>
所以，
ub=Cbn⋅un
<script id="MathJax-Element-17" type="math/tex; mode=display"> \boldsymbol{u^{b}}=\boldsymbol{C}_{n}^{b}\cdot \boldsymbol{u^{n}}</script>
Cbn=⎡⎣⎢⎢q21+q20−q23−q222(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q22−q23+q20−q212(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q23−q22−q21+q20⎤⎦⎥⎥
<script id="MathJax-Element-18" type="math/tex; mode=display">\boldsymbol{C}_{n}^{b}=\left [ \begin{matrix} q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2} & 2(q_{1}q_{2}-q_{0}q_{3}) & 2(q_{1}q_{3}+q_{0}q_{2}) \\ 2(q_{1}q_{2}+q_{0}q_{3}) & q_{2}^{2}-q_{3}^{2}+q_{0}^{2}-q_{1}^{2} & 2(q_{2}q_{3}-q_{0}q_{1}) \\ 2(q_{1}q_{3}-q_{0}q_{2}) & 2(q_{2}q_{3}+q_{0}q_{1}) & q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2}\\ \end{matrix} \right ]</script>
1.3 四元数 - - > 欧拉角
根据上面的推导，可以发现旋转矩阵作为中间桥梁衔接了几种不同的旋转表达方式。通过欧拉角能够推出对应的旋转矩阵，通过四元数也能够推出对应的旋转矩阵，所以欧拉角和四元数的对应关系也就变得清晰明了了。通过四元数可以得到欧拉角，通过欧拉角也能求出四元数。这里以四元数得到欧拉角举例：
根据上面的推导，很容易得到：
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪−sinθ=2(q1q3+q0q2)cosθcosψ=q21+q20−q23−q22cosθsinψ=2(q1q2−q0q3)cosθsinγ=2(q2q3−q0q1)cosθcosγ=q23−q22−q21+q20
<script id="MathJax-Element-19" type="math/tex; mode=display">\begin{cases} -\sin{\theta} = 2(q_{1}q_{3}+q_{0}q_{2})\\ \cos{\theta} \cos{\psi}=q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2} \\ \cos{\theta} \sin{\psi}=2(q_{1}q_{2}-q_{0}q_{3}) \\ \cos{\theta} \sin{\gamma} = 2(q_{2}q_{3}-q_{0}q_{1}) \\ \cos{\theta} \cos{\gamma} = q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2} \end{cases}</script>
所以，可以得到：
θ=arcsin[−2(q1q3+q0q2)]
<script id="MathJax-Element-20" type="math/tex; mode=display">\theta = \arcsin \left[ -2(q_{1}q_{3}+q_{0}q_{2}) \right]</script>
ψ=arctan[2(q1q2−q0q3)q21+q20−q23−q22]
<script id="MathJax-Element-21" type="math/tex; mode=display">\psi = \arctan \left[\frac{2(q_{1}q_{2}-q_{0}q_{3})}{q_{1}^{2}+q_{0}^{2}-q_{3}^{2}-q_{2}^{2} } \right]</script>
γ=arctan[2(q2q3−q0q1))q23−q22−q21+q20]
<script id="MathJax-Element-22" type="math/tex; mode=display">\gamma = \arctan \left[\frac{2(q_{2}q_{3}-q_{0}q_{1}))}{ q_{3}^{2}-q_{2}^{2}-q_{1}^{2}+q_{0}^{2} } \right]</script>
需要注意的是，在机器人的运动中，俯仰角
θ <script id="MathJax-Element-23" type="math/tex">\theta</script>都是定义在
−π/2 <script id="MathJax-Element-24" type="math/tex">-\pi/2</script>到
π/2 <script id="MathJax-Element-25" type="math/tex">\pi/2</script>之间的，所以
cosθ <script id="MathJax-Element-26" type="math/tex">\cos{\theta}</script>大于0。由此也可以推出，其余两个角在
[0 2π] <script id="MathJax-Element-27" type="math/tex">[0 \ 2\pi]</script>范围内的具体大小。
2 运动学方程的引入
运动学主要是研究物体的位置、速度、角速度、加速度、角加速度等特征之间的关系。有了运动学方程，就能很容易的从其中的某些量出发推导得到另一些量，比如可以测量物体加速度和角速度就能根据这二者积分解算出位置和速度（惯性导航的基本原理）。本文将以四麦轮机器人以及四旋翼无人机为例，简单的介绍一下运动学方程是如何建立的。
要学习构造运动学方程，首先要理解一点，同一个运动在不同坐标系下的描述是不相同的。
举个例子，假如一个物体P相对于坐标系A下以速度 [10,10,0] <script id="MathJax-Element-28" type="math/tex">[10,10,0]</script>运动，而坐标系B是A系绕Z轴经过180度的旋转得到的，那么这个物体的速度在B系中的表示就应该是 [−10,−10,0] <script id="MathJax-Element-29" type="math/tex">[-10,-10,0]</script>。理解了这一点后，我们可以思考一个稍微复杂一些的问题，当B系相对与A系存在一个平移速度 [1,1,0] <script id="MathJax-Element-30" type="math/tex">[1,1,0]</script>时，这个物体的速度在B系下的表示就应该是
[−10,−10,0]−[−1,−1,0]=[−9,−9,0]
<script id="MathJax-Element-31" type="math/tex; mode=display">[-10,-10,0]-[-1,-1,0]=[-9,-9,0]</script>
这也很好理解，那么要是B系不仅与A系之间有平移速度还有旋转角速度呢，这个应该怎么表示？这时就轮到
哥氏定理闪亮登场了！
2.1 理解哥氏定理
这部分内容主要参考一个介绍刚体转动动力学的PPT。
哥氏定理描述的是一般的空间自由质点相对于不同坐标系的速度和加速度的相互转换。尤其是当两个坐标系之间存在相对旋转时，需要格外注意。
下文中推导了哥氏定理的具体形式。
假设有一个坐标系n与一个坐标系b，另外定义一个向量 r <script id="MathJax-Element-32" type="math/tex">\boldsymbol{r}</script>，如下：
r=rx⋅ib+ry⋅jb+rz⋅kb
<script id="MathJax-Element-33" type="math/tex; mode=display">\boldsymbol{r}=r_{x} \cdot \boldsymbol{i}_{b}+r_{y} \cdot \boldsymbol{j}_{b}+r_{z} \cdot \boldsymbol{k}_{b}</script>
将该向量在n系下求导，即：
dnrdt=dndt(rx⋅ib+ry⋅jb+rz⋅kb)
<script id="MathJax-Element-34" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{r}}{dt}=\frac{d_{n}}{dt} \left ( r_{x} \cdot \boldsymbol{i}_{b}+r_{y} \cdot \boldsymbol{j}_{b}+r_{z} \cdot \boldsymbol{k}_{b} \right ) </script>
dnrdt=dnrxdt⋅ib+dnrydt⋅jb+dnrzdt⋅kb+rx⋅dnibdt+ry⋅dnjbdt+rz⋅dnkbdt
<script id="MathJax-Element-35" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{r}}{dt}= \frac{d_{n} r_{x}}{dt} \cdot \boldsymbol{i}_{b} + \frac{d_{n} r_{y}}{dt} \cdot \boldsymbol{j}_{b} + \frac{d_{n} r_{z}}{dt} \cdot \boldsymbol{k}_{b} + r_{x} \cdot \frac{d_{n} \boldsymbol{i}_{b}}{dt} + r_{y} \cdot \frac{d_{n} \boldsymbol{j}_{b}}{dt} + r_{z} \cdot \frac{d_{n} \boldsymbol{k}_{b}}{dt} </script>
又因为，当两个参考系之间存在相对转动时，有
dnibdt=ωnb×ib
<script id="MathJax-Element-36" type="math/tex; mode=display">\frac{d_{n} \boldsymbol{i}_{b}}{dt}=\boldsymbol{\omega}_{nb} \times \boldsymbol{i}_{b}</script>
dnjbdt=ωnb×jb
<script id="MathJax-Element-37" type="math/tex; mode=display">\frac{d_{n} \boldsymbol{j}_{b}}{dt}=\boldsymbol{\omega}_{nb} \times \boldsymbol{j}_{b}</script>
dnkbdt=ωnb×kb
<script id="MathJax-Element-38" type="math/tex; mode=display">\frac{d_{n} \boldsymbol{k}_{b}}{dt}=\boldsymbol{\omega}_{nb} \times \boldsymbol{k}_{b}</script>
所以，最终可以得到下面等式：
dnrdt=dbrdt+ωnb×r
<script id="MathJax-Element-39" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{r}}{dt}=\frac{d_{b}\boldsymbol{r}}{dt} + \boldsymbol{\omega}_{nb} \times \boldsymbol{r} </script>
根据上面的推导可知，同一个向量相对两个不同的坐标系对于时间求导，只有在两个坐标系没有相对旋转时才会相等，否则应该运用哥氏定理来求解。
另外，关于哥氏定理，有两点需要注意！！！
第一点，在上面推导中用到的向量 r <script id="MathJax-Element-40" type="math/tex">\boldsymbol{r}</script>既可以代表速度向量也可以代表位置向量。
例如，将 r <script id="MathJax-Element-41" type="math/tex">\boldsymbol{r}</script>用速度向量 v <script id="MathJax-Element-42" type="math/tex">\boldsymbol{v}</script>替代，则有：
dnvdt=dbvdt+ωnb×v
<script id="MathJax-Element-43" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{v}}{dt}=\frac{d_{b}\boldsymbol{v}}{dt} + \boldsymbol{\omega}_{nb} \times \boldsymbol{v} </script>
第二点，必须理解：
dnrdt≠Cnb⋅dbrdt
<script id="MathJax-Element-44" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{r}}{dt} \ne \boldsymbol{C}_{b}^{n} \cdot \frac{d_{b}\boldsymbol{r}}{dt}</script>
事实上，左边这一项称为绝对导数，是指向量
r <script id="MathJax-Element-45" type="math/tex">\boldsymbol{r}</script>在n系下的时间变化率在b系下的投影，右边这一项是相对导数，指的是向量
r <script id="MathJax-Element-46" type="math/tex">\boldsymbol{r}</script>在b系下的时间变化率在b系下的投影。
两者实际上都是表示在b系下的，不存在上面这个等式的关系。另外，正是由于n系和b系之间存在相对转动，才会导致向量
r <script id="MathJax-Element-47" type="math/tex">\boldsymbol{r}</script>在两个系中的时间变化率不相同，而与两个坐标系之间的相对角速度有关。
3 四麦轮小车运动学模型的构建
在前文中，我们花了很大的篇幅介绍了坐标系的定义、旋转的表达、甚至引入了哥氏定理，这一切都是在为我们建立小车的运动学模型打下基础。下面就从麦克纳姆轮的性质开始，首先探讨了轮速与小车车速的关系，之后讨论了小车车速与加速度、位置之间的关系。
3.1 轮速运动学模型
本部分内容主要参考了一篇知乎专栏《麦克纳姆轮浅谈》。
在这篇博文中，作者介绍了麦克纳姆轮的前世今生，也通过一系列的推导，得出了四麦克纳姆轮小车的轮速与小车车速之间的关系，在这里就不多啰嗦啦，直接使用相关结论，大家感兴趣可以移步去看那一篇博文。
四麦轮小车的结构如图：
小车轮速与车速的关系如下：
⎡⎣⎢⎢⎢ω1ω2ω3ω4⎤⎦⎥⎥⎥=⎡⎣⎢⎢⎢⎢1111−11−11a+b−(a+b)−(a+b)a+b⎤⎦⎥⎥⎥⎥⎡⎣⎢vxvywz⎤⎦⎥
<script id="MathJax-Element-154" type="math/tex; mode=display">\left[ \begin{matrix} \omega_{1} \\ \omega_{2} \\ \omega_{3} \\ \omega_{4} \end{matrix} \right ] = \left [ \begin{matrix} 1 & -1 & a+b \\ 1 & 1 & -(a+b) \\ 1 & -1 & -(a+b) \\ 1 & 1 & a+b \end{matrix} \right ] \left [ \begin{matrix} v_{x} \\ v_{y} \\ w_{z} \end{matrix} \right ] </script>
其中，
ω1 <script id="MathJax-Element-155" type="math/tex">\omega_{1}</script>等代表轮1到轮4的旋转角速度，而
vx,vy <script id="MathJax-Element-156" type="math/tex">v_{x},v_{y}</script>代表小车在体坐标系下的X、Y轴速度，
ωz <script id="MathJax-Element-157" type="math/tex">\omega_{z}</script>代表小车Z轴旋转角速度。上式是小车的逆运动学模型，而小车的正运动学模型，可以通过上面的方程中抽取三个解出来，或者使用最小二乘法的到，在这里不再赘述。
3.2 速度运动学模型
在上一部分中，我们介绍了轮速与车速之间的关系。这个运动学方程主要作用是通过码盘测量轮速，然后间接得到小车车速。而在这一部分的运动学模型推导中，主要就是为了建立小车的加速度、角速度与速度和位移之间的关系。（因为加速度和角速度能够通过IMU测量得到）
参考下面这幅图，定义了导航系n以及小车的体坐标系b（图中未表示的Z轴的方向：垂直纸面向里）。由于大多数情况下，小车还是在平地上运动的，这里假设小车做二维运动。
所以，首先可以推出b系与n系之间的旋转关系。由于是在二维平面上的旋转，不必考虑复杂的欧拉角，可以直接得到旋转矩阵，如下：
Cbn=[cosθ−sinθsinθcosθ]
<script id="MathJax-Element-158" type="math/tex; mode=display">\boldsymbol{C}_{n}^{b}= \left [ \begin{matrix} \cos{\theta} & \sin{\theta} \\ -\sin{\theta} & \cos{\theta} \end{matrix} \right ]</script>
根据牛顿定律，位置是速度的一阶积分，速度是加速度的一阶积分，所以有：
p˙n=Cbn⋅vb
<script id="MathJax-Element-159" type="math/tex; mode=display">\dot{\boldsymbol{p}}^{n}=\boldsymbol{C}_{n}^{b} \cdot \boldsymbol{v}^{b}</script>
v˙b=dnvbdt−ωnb×vb
<script id="MathJax-Element-160" type="math/tex; mode=display">\dot{\boldsymbol{v}}^{b}=\frac{d_{n}\boldsymbol{v}^{b}}{dt} - \boldsymbol{\omega}_{nb} \times \boldsymbol{v}^{b} </script>
θ˙=ωz
<script id="MathJax-Element-161" type="math/tex; mode=display">\dot{\theta}=\omega_{z} </script>
其中，
pn <script id="MathJax-Element-162" type="math/tex">\boldsymbol{p}^{n}</script>代表小车在导航系n中的位置，
vb <script id="MathJax-Element-163" type="math/tex">\boldsymbol{v}^{b}</script>代表小车相对于n系的速度在b系下的表示，
ωnb <script id="MathJax-Element-164" type="math/tex">\boldsymbol{\omega}_{nb}</script>代表b系相对与n系的旋转角速度，
θ <script id="MathJax-Element-165" type="math/tex">\theta</script>代表b系与n系的夹角（见上图），
ωz <script id="MathJax-Element-166" type="math/tex">\omega_{z}</script>代表
ωnb <script id="MathJax-Element-167" type="math/tex">\boldsymbol{\omega}_{nb}</script>在Z轴上的分量。
上面三个式子中，第一个很简单，先把b系下表示的速度通过旋转矩阵投影到n系下，然后积分得到位置。第三个式子也很简单，就是角速度的积分等于角度。而在第二个式子中则用到前文提到的哥氏定理，但是这里产生了一个疑问？
问题：
为什么这里不写成：
v˙b=dbvbdt
<script id="MathJax-Element-168" type="math/tex; mode=display">\dot{\boldsymbol{v}}^{b}=\frac{d_{b}\boldsymbol{v}^{b}}{dt}</script>
而是要写成：
v˙b=dnvbdt−ωnb×vb
<script id="MathJax-Element-169" type="math/tex; mode=display">\dot{\boldsymbol{v}}^{b}=\frac{d_{n}\boldsymbol{v}^{b}}{dt} - \boldsymbol{\omega}_{nb} \times \boldsymbol{v}^{b} </script>
这个形式呢？
回答：
这和传感器能够测量到的量有关。在机器人领域最常用的加速度计传感器只能测量到的是比力，也就是单位质量上作用的非引力外力。用更通俗的话来说，加速度计不能区分载体的运动加速度和引力加速度，只能测量载体相对于惯性空间的绝对加速度和引力加速度之和。用数学的语言表示就是：
f=dnvbdt−G
<script id="MathJax-Element-170" type="math/tex; mode=display">\boldsymbol{f}=\frac{d_{n}\boldsymbol{v}^{b}}{dt}-\boldsymbol{G}</script>
其中，
f <script id="MathJax-Element-171" type="math/tex">\boldsymbol{f}</script>代表加速度计测量到的比力，
G <script id="MathJax-Element-172" type="math/tex">\boldsymbol{G}</script>代表重力矢量，中间的微分项代表相对于惯性空间的绝对加速度（因为我们在第一节中就已经假设了n系是惯性系）。
简单的说就是，我们通常只能测到：
dnvbdt
<script id="MathJax-Element-173" type="math/tex; mode=display">\frac{d_{n}\boldsymbol{v}^{b}}{dt}</script>
而很难测到：
dbvbdt
<script id="MathJax-Element-174" type="math/tex; mode=display">\frac{d_{b}\boldsymbol{v}^{b}}{dt}</script>
所以我们把第二个运动学方程式写成这样。我们还能进一步把运动学方程写为：
v˙b=f−ωnb×vb+G
<script id="MathJax-Element-175" type="math/tex; mode=display">\dot{\boldsymbol{v}}^{b}=\boldsymbol{f} - \boldsymbol{\omega}_{nb} \times \boldsymbol{v}^{b} +\boldsymbol{G} </script>
3.3 运动学模型汇总
在上面两部分的描述后，我们建立了轮速与车速之间的运动学模型，并且建立了车速与加速度、速度、角度、角速度之间的运动学模型。基于这两个运动学模型，我们就能够方便的通过诸如码盘、加速度计、陀螺仪等传感器测量得到轮速、加速度、角速度等，然后再推算出位置、速度、姿态等等余下的状态量。
两个模型的最终形式如下。
1、轮速-车速模型：
⎡⎣⎢vbxvbywz⎤⎦⎥=(JT⋅J)−1JT⎡⎣⎢⎢⎢ω1ω2ω3ω4⎤⎦⎥⎥⎥
<script id="MathJax-Element-70" type="math/tex; mode=display"> \left [ \begin{matrix} v_{x}^{b} \\ v_{y}^{b} \\ w_{z} \end{matrix} \right ] = (\boldsymbol{J}^{T} \cdot \boldsymbol{J})^{-1}\boldsymbol{J}^{T} \left[ \begin{matrix} \omega_{1} \\ \omega_{2} \\ \omega_{3} \\ \omega_{4} \end{matrix} \right ] </script>
其中，
J=⎡⎣⎢⎢⎢⎢1111−11−11a+b−(a+b)−(a+b)a+b⎤⎦⎥⎥⎥⎥
<script id="MathJax-Element-71" type="math/tex; mode=display"> \boldsymbol{J}= \left [ \begin{matrix} 1 & -1 & a+b \\ 1 & 1 & -(a+b) \\ 1 & -1 & -(a+b) \\ 1 & 1 & a+b \end{matrix} \right ] </script>
2、加速度-速度-位置模型：
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪p˙n=Cbn⋅vbv˙b=f−ωnb×vb+Gθ˙=ωz
<script id="MathJax-Element-72" type="math/tex; mode=display">\begin{cases} \dot{\boldsymbol{p}}^{n}=\boldsymbol{C}_{n}^{b} \cdot \boldsymbol{v}^{b} \\ \\ \dot{\boldsymbol{v}}^{b}=\boldsymbol{f} - \boldsymbol{\omega}_{nb} \times \boldsymbol{v}^{b} +\boldsymbol{G} \\ \\ \dot{\theta}=\omega_{z} \end{cases} </script>
将式中的向量都分别展开，得到：
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪p˙nx=cosθ⋅vbx+sinθ⋅vbyp˙ny=−sinθ⋅vbx+cosθ⋅vbyv˙bx=fbx+ωz⋅vbyv˙by=fby−ωz⋅vbxθ˙=ωz
<script id="MathJax-Element-73" type="math/tex; mode=display">\begin{cases} \dot{{p}}_{x}^{n}=\cos{\theta} \cdot v_{x}^{b} + \sin{\theta} \cdot v_{y}^{b} \\ \\ \dot{{p}}_{y}^{n}=-\sin{\theta} \cdot v_{x}^{b} + \cos{\theta} \cdot v_{y}^{b} \\ \\ \dot{{v}}_{x}^{b}=f_{x}^{b}+\omega_{z} \cdot v_{y}^{b}\\ \\ \dot{{v}}_{y}^{b}=f_{y}^{b}-\omega_{z} \cdot v_{x}^{b}\\ \\ \dot{\theta}=\omega_{z} \end{cases} </script>
其中，由于假设了小车在水平面上运动，所以重力不影响X和Y轴上的加速度。另外，各个变量的含义前文中也介绍得很清楚了，不再复述。
小结
这篇文章中，首先介绍了旋转之间的互相转换。这里貌似还看不出它们的作用，但是在下一篇介绍四旋翼运动学模型的文章中，你将会看到它们有多么重要。之后，引入了很重要的哥氏定理。这主要是因为加速度计的测量值包括两部分：相对惯性空间的绝对加速度以及引力加速度。而相对惯性空间的绝对加速度中包含了由于物体转动引起的附加加速度，只有去除这部分有害加速度，才能积分得到物体相对与惯性系的速度在物体系下的投影（需要仔细理解一下哥氏定理）。最后，讨论了四麦轮小车的运动学模型。
限于篇幅，四旋翼无人机运动学模型的构建将放在下一篇文章中介绍。由于从二维空间进入了三维空间，关于旋转的部分理解起来难度会加大，另外也将会引入重要的姿态更新方法，敬请期待啦！！！
10
0
已为社区贡献12条内容
所有评论(0)