Hello! 歡迎來(lái)到我的博客
今天的內(nèi)容關(guān)于機(jī)器人中常用的傳感器IMU，我們用它來(lái)實(shí)現(xiàn)機(jī)器人姿態(tài)、速度、位置的估計(jì)。
今天將會(huì)介紹使用低成本IMU進(jìn)行機(jī)器人運(yùn)動(dòng)估計(jì)的一個(gè)常用方法——ESKF。

1 四元數(shù)基礎(chǔ)

四元數(shù)，英文名稱為：quaternion。正如一個(gè)單位復(fù)數(shù)可以表示在復(fù)平面上的旋轉(zhuǎn)一樣，單位四元數(shù)也可以用于表示三維空間中的旋轉(zhuǎn)，并且由于其在更新過(guò)程中不會(huì)發(fā)生奇異現(xiàn)象，因此在慣性導(dǎo)航中被廣泛使用。一般來(lái)說(shuō)，四元數(shù)的表示方式有許多，其中最常用的是兩種，一種是Hamilton，另一種是JPL。由于，在機(jī)器人領(lǐng)域中，無(wú)論是ROS，Eigen，Ceres等等項(xiàng)目中都用到的是Hamilton四元數(shù)，因此，這里后續(xù)文章中使用以及介紹的也是這種。

以下介紹的都是在IMU運(yùn)動(dòng)方程以及ESKF中必要的關(guān)于四元數(shù)的知識(shí)，因此可能會(huì)過(guò)于簡(jiǎn)略，讀者可暫時(shí)先跳轉(zhuǎn)至第二部分閱讀，遇到不明的四元數(shù)定義時(shí)，再回來(lái)查閱。

（1）四元數(shù)定義

$$Q=a+bi+cj+dk\in \mathbb{H}$$
其中，$\{a,b,c,d\}\in \mathbb{R}$是系數(shù)，$\{i,j,k\}$是虛部單位。
一個(gè)四元數(shù)可以表示為實(shí)部和虛部，如下：
$$Q=q_w+q_{x}i+q_{y}j+q_{z}k?Q=q_w+{\mathbf{q}}_v$$
記作向量形式則為：
$$\mathbf{q}?\left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]=?\begin{array}{l}{q}_w\\ q_x\\ q_y\\ q_z\end{array}?$$

（2）單位四元數(shù)

單位四元數(shù)可以作為一種旋轉(zhuǎn)的表示方式，它可以轉(zhuǎn)換為歐拉角，也可以轉(zhuǎn)換為旋轉(zhuǎn)矩陣。
單位四元數(shù)的定義：
$$\mathbf{q}=\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$
其中，$\mathbf{u}=u_{x}i+u_{y}j+u_{z}k$是單位向量，而$\theta$為一個(gè)標(biāo)量。
由此可知，單位四元數(shù)的模長(zhǎng)為1。
$$||\mathbf{q}||=\sqrt{{\cos }^2\theta +{\sin }^2\theta (u_x^2+u_y^2+u_z^2)}=1$$

（3）乘法

兩個(gè)四元數(shù)乘法的記號(hào)為：$?$，乘法的具體定義為：
$$\mathbf{p}?\mathbf{q}=?\begin{array}{l}{p}_w{q}_w?p_x{q}_x?p_y{q}_y?p_z{q}_z\\ p_w{q}_x+p_x{q}_w+p_y{q}_z?p_z{q}_y\\ p_w{q}_y?p_x{q}_z+p_y{q}_w+p_z{q}_x\\ p_w{q}_z+p_x{q}_y?p_y{q}_x+p_z{q}_w\end{array}?$$
值得注意的是，兩個(gè)單位四元數(shù)相乘還是單位四元數(shù)。

（4）純四元數(shù)的指數(shù)函數(shù)

純四元數(shù)的定義為，實(shí)部為0的四元數(shù)，即：$\mathbf{v}=\left[\begin{array}{l}0\\ {\mathbf{q}}_v\end{array}\right]$

定義純四元數(shù)的指數(shù)函數(shù)如下：
$$e^{\mathbf{v}}=e^{\mathbf{u}\theta }=\cos \theta +\mathbf{u}\sin \theta =\left[\begin{array}{c}\cos \theta \\ \mathbf{u}\sin \theta \end{array}\right]$$
其中：$\mathbf{v}=\mathbf{u}\theta$，并且$\theta =\Vert \mathbf{v}\Vert \in \mathbb{R}$，$\mathbf{u}$為單位向量。

（5）單位四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣

$$\mathbf{R}=\mathbf{R}\{\mathbf{q}\}=?\begin{array}{ccc}{q}_w^2+q_x^2?q_y^2?q_z^2& 2\left(q_x{q}_y?q_w{q}_z\right)& 2\left(q_x{q}_z+q_w{q}_y\right)\\ 2\left(q_x{q}_y+q_w{q}_z\right)& q_w^2?q_x^2+q_y^2?q_z^2& 2\left(q_y{q}_z?q_w{q}_x\right)\\ 2\left(q_x{q}_z?q_w{q}_y\right)& 2\left(q_y{q}_z+q_w{q}_x\right)& q_w^2?q_x^2?q_y^2+q_z^2\end{array}?$$
注意，必須是單位四元數(shù)！！！

