喬志健　康永　許峰榕

1引言

目前，自主移動(dòng)機(jī)器人在諸如家庭服務(wù)，養(yǎng)老輔助，教育等領(lǐng)域迅速發(fā)展，而機(jī)器人定位技術(shù)是可自主移動(dòng)的機(jī)器人的技術(shù)關(guān)鍵點(diǎn)，也是近年來研究熱點(diǎn)之一?？勺灾饕苿?dòng)機(jī)器人的定位方式可分為相對(duì)定位和絕對(duì)定位兩種方式。相對(duì)定位也稱之為航跡推算（dead reckoning）。它通過裝在輪子上的磁編碼器來估算機(jī)器人的速度，通過自帶的慣性元器件測(cè)量機(jī)器人的姿態(tài)信息，因此它并不依賴外部信號(hào)，就能估算出機(jī)器人相對(duì)于起始位置的位姿信息。近年來，mems技術(shù)快速發(fā)展，使得這種定位方法的成本逐漸降低，成為一種低成本，高效率，實(shí)時(shí)性強(qiáng)的機(jī)器人定位方式。

2 機(jī)械與電路設(shè)計(jì)

如圖一所示，機(jī)械結(jié)構(gòu)由可安放主電路板的凹槽和兩個(gè)正交安放并裝有磁編碼器的從動(dòng)輪組成。主電路板捷聯(lián)陀螺儀芯片（icm20602-g）與三軸電子羅盤（LSM303AGR）可提供角速度，加速度，地磁信息。從動(dòng)輪軸心裝有隨輪轉(zhuǎn)動(dòng)的磁鐵，而磁編碼器固定在機(jī)械結(jié)構(gòu)上不隨其轉(zhuǎn)動(dòng)，由此可獲取從動(dòng)輪轉(zhuǎn)動(dòng)圈數(shù)，結(jié)合輪子半徑參數(shù)，可算出從動(dòng)輪的里程。同時(shí)，兩個(gè)從動(dòng)輪的外側(cè)安裝了兩個(gè)彈簧，這使其可以適應(yīng)不平整的地面，并增大對(duì)地面的壓力，增大摩擦力，減少打滑。

3 姿態(tài)解算

常用的姿態(tài)描述方法有歐拉角與四元數(shù)兩種。歐拉角便于用戶使用，但是歐拉角本身在運(yùn)算中卻具有極大的缺點(diǎn)： 一，歐拉角運(yùn)算的時(shí)候會(huì)有大量的三角函數(shù)運(yùn)算，計(jì)算量大；二，歐拉角存在萬向節(jié)死鎖。因此本文采用旋轉(zhuǎn)四元數(shù)描述旋轉(zhuǎn)過程，旋轉(zhuǎn)四元數(shù)可以表示成如下形式：

（1）

上式中 描述旋轉(zhuǎn)角度， 是一個(gè)單位向量，描述的是旋轉(zhuǎn)軸的方向，所以上述四元數(shù)描述的是物體繞 旋轉(zhuǎn)軸旋轉(zhuǎn)了 。

通過查閱文獻(xiàn)可以知道四元數(shù)微分如下：

其中Gyrox，Gyroy，Gyroz分別表示陀螺儀測(cè)量輸出的三軸角速度。結(jié)合上式以及最新獲取的角速度信息，使用四階龍哥庫塔可對(duì)四元數(shù)進(jìn)行更新。

由于 是單位向量，所以滿足：

從上面的式子就可以得到四元數(shù)描述的旋轉(zhuǎn)對(duì)應(yīng)的坐標(biāo)變換公式，如下所示：

另外，可獲取加速度的信息：x軸加速度分量measure.x，y軸加速度分量measure.y，z軸加速度分量measure.z。并通過以下公式計(jì)算出歐拉角信息：

最后，通過卡爾曼濾波算法對(duì)陀螺儀解算出的歐拉角與加速度計(jì)解算出的歐拉角進(jìn)行融合，得出新姿態(tài)。

4 IMU冗余配置

陀螺儀的角度隨機(jī)游走噪聲是一段白噪聲，通常使用ARMA時(shí)間序列分析法對(duì)陀螺儀靜態(tài)輸出進(jìn)行建模，得到的模型可作為卡爾曼濾波器狀態(tài)方程，對(duì)噪聲進(jìn)行過濾。本文使用的冗余配置常用于故障診斷中，但由于陀螺儀的角度隨機(jī)游走噪聲近似為高斯分布，因此陀螺儀的冗余配置同樣可以減少噪聲的強(qiáng)度，達(dá)到濾波的效果。本文使用三個(gè)IMU單元進(jìn)行同時(shí)測(cè)量，并以三個(gè)IMU單元在靜止?fàn)顟B(tài)下輸出的方差的大小對(duì)三個(gè)IMU單元融合的權(quán)重進(jìn)行確定。如下圖所示為三個(gè)IMU輸出姿態(tài)與三個(gè)IMU冗余配置輸出結(jié)果（由上至下第三條）。結(jié)果表明，IMU冗余配置能有效減小陀螺儀角度隨機(jī)游走帶來的影響。

5 坐標(biāo)計(jì)算

5.1 坐標(biāo)解算過程

由航跡推算法可得，在很短的時(shí)間間隔內(nèi)，可以將整個(gè)系統(tǒng)的運(yùn)動(dòng)狀態(tài)看成是姿態(tài)不變的直線運(yùn)動(dòng)。由于采樣時(shí)間和計(jì)算時(shí)間都很短（位姿計(jì)算周期5ms），本系統(tǒng)適用上述規(guī)律，因此可以采用下述算法，結(jié)果如下。

5.2 機(jī)械參數(shù)的矯正

如下圖，矯正裝置由一根直線導(dǎo)軌和一個(gè)可在滑軌上滑動(dòng)的連接器構(gòu)成。連接器可帶動(dòng)定位系統(tǒng)機(jī)加件在導(dǎo)軌上滑動(dòng)，并且可以在水平面上進(jìn)行轉(zhuǎn)動(dòng)。

通過磁編碼器可間接計(jì)算出輪子轉(zhuǎn)動(dòng)的圈數(shù)num1和num2，并使定位裝置在連接器的帶動(dòng)下，不改變角度，從導(dǎo)軌的起點(diǎn)走到終點(diǎn)。其中導(dǎo)軌的長度為729mm。根據(jù)機(jī)械誤差模型可建立以下等式。

如圖中所示，一個(gè)輪子的實(shí)際半徑為 25.44mm， 另一個(gè)輪子的半徑為 25.31mm， 并且機(jī)械角度誤差為0.3度。

6 結(jié)論

航跡推算法是一種低成本，實(shí)時(shí)性強(qiáng)的機(jī)器人定位算法，但是累積誤差一直是阻礙其性能的關(guān)鍵因素。本文通過陀螺儀和加速度計(jì)解算出機(jī)器人姿態(tài)，并用卡爾曼濾波對(duì)數(shù)據(jù)進(jìn)行融合，同時(shí)，采用冗余配置減小陀螺儀輸出噪聲。并且，為了提高坐標(biāo)計(jì)算精度，本文設(shè)計(jì)算法矯正機(jī)械加工誤差。實(shí)驗(yàn)證明，本文提出的這些方法能有效提高傳統(tǒng)航跡推算法的精度，為航跡推算法的研究提供參考。

（作者單位：東北大學(xué)信息科學(xué)與工程學(xué)院）