基于多傳感器數(shù)據(jù)融合的機(jī)器人定位系統(tǒng)

喬志健　康永　許峰榕

1引言

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

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

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

3 姿態(tài)解算

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

（1）

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

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

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

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

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

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

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

4 IMU冗余配置

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

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

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

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

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

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

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

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

6 結(jié)論

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

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