基于慣性導(dǎo)航對物體運動狀態(tài)的推算

姚悅東 閆帥 吳林晉 米心馨

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

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

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

1 試驗裝置設(shè)計

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

制作一臺裝有色板的遙控小車，在無人機頭安裝機器視覺攝像頭OPENMV，在空中，攝像頭實時返回小車的位置信息。

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

2 試驗步驟

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.4 串級PID控制物體跟蹤

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

4 試驗驗證

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

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

5 結(jié)語

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

參考文獻：

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

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