（6）單位四元數(shù)轉(zhuǎn)歐拉角

由于歐拉角的定義方式也有很多，這里說(shuō)明的是依據(jù)Z-Y-X定義的歐拉角，其中歐拉角與旋轉(zhuǎn)矩陣的變換關(guān)系是：
$$\mathbf{R}=?\begin{array}{ccc}\cos \psi \cos \theta & \cos \psi \sin \theta \sin ??\sin \psi \cos ?& \cos \psi \sin \theta \cos ?+\sin \psi \sin ?\\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin ?+\cos \psi \cos ?& \sin \psi \sin \theta \cos ??\cos \psi \sin ?\\ ?\sin \theta & \cos \theta \sin ?& \cos \theta \cos ?\end{array}?$$
其中：$?,\theta ,\psi ?$為歐拉角，分別為橫滾角(X)，俯仰角(Y)，偏航角(Z)。
所以，四元數(shù)轉(zhuǎn)歐拉角的關(guān)系為：
$$\begin{array}{rl}?& =\arctan \frac{2\left(q_y{q}_z+q_w{q}_x\right)}{q_w^2?q_x^2?q_y^2+q_z^2}\\ \theta & =?\arcsin [2\left(q_x{q}_z?q_w{q}_y\right)]\\ \psi & =\arctan \frac{2\left(q_x{q}_y+q_w{q}_z\right)}{q_w^2+q_x^2?q_y^2?q_z^2}\end{array}$$
注意，必須是單位四元數(shù)！！！

2 IMU運(yùn)動(dòng)方程

2.1 關(guān)于IMU測(cè)量數(shù)據(jù)

在聊到IMU測(cè)量數(shù)據(jù)的時(shí)候，我們首先需要明白兩個(gè)坐標(biāo)系的定義。

一是慣性系，二是IMU坐標(biāo)系。

這里的慣性系就是指靜止或者其速度的改變可以忽略不記的坐標(biāo)系。通常在機(jī)器人的應(yīng)用中，由于所用的IMU都是低成本IMU，對(duì)于地球自轉(zhuǎn)角速度不夠敏感，所以可以認(rèn)為與地面固連的坐標(biāo)系都是慣性坐標(biāo)系。然而對(duì)于，航空航天應(yīng)用而言，用到的IMU精度很高，對(duì)于慣性導(dǎo)航的要求也很嚴(yán)格，則需要考慮地球自轉(zhuǎn)角速度，這是慣性坐標(biāo)系為地球慣性坐標(biāo)系。

IMU測(cè)量的實(shí)際上就是IMU坐標(biāo)系相對(duì)于慣性系的加速度和角速度。值得注意的是，這里的相對(duì)于慣性系的加速度不僅包括載體的運(yùn)動(dòng)加速度，還包括重力加速度。
所以，我們需要特別注意，加速度計(jì)測(cè)量的不是載體的運(yùn)動(dòng)加速度，而是載體相對(duì)慣性空間的絕對(duì)加速度和引力加速度之和，稱作“比力”。

通常，IMU的測(cè)量模型如下：
$$\begin{array}{l}{\mathbf{a}}_m={\mathbf{R}}_t^{?}\left({\mathbf{a}}_t?{\mathbf{g}}_t\right)+{\mathbf{a}}_{bt}+{\mathbf{a}}_n\\ {\mathbf{\omega }}_m={\mathbf{\omega }}_t+{\mathbf{\omega }}_{bt}+{\mathbf{\omega }}_n\end{array}$$
其中，${\mathbf{R}}_t^{?}$表示從慣性系到IMU坐標(biāo)系的旋轉(zhuǎn)矩陣。${\mathbf{a}}_{bt},{\mathbf{\omega }}_{bt}$代表隨機(jī)游走噪聲，可以理解為加速度計(jì)和陀螺儀的零漂。${\mathbf{a}}_n,{\mathbf{\omega }}_n$為零均值高斯白噪聲。${\mathbf{g}}_t$代表重力加速度，表示在慣性系下。這里關(guān)于${\mathbf{g}}_t$的符號(hào)不用特別在意，這個(gè)負(fù)號(hào)并不是一定的，也可以寫(xiě)成正號(hào)，取決于${\mathbf{g}}_t$的定義。

2.2 IMU運(yùn)動(dòng)方程

IMU的運(yùn)動(dòng)方程主要描述了物體速度，位置以及姿態(tài)在IMU測(cè)量數(shù)據(jù)的激勵(lì)下產(chǎn)生的變化。除了關(guān)于四元數(shù)的部分以外，用到的知識(shí)就是小學(xué)二年級(jí)學(xué)過(guò)的物理知識(shí)。比如，位置的導(dǎo)數(shù)是速度，速度的導(dǎo)數(shù)是加速度。
我們定義我們關(guān)心的狀態(tài)量為：

• ${\mathbf{p}}_t$：表示IMU系相對(duì)慣性系的位置，在慣性系下表示的，默認(rèn)是三維的
• ${\mathbf{v}}_t$：表示IMU系相對(duì)慣性系的速度，同樣是在慣性系下表示的，默認(rèn)是三維的
• ${\mathbf{q}}_t$：表示從IMU系到慣性系的四元數(shù)，表示從IMU系到慣性系的旋轉(zhuǎn)變換

于是，IMU的運(yùn)動(dòng)方程可以表示如下：
$$\begin{array}{rl}{\dot{\mathbf{p}}}_t& ={\mathbf{v}}_t\\ {\dot{\mathbf{v}}}_t& ={\mathbf{a}}_t={\mathbf{R}}_t\left({\mathbf{a}}_m?{\mathbf{a}}_{bt}?{\mathbf{a}}_n\right)+{\mathbf{g}}_t\\ {\dot{\mathbf{q}}}_t& =\frac{1}{2}{\mathbf{q}}_t?{\mathbf{\omega }}_t=\frac{1}{2}{\mathbf{q}}_t?\left({\mathbf{\omega }}_m?{\mathbf{\omega }}_{bt}?{\mathbf{\omega }}_n\right)\\ {\dot{\mathbf{a}}}_{bt}& ={\mathbf{a}}_w\\ {\dot{\mathbf{\omega }}}_{bt}& ={\mathbf{\omega }}_w\\ {\dot{\mathbf{g}}}_t& =0\end{array}$$
其中，${\mathbf{R}}_t=\mathbf{R}\{{\mathbf{q}}_t\}$為基于${\mathbf{q}}_t$得到的旋轉(zhuǎn)矩陣，表示了從IMU系到慣性系的旋轉(zhuǎn)變換。${\mathbf{a}}_w,{\mathbf{\omega }}_w$為零均值高斯白噪聲。

值得注意的是，在這組運(yùn)動(dòng)方程中，把${\mathbf{g}}_t$也視作變化的量，后續(xù)基于此運(yùn)動(dòng)方程構(gòu)造的ESKF也會(huì)對(duì)${\mathbf{g}}_t$進(jìn)行更新。這在許多IMU運(yùn)動(dòng)方程中是不常見(jiàn)到的，這樣做有什么好處呢？

首先，我們要明白${\mathbf{g}}_t$并不等于$[0,0,9.8{]}^T$，${\mathbf{g}}_t$的值取決于IMU的初始姿態(tài)。這是因?yàn)?#xff0c;IMU運(yùn)動(dòng)方程是遞推的方程，計(jì)算出來(lái)的位置和速度都是相對(duì)于初始狀態(tài)的，而不是絕對(duì)的位置和速度。因此為了方便起見(jiàn)，我們通常都會(huì)定義初始時(shí)刻的IMU坐標(biāo)系為慣性系，這樣我們后面計(jì)算出來(lái)的雖然是相對(duì)于初始IMU系的位置和速度，但也可以作為絕對(duì)的位置和速度了。于是，${\mathbf{g}}_t$既然代表了慣性系下的重力加速度，也就是初始的IMU坐標(biāo)系下表示的重力加速度，所以當(dāng)初始IMU坐標(biāo)系不是水平的時(shí)候，${\mathbf{g}}_t$自然也就不等于$[0,0,9.8{]}^T$了。

進(jìn)一步地，我們也可以知道，估計(jì)${\mathbf{g}}_t$，也就是估計(jì)初始時(shí)刻IMU的姿態(tài)。既然我們通過(guò)可以${\mathbf{g}}_t$估計(jì)了初始IMU的姿態(tài)，那么初始時(shí)刻的四元數(shù)就可以直接定義為${\mathbf{q}}_0=(1,0,0,0)$。這種操作實(shí)際上就是，我們先假設(shè)初始時(shí)刻四元數(shù)為${\mathbf{q}}_0=(1,0,0,0)$，然后在此基礎(chǔ)上估計(jì)${\mathbf{g}}_t$。當(dāng)然，還有另外一種方法就是，先假設(shè)${\mathbf{g}}_t=[0,0,9.8]$，然后估計(jì)初始時(shí)刻的IMU相距水平姿態(tài)相差多少。這兩種方法都可以，然后估計(jì)${\mathbf{g}}_t$的話可以使模型的線性化程度更好。

為了更好的閱讀體驗(yàn)，本文分為上下兩文，關(guān)于ESKF的內(nèi)容以及MATLAB代碼，請(qǐng)移步下一篇文章。

總結(jié)

以上是生活随笔為你收集整理的机器人运动估计——IMU运动方程与ESKF原理介绍（上）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。