基于慣性導(dǎo)航對(duì)物體運(yùn)動(dòng)狀態(tài)的推算

姚悅東 閆帥 吳林晉 米心馨

摘 要：為了解決獲取物體在空間中的位置信息，本文研究試驗(yàn)出一種基于慣性導(dǎo)航系統(tǒng)推算物體運(yùn)動(dòng)狀態(tài)和空間位置的定位方式。此定位方式可以不受環(huán)境限制、原理簡(jiǎn)單、可行性強(qiáng)，能有效滿足學(xué)者需求。

關(guān)鍵詞：慣性導(dǎo)航;運(yùn)動(dòng)狀態(tài);定位方式

中圖分類(lèi)號(hào)：TN96;TP212文獻(xiàn)標(biāo)識(shí)碼：A文章編號(hào)：1003-5168（2019）11-0059-02

Abstract： In order to obtain the position information of objects in space， this paper studied and tested a positioning method based on inertial navigation system to calculate the motion state and position of objects. This positioning method is not limited by the environment， simple in principle， strong in feasibility， and can effectively meet the needs of scholars.

Keywords： inertial navigation;motion state;location mode

隨著科技發(fā)展的日新月異，無(wú)人機(jī)航拍、室內(nèi)掃地機(jī)器人、無(wú)人駕駛汽車(chē)等一系列高科技產(chǎn)物逐漸融入人們的生活。但目前，這些產(chǎn)品都面臨著如何獲取物體在空間中的位置信息的問(wèn)題。通常情況下，常用的定位方法主要有基于衛(wèi)星的GPS定位、基于機(jī)器視覺(jué)的SLAM定位和基于運(yùn)動(dòng)傳感器的慣性導(dǎo)航定位等。這些定位方法各有優(yōu)缺點(diǎn)，有的計(jì)算量復(fù)雜，有的只能在室外定位，有的對(duì)環(huán)境要求苛刻。由此，本文提出通過(guò)慣性導(dǎo)航推算出物體運(yùn)動(dòng)狀態(tài)和空間位置。該方法不受環(huán)境限制，只需要在物體上安裝一個(gè)陀螺儀，測(cè)得基本的運(yùn)動(dòng)數(shù)據(jù)即可定位。同時(shí)，其計(jì)算量不大，32系列的單片機(jī)即可工作

1 試驗(yàn)裝置設(shè)計(jì)

使用一臺(tái)無(wú)人機(jī)，通過(guò)采集其飛行控制器內(nèi)置的電子羅盤(pán)和陀螺儀提供的數(shù)據(jù)建立慣性導(dǎo)航系統(tǒng)。

制作一臺(tái)裝有色板的遙控小車(chē)，在無(wú)人機(jī)頭安裝機(jī)器視覺(jué)攝像頭OPENMV，在空中，攝像頭實(shí)時(shí)返回小車(chē)的位置信息。

將小車(chē)的位置與無(wú)人機(jī)慣性導(dǎo)航得出的位置融合建立空間模型，使用驅(qū)動(dòng)算法使無(wú)人機(jī)追蹤小車(chē)[1]。

2 試驗(yàn)步驟

①建立慣性導(dǎo)航系統(tǒng)。試驗(yàn)采用地球固聯(lián)坐標(biāo)系作為慣性導(dǎo)航坐標(biāo)系，以無(wú)人機(jī)起飛位置作為坐標(biāo)原點(diǎn)。設(shè)X軸在水平面內(nèi)，指向某一方向，Y軸垂直于地面向下。然后，按右手定則確定Z軸。

②通過(guò)電子羅盤(pán)與陀螺儀測(cè)得數(shù)據(jù)解算姿態(tài)。在使用慣性導(dǎo)航公式時(shí)，需要解算出物體的姿態(tài)角，最常用的方法有兩種：一種是通過(guò)歐拉角微分方程;另一種是使用四元數(shù)軟件解算。而在使用歐拉公式進(jìn)行運(yùn)算時(shí)，由于歐拉角微積分方程中有大量三角函數(shù)出現(xiàn)，使得解決問(wèn)題變得尤為復(fù)雜。同時(shí)，歐拉公式具有限制性，其僅適用于姿態(tài)角變化量較小的情況，不適合全姿態(tài)的飛行姿態(tài)的確定。例如，俯仰角在90°時(shí)，出現(xiàn)萬(wàn)向節(jié)鎖死的情況，限制了導(dǎo)航功效的展現(xiàn)。而四元數(shù)法因只含有四個(gè)未知量，計(jì)算量較小，是比較實(shí)用的工程方法。

③通過(guò)周期性速度積分獲得位移、更新物體坐標(biāo)。陀螺儀可形成一個(gè)導(dǎo)航坐標(biāo)系的坐標(biāo)測(cè)量軸，以穩(wěn)定與反饋出航向軌跡角和偏航姿態(tài)角;而用來(lái)測(cè)量運(yùn)動(dòng)體加速度大小的加速度計(jì)通過(guò)測(cè)量運(yùn)動(dòng)體的加速度經(jīng)過(guò)對(duì)時(shí)間的一次積分得到速度，速度再經(jīng)過(guò)對(duì)時(shí)間的一次積分即可得到距離[2]。

④使用OPENMV視覺(jué)模塊獲得小車(chē)位置。在試驗(yàn)中，每隔2ms對(duì)傳感器數(shù)據(jù)進(jìn)行一次更新，帶入慣性導(dǎo)航方程，推算出無(wú)人機(jī)在空間的位置。

OPENMV是一個(gè)機(jī)器視覺(jué)模塊，其內(nèi)置一個(gè)攝像頭，進(jìn)行顏色識(shí)別。為了實(shí)現(xiàn)準(zhǔn)確識(shí)別，筆者選擇了和地面反差較大的紅色。通過(guò)編寫(xiě)程序，使模塊每10ms對(duì)攝像頭拍攝的圖像進(jìn)行掃描，提取整幅圖像中顏色閾值接近紅色的區(qū)域，并將其中心坐標(biāo)發(fā)送給無(wú)人機(jī)飛行控制器。為了便于調(diào)試，制作了一輛小車(chē)，在其車(chē)身上放置了一塊紅色板，通過(guò)手機(jī)藍(lán)牙APP進(jìn)行遙控，經(jīng)測(cè)試其最大速度為1m/s。

⑤算法驅(qū)動(dòng)無(wú)人機(jī)跟蹤小車(chē)。在獲得小車(chē)與無(wú)人機(jī)在空間的位置后，使用卡爾曼濾波、串級(jí)PID等算法，同步演算和規(guī)劃出小車(chē)和無(wú)人機(jī)的相對(duì)位置，對(duì)小車(chē)進(jìn)行反饋性追蹤。在反復(fù)試驗(yàn)，整定參數(shù)后，取得了較好的追蹤效果。

3 該方法的創(chuàng)新點(diǎn)

3.1 使用互補(bǔ)濾波器解算姿態(tài)角

姿態(tài)角可以由加速度計(jì)和電子羅盤(pán)測(cè)量得到，漂移幅度小，卻會(huì)產(chǎn)生較大噪聲。同時(shí)，通過(guò)加速度計(jì)測(cè)得角速度積分后得到的偏航姿態(tài)角產(chǎn)生的噪聲小，但漂移幅度大。互補(bǔ)濾波器的基本思想是利用其各自的優(yōu)勢(shì)，在頻域上特征互補(bǔ)，得到更精確的姿態(tài)角。

互補(bǔ)濾波器估計(jì)俯仰角試驗(yàn)仿真圖如圖1所示，其中acc、wx和cf分別表示利用加速度估計(jì)的俯仰角、角速度直接積分后的俯仰角以及互補(bǔ)濾波后的俯仰角，利用Pixhawk自駕儀傳感器實(shí)時(shí)傳回的數(shù)據(jù)進(jìn)行一階向后差分法估計(jì)。從結(jié)果不難看出，通過(guò)互補(bǔ)濾波器，可以得到平滑的俯仰角估值，而對(duì)陀螺儀直接進(jìn)行積分，結(jié)果是發(fā)散的。

3.2 使用離散卡爾曼濾波估算物體位置

卡爾曼濾波器是一種遞歸濾波器，可將過(guò)去的測(cè)量估計(jì)誤差合并納入新的測(cè)量誤差中來(lái)估計(jì)將來(lái)的誤差。在無(wú)人機(jī)的位置估算中引入離散卡爾曼濾波器，通過(guò)對(duì)運(yùn)動(dòng)狀態(tài)卡爾曼濾波，推測(cè)下一時(shí)刻無(wú)人機(jī)位置，提前輸入PID系統(tǒng)做出響應(yīng)。在小車(chē)位置識(shí)別中引入離散卡爾曼濾波器，推測(cè)下一幀圖像中小車(chē)的位置，縮小圖片檢測(cè)范圍，加快處理速度。

3.3 融合慣導(dǎo)參數(shù)矯正物體位置誤差

圖2是使用OPENMV 攝像頭識(shí)別色板后輸出顏色位置。但是在無(wú)人機(jī)實(shí)際飛行中，為了追蹤小車(chē)機(jī)身會(huì)傾斜，攝像頭識(shí)別到的小車(chē)位置會(huì)產(chǎn)生誤差，這種誤差經(jīng)過(guò)PID控制器放大后會(huì)導(dǎo)致系統(tǒng)嚴(yán)重超調(diào)。通過(guò)慣導(dǎo)數(shù)據(jù)的融合，小車(chē)的位置在機(jī)身傾斜的情況下基本不會(huì)改變，保證了整個(gè)系統(tǒng)的穩(wěn)定性。

3.4 串級(jí)PID控制物體跟蹤

在無(wú)人機(jī)追蹤小車(chē)的過(guò)程中，普通的單級(jí)PID已經(jīng)無(wú)法滿足系統(tǒng)迅速消除誤差的需求。因此，基于串級(jí)PID控制以小車(chē)速度作為內(nèi)環(huán)使用。將小車(chē)位置設(shè)置為外環(huán)，由于控制系統(tǒng)中存在的內(nèi)環(huán)回路，原有的對(duì)象特征被改變，等效時(shí)間常數(shù)變小，整個(gè)控制系統(tǒng)的過(guò)渡時(shí)間也被縮短，控制穩(wěn)定性得到提高，并且外環(huán)控制器依據(jù)誤差變換實(shí)時(shí)校準(zhǔn)內(nèi)環(huán)控制性的預(yù)設(shè)值，使得位置突發(fā)改變時(shí)，控制系統(tǒng)仍有較好的控制效果。

4 試驗(yàn)驗(yàn)證

在不斷測(cè)試后，筆者整定了如圖3所示的參數(shù)。導(dǎo)入經(jīng)過(guò)飛控輸出的慣性導(dǎo)航速度信息與OPENMV 輸出的小車(chē)速度信息做差，由MATLAB工具箱畫(huà)出無(wú)人機(jī)在25s內(nèi)追蹤速度圖。

由圖3可知，誤差曲線基本平穩(wěn)，實(shí)物追蹤效果良好。

5 結(jié)語(yǔ)

本作品通過(guò)對(duì)無(wú)人機(jī)建立慣性導(dǎo)航系統(tǒng)，有效完成了對(duì)小車(chē)的追蹤，是一次對(duì)經(jīng)典力學(xué)的拓展與延伸。本作品可以直接應(yīng)用于無(wú)人機(jī)在空中的定位問(wèn)題，亦可以推廣到機(jī)器人、無(wú)人汽車(chē)等定位問(wèn)題上。

參考文獻(xiàn)：

[1]楊鴻瑞.牛頓力學(xué)在慣性導(dǎo)航系統(tǒng)中的應(yīng)用[J].科技創(chuàng)新導(dǎo)報(bào)，2015（32）：243-247.

[2]鄧一民. 多旋翼無(wú)人機(jī)光流/慣性組合導(dǎo)航技術(shù)研究[D].南京：南京航空航天大學(xué)，2016.