包建華，喬 曦，李道亮

?

雙矢量定姿算法提高海參捕撈裝置捷聯(lián)慣導(dǎo)系統(tǒng)粗對準(zhǔn)精度

包建華1,2,3，喬 曦1,3，李道亮1,3※

（1. 中國農(nóng)業(yè)大學(xué)信息與電氣工程學(xué)院，北京 100083；2. 江蘇師范大學(xué)電氣工程及自動化學(xué)院，徐州 221116；3. 北京農(nóng)業(yè)物聯(lián)網(wǎng)工程技術(shù)研究中心，北京 100083）

針對傳統(tǒng)解析粗對準(zhǔn)算法中水平對準(zhǔn)精度同時受陀螺儀和加速度計測量誤差影響的問題，提出一種雙矢量定姿粗對準(zhǔn)算法，該算法在選定重力矢量為主參考矢量的前提下，預(yù)先對參與姿態(tài)解算的矢量作單位正交化處理。理論分析表明，所述算法使得水平對準(zhǔn)精度只受加速度計水平測量誤差影響，提高了捷聯(lián)慣導(dǎo)系統(tǒng)粗對準(zhǔn)精度；實測數(shù)據(jù)的試驗結(jié)果表明，所述粗對準(zhǔn)算法的水平誤差角的大小不超過1°，方位誤差角的大小不超過3°，在粗對準(zhǔn)所要求的誤差范圍之內(nèi)，為后續(xù)利用濾波方法進(jìn)行精對準(zhǔn)提供了有效的初始條件。

漁業(yè)；導(dǎo)航；算法；捷聯(lián)慣導(dǎo)；粗對準(zhǔn)；雙矢量定姿；靜基座；海參捕撈

0 引 言

用信息技術(shù)提升農(nóng)業(yè)機(jī)械化水平是建設(shè)現(xiàn)代農(nóng)業(yè)的戰(zhàn)略選擇，為加強(qiáng)農(nóng)機(jī)裝備的信息技術(shù)創(chuàng)新，需要突破智能農(nóng)業(yè)裝備數(shù)字化設(shè)計、自動導(dǎo)航協(xié)調(diào)控制等關(guān)鍵技術(shù)[1]。農(nóng)業(yè)機(jī)械自動導(dǎo)航是精準(zhǔn)農(nóng)業(yè)的重要技術(shù)支撐，已廣泛應(yīng)用于現(xiàn)代農(nóng)業(yè)生產(chǎn)各過程[2-5]。中國是海參生產(chǎn)和消費大國，目前，海參捕撈方式主要是拖網(wǎng)和人工捕撈，對潛水員的生命健康和海洋生態(tài)都有潛在危害[6]。為減輕潛水員的勞動強(qiáng)度，人們已開發(fā)了一些機(jī)械式捕撈裝置來代替人工進(jìn)行海參捕撈[7-9]，但自動化程度低，作業(yè)效率不高。因此，在當(dāng)前海參養(yǎng)殖規(guī)模不斷擴(kuò)大的背景下，開發(fā)一種自主式海參捕撈裝置顯得尤其迫切。自主式海參捕撈裝置關(guān)鍵要解決海參柔性抓捕和載體的自主導(dǎo)航定位問題。捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS，strapdown inertial navigation system）具有自主性和無源性的特點，已被廣泛用于飛機(jī)、艦艇、船舶等的導(dǎo)航[10-12]，隨著低成本MEMS（micro-electro- mechanical system）慣性傳感器技術(shù)的進(jìn)步，捷聯(lián)慣性導(dǎo)航系統(tǒng)被越來越多地應(yīng)用于民用領(lǐng)域[13]。為解決海參捕撈裝置的自主導(dǎo)航定位問題，課題組開展了以捷聯(lián)慣導(dǎo)為主的組合導(dǎo)航研究。

初始對準(zhǔn)是捷聯(lián)慣導(dǎo)系統(tǒng)關(guān)鍵技術(shù)之一，其目的就是在執(zhí)行導(dǎo)航任務(wù)之前，事先確定出載體坐標(biāo)系相對導(dǎo)航坐標(biāo)系的初始位置關(guān)系，即確定姿態(tài)矩陣的初始值，為后續(xù)捷聯(lián)解算提供必要的初始條件[14-15]，因此，初始對準(zhǔn)的好壞將直接影響導(dǎo)航系統(tǒng)的性能。為了既快又準(zhǔn)地實現(xiàn)對準(zhǔn)，捷聯(lián)慣導(dǎo)系統(tǒng)的初始對準(zhǔn)過程通常分為粗對準(zhǔn)和精對準(zhǔn)兩個階段[16-17]。粗對準(zhǔn)著重解決對準(zhǔn)的快速性問題，盡快將姿態(tài)角誤差縮小到一定的范圍內(nèi)，為下一步精對準(zhǔn)提供有效的初始條件，因此，研究一種算法簡單且具有較高精度的粗對準(zhǔn)方案有其實際的應(yīng)用價值[18-19]。傳統(tǒng)解析粗對準(zhǔn)是直接利用重力矢量和地球自轉(zhuǎn)角速度矢量估算姿態(tài)矩陣[20]，由于其水平對準(zhǔn)精度同時受陀螺儀和加速度計測量誤差的影響，使得粗對準(zhǔn)效果不佳。為改善水平對準(zhǔn)精度，本文提出一種雙矢量定姿粗對準(zhǔn)方案，并用MEMS慣性測量單元與三維電子羅盤的實測數(shù)據(jù)進(jìn)行了試驗驗證。

1 材料與方法

1.1 坐標(biāo)系規(guī)定

1）導(dǎo)航坐標(biāo)系文中選地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，其原點位于載體重心，軸指向東，軸指向北，軸指向天。

1.2 解析粗對準(zhǔn)算法

1.2.1 傳統(tǒng)解析粗對準(zhǔn)

傳統(tǒng)解析粗對準(zhǔn)是借助空間兩個“不共線”的地球自轉(zhuǎn)角速度矢量和地球重力矢量，直接估算從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的初始姿態(tài)矩陣[21-22]。重力矢量、地球自轉(zhuǎn)角速度矢量在導(dǎo)航坐標(biāo)系和載體坐標(biāo)系上的投影關(guān)系可分別表示為：

（2）

為了求解從系到系的變換矩陣，再利用和的叉乘運(yùn)算構(gòu)造一個新的矢量：

（4）

聯(lián)立（1）、（2）、（4）式，寫成矩陣形式：

（6）

（8）

受慣性傳感器本身測量偏差和外界干擾的影響，使得按（6）式求解的姿態(tài)矩陣不能嚴(yán)格滿足正交化要求[23]，可以按下式進(jìn)行正交化處理：

1.2.2 雙矢量定姿粗對準(zhǔn)

1）雙矢量定姿原理

雙矢量定姿是利用兩個不共線的矢量在直角坐標(biāo)系系和系下的投影坐標(biāo)，從而確定兩坐標(biāo)系之間的方位關(guān)系[24-25]。不妨假設(shè)兩個不共線的矢量為和，它們在系和系下的投影坐標(biāo)依次記為、、、，則系和系間方位關(guān)系可用方向余弦矩陣（姿態(tài)矩陣）來描述，通過引入輔助矢量并利用矢量坐標(biāo)變換關(guān)系，不難得到：

針對（10）式的一種改進(jìn)思路是預(yù)先對參與解算的所有矢量作單位正交化處理。圖1給出了由測量矢量和構(gòu)造3個單位正交矢量、和的空間方位示意圖。圖1中，被選為主矢量，選擇主矢量的一般原則是選擇兩個矢量中的測量誤差較小者。

2）雙矢量定姿粗對準(zhǔn)

（13）

（15）

于是式（12）、（13）分別改寫為：

（17）

進(jìn)一步寫成：

（19）

（21）

考慮到加速度計的測量誤差一般小于陀螺儀的測量誤差，文中選擇作為主參考矢量，參照式（11）可得姿態(tài)矩陣估計算法表達(dá)式：

（22）

將式（7）、（8）代入式（22），經(jīng)化簡后得到：

實際對準(zhǔn)過程中，為降低慣性傳感器高頻噪聲的影響，可以采集一段時間內(nèi)的傳感器數(shù)據(jù)求平均，然后用該時間段內(nèi)的平均角速度及平均加速度分別代替式（23）中的和，從而估算出初始姿態(tài)矩陣。

估算出姿態(tài)矩陣后，不難解算出載體的姿態(tài)角。假設(shè)海參捕撈裝置的俯仰角、橫滾角和航向角分別用（°）、（°）和（°）表示，則姿態(tài)矩陣可寫成[26]：

（24）

由式（24），得：

1.3 誤差分析

從理論上分析不同對準(zhǔn)方法的誤差特性，對于工程實踐中選擇更好的對準(zhǔn)方案具有指導(dǎo)意義。傳統(tǒng)解析粗對準(zhǔn)中，正交化后的姿態(tài)矩陣估計值消除了刻度系數(shù)誤差和歪斜誤差，但仍然含有漂移誤差，文獻(xiàn)[27]導(dǎo)出的漂移誤差失準(zhǔn)角的估算式為：

（27）

（28）

將式（29）按向量叉乘運(yùn)算規(guī)則展開，忽略二階及以上小量，并記慣性傳感器的測量誤差和，得：

為簡化書寫，式（30）中與誤差分析結(jié)果不相關(guān)的元素用“*”表示。

（32）

即有：

可見，雙矢量定姿粗對準(zhǔn)的水平失準(zhǔn)角只受加速度計水平測量誤差、的影響，而傳統(tǒng)解析粗對準(zhǔn)的水平失準(zhǔn)角除跟加速度計測量誤差有關(guān)外，還和天向陀螺漂移誤差有關(guān)，一般情況下，由于陀螺測量誤差遠(yuǎn)大于加速度計測量誤差，因此，雙矢量定姿粗對準(zhǔn)算法提高了水平對準(zhǔn)精度。對比式（28）、（33）可知，兩者方位對準(zhǔn)精度相當(dāng)，且方位失準(zhǔn)角主要取決于陀螺的東向漂移誤差。

2 結(jié)果與分析

為驗證上述2種粗對準(zhǔn)算法的優(yōu)劣，試驗中使用瑞芬科技有限公司的AH106B型MEMS慣性測量單元和DCM260型三維電子羅盤的實測值進(jìn)行對比試驗，實物如圖2所示。試驗條件：當(dāng)?shù)鼐暥?40.004 9°；所用慣性測量單元的陀螺儀常值漂移20°/h，陀螺儀隨機(jī)游走系數(shù)，加速度計零偏，陀螺儀和加速度計的最大數(shù)據(jù)輸出頻率300 Hz，試驗中陀螺儀和加速度計的數(shù)據(jù)采集周期均設(shè)置為20 ms；三維電子羅盤的傾角分辨率0.1°，數(shù)據(jù)輸出頻率20 Hz。海參捕撈裝置主要由導(dǎo)航模塊、推進(jìn)器模塊、水下攝像機(jī)、捕撈機(jī)構(gòu)等構(gòu)成；其中，導(dǎo)航模塊安裝于海參捕撈裝置載體中部的封閉式平臺上。試驗地點選擇山東海陽某海參養(yǎng)殖場水域，與本試驗相關(guān)場景圖如圖3所示。數(shù)據(jù)采集試驗步驟如下：1）將慣性測量單元、三維電子羅盤固定于減振平臺上，并保證兩模塊的向軸線分別平行，然后將減振平臺靜置于海參捕撈裝置的載體上；2）同時采集并存儲慣性測量單元中陀螺儀和加速度計輸出的原始數(shù)據(jù)以及三維電子羅盤輸出的姿態(tài)角數(shù)據(jù)；3）保持試驗系統(tǒng)靜止30 min后停止數(shù)據(jù)采集與記錄。

選取慣性測量單元采集的同一原始試驗數(shù)據(jù)，在Matlab平臺上，應(yīng)用前述的兩種粗對準(zhǔn)算法分別進(jìn)行粗對準(zhǔn)解算，每次對準(zhǔn)時間為1 min，共進(jìn)行了30次對準(zhǔn)試驗。圖4為傳統(tǒng)解析粗對準(zhǔn)算法得到的捷聯(lián)慣導(dǎo)系統(tǒng)俯仰角、橫滾角和航向角的估算值，而雙矢量定姿粗對準(zhǔn)算法得到的俯仰角、橫滾角和航向角的估算值如圖5所示。圖4、5的俯仰角和橫滾角代表載體的水平姿態(tài)，而航向角代表載體的方位姿態(tài)。圖4的水平姿態(tài)角變化幅度大且呈現(xiàn)出明顯的振蕩特性，而圖5的水平姿態(tài)角變化幅度小且變化平穩(wěn)；圖4和5的方位姿態(tài)角變化規(guī)律一致。

圖4 傳統(tǒng)解析粗對準(zhǔn)算法估算的姿態(tài)角

圖5 雙矢量定姿粗對準(zhǔn)算法估算的姿態(tài)角

為進(jìn)一步驗證粗對準(zhǔn)算法效果，文中使用三維電子羅盤實測值作為算法評估的參考基準(zhǔn)，試驗中所用三維電子羅盤的俯仰和橫滾精度為0.1°、航向精度為0.5°。為使數(shù)據(jù)比較有意義，安裝時，該電子羅盤的3個軸向與慣性測量單元模塊的對應(yīng)軸向平行且一致。在進(jìn)行靜態(tài)數(shù)據(jù)采集的30 min內(nèi)，三維電子羅盤輸出的俯仰角和橫滾角的數(shù)值穩(wěn)定，分別為–0.8°和–0.2°，而航向角做低頻小幅度變化，取其平均值為143.5°，將這3個實測值在圖4、5中標(biāo)示成水平直線，以便與30次粗對準(zhǔn)試驗的估算值作對比。將粗對準(zhǔn)算法所得的姿態(tài)估算值減去對應(yīng)時刻三維電子羅盤實測的姿態(tài)值得到姿態(tài)誤差。表1給出30次粗對準(zhǔn)試驗中2種粗對準(zhǔn)算法對應(yīng)姿態(tài)誤差的試驗結(jié)果，其中“算法1”代表傳統(tǒng)解析粗對準(zhǔn)算法，“算法2”代表雙矢量定姿粗對準(zhǔn)算法，由于2種算法的方位對準(zhǔn)的精度相當(dāng)，表1中合并列出了航向角誤差?？紤]到電子羅盤提供的參考基準(zhǔn)精度僅為0.1°，表1中數(shù)值只保留一位有效數(shù)字。

表1 粗對準(zhǔn)姿態(tài)估算值與電子羅盤姿態(tài)測量值間的誤差

從表1所列粗對準(zhǔn)結(jié)果可見，傳統(tǒng)解析粗對準(zhǔn)算法的俯仰角誤差和橫滾角誤差偏大且變化比較劇烈，而雙矢量定姿粗對準(zhǔn)算法的水平誤差角的大小不超過1°，方位誤差角的大小不超過3°，方位誤差大于水平誤差，與前述誤差特性的理論分析結(jié)果相吻合。

3 結(jié) 論

1）所提出的雙矢量定姿粗對準(zhǔn)算法預(yù)先對參與姿態(tài)解算的矢量作單位正交化處理，使得姿態(tài)矩陣估計值自然滿足正交化條件，有效降低了慣性傳感器測量誤差和外界干擾對粗對準(zhǔn)精度的影響。

2）理論分析和試驗結(jié)果都表明，所述算法提高了海參捕撈裝置捷聯(lián)慣導(dǎo)系統(tǒng)的水平對準(zhǔn)精度。以三維電子羅盤實測值作為基準(zhǔn)，所述粗對準(zhǔn)算法的水平誤差角小于1°，方位誤差角小于3°，在粗對準(zhǔn)所要求的誤差范圍之內(nèi)，為后續(xù)利用濾波方法進(jìn)行精對準(zhǔn)提供了有效的初始條件。

[1] 羅錫文，廖娟，鄒湘軍，等. 信息技術(shù)提升農(nóng)業(yè)機(jī)械化水平[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(20)：1－14. Luo Xiwen, Liao Juan, Zou Xiangjun, et al. Enhancing agricultural mechanization level through information technology[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(20): 1－14. (in Chinese with English abstract)

[2] 胡靜濤，高雷，白曉平，等. 農(nóng)業(yè)機(jī)械自動導(dǎo)航技術(shù)研究進(jìn)展[J]. 農(nóng)業(yè)工程學(xué)報，2015，31(10)：1－10. Hu Jingtao, Gao Lei, Bai Xiaoping, et al. Review of research on automatic guidance of agricultural vehicles[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(10): 1－10. (in Chinese with English abstract)

[3] 陳艷，張漫，馬文強(qiáng)，等. 基于GPS和機(jī)器視覺的組合導(dǎo)航定位方法[J]. 農(nóng)業(yè)工程學(xué)報，2011，27(3)：126－130. Chen Yan, Zhang Man, Ma Wenqiang, et al. Positioning method of integrated navigation based on GPS and machine vision[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2011, 27(3): 126－130. (in Chinese with English abstract)

[4] 趙德安，羅吉，孫月平，等. 河蟹養(yǎng)殖自動作業(yè)船導(dǎo)航控制系統(tǒng)設(shè)計與測試[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(11)：181－188. Zhao Dean, Luo Ji, Sun Yueping, et al. Design and experiment of navigation control system of automatic operation boat in river crab culture[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(11): 181－188. (in Chinese with English abstract)

[5] 洪劍青，趙德安，孫月平，等. 水產(chǎn)養(yǎng)殖自動導(dǎo)航無人明輪船航向的多模自適應(yīng)控制[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(1)：95－101. Hong Jianqing, Zhao Dean, Sun Yueping, et al. Multi model adaptive control of paddlewheel vehicle’s course in aquaculture[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(1): 95－101. (in Chinese with English abstract)

[6] 楊衛(wèi)中，徐銀麗，喬曦，等. 基于對比度受限直方圖均衡化的水下海參圖像增強(qiáng)方法[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(6)：197－203. Yang Weizhong, Xu Yinli, Qiao Xi, et al. Method for image intensification of underwater sea cucumber based on contrast-limited adaptive histogram equalization[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(6): 197－203. (in Chinese with English abstract)

[7] Guo Haizhi, Yu Fusheng, Zhao Yang, et al. Structure design of sea cucumber catching robot based on TRIZ theory[J]. Mechanical Engineering and Technology, 2016, 5(3): 227－237.

[8] 謝鑫建，王麗. 水上操控管吸式海參捕撈裝置：2013106776810[P]. 2015-11-11. Xie Xinjian, Wang Li. Tube suction sea cucumber fishing device on water operation: 2013106776810[P]. 2015-11-11.

[9] 馬秀芬. 吸入式海參捕撈器及捕撈方法：2014101574484[P]. 2015-10-28. Ma Xiufen. Suction sea cucumber fishing device and fishing method: 2014101574484[P]. 2015-10-28.

[10] 岳鵬，史震，王劍，等. 基于MEMS加速度計的無陀螺慣導(dǎo)系統(tǒng)[J]. 中國慣性技術(shù)學(xué)報，2011，19(2)：152－156. Yue Peng, Shi Zhen, Wang Jian, et al. Gyro free inertial navigation system based on MEMS accelerometer[J]. Journal of Chinese Inertial Technology, 2011, 19(2): 152－156. (in Chinese with English abstract)

[11] Zhang Q Z, Meng X L, Zhang S B, et al. Singular value decomposition-based robust cubature Kalman filtering for an integrated GPS/SINS navigation system[J]. Journal of Navigation, 2015, 68(3): 549－562.

[12] 劉軍，袁俊，蔡駿宇，等. 基于GPS/INS和線控轉(zhuǎn)向的農(nóng)業(yè)機(jī)械自動駕駛系統(tǒng)[J]. 農(nóng)業(yè)工程學(xué)報，2016，32(1)：46－53. Liu Jun, Yuan Jun, Cai Junyu, et al. Autopilot system of agricultural vehicles based on GPS/INS and steer-by-wire[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2016, 32(1): 46－53. (in Chinese with English abstract)

[13] 李博文，姚丹亞. 低成本車載MEMS慣導(dǎo)導(dǎo)航定位方法[J].中國慣性技術(shù)學(xué)報，2014，22(6)：719－723. Li Bowen, Yao Danya. Low-cost MEMS IMU navigation positioning method for land vehicle[J]. Journal of Chinese Inertial Technology, 2014, 22(6): 719－723. (in Chinese with English abstract)

[14] 嚴(yán)恭敏，翁浚，白亮，等. 基于慣性參考系的動基座初始對準(zhǔn)與定位導(dǎo)航[J]. 系統(tǒng)工程與電子技術(shù)，2011，33(3)：618－621. Yan Gongmin, Weng Jun, Bai Liang, et al. Initial in-movement alignment and position determination based on inertial reference frame[J]. Systems Engineering and Electronics, 2011, 33(3): 618－621. (in Chinese with English abstract)

[15] Du Tao, Guo Lei, Yang Jian. A fast initial alignment for SINS based on disturbance observer and Kalman filter[J]. Transactions of the Institute of Measurement and Control, 2016, 38(10): 1261－1269.

[16] 趙長山，秦永元，白亮. 基于雙矢量定姿的搖擺基座粗對準(zhǔn)算法分析與實驗[J]. 中國慣性技術(shù)學(xué)報，2009，17(4)：436－440. Zhao Changshan, Qin Yongyuan, Bai Liang. Analysis and test of swaying-based analytic coarse alignment[J]. Journal of Chinese Inertial Technology, 2009, 17(4): 436－440. (in Chinese with English abstract)

[17] 王躍鋼，蔚躍，雷堰龍，等. 捷聯(lián)慣導(dǎo)一種優(yōu)化的慣性系粗對準(zhǔn)方法[J]. 科學(xué)技術(shù)與工程，2013，13(4)：1102－1105. Wang Yuegang, Yu Yue, Lei Yanlong, et al. An improved method of inertial coordination coarse alignment in SINS[J]. Science Technology and Engineering, 2013, 13(4): 1102－1105. (in Chinese with English abstract)

[18] 陳令剛，劉建業(yè)，孫永榮，等. 微小型捷聯(lián)慣導(dǎo)系統(tǒng)解析式對準(zhǔn)方法研究[J]. 航天控制，2005，23(4)：9－12. Chen Linggang, Liu Jianye, Sun Yongrong, et al. Study of analytic coarse alignment methods to micro SINS [J]. Aerospace Control, 2005, 23(4): 9－12. (in Chinese with English abstract)

[19] Weng J, Qin Y Y, Yan G M, et al. Analysis method on error distributions of in-flight alignment schemes [J]. Journal of Chinese Inertial Technology, 2015, 23(5): 570－574.

[20] 于飛，翟國富，高偉，等. 艦船捷聯(lián)慣導(dǎo)系統(tǒng)粗對準(zhǔn)方法研究[J]. 傳感器與微系統(tǒng)，2009，28(5)：15－18. Yu Fei, Zhai Guofu, Gao Wei, et al. Research on coarse alignment for vessel strapdown inertial navigation system[J]. Transducer and Microsystem Technologies, 2009, 28(5): 15－18. (in Chinese with English abstract)

[21] 徐博，孫楓，高偉. 艦船捷聯(lián)航姿系統(tǒng)自主粗對準(zhǔn)仿真與實驗研究[J]. 兵工學(xué)報，2008，29(12)：1467－1473. Xu Bo, Sun Feng, Gao Wei. The coarse alignment of strap-down inertial navigation system for ship[J]. Acta Armamentarii, 2008, 29(12): 1467－1473. (in Chinese with English abstract)

[22] Xiong J, Guo H, Yang Z H. A two-position SINS initial alignment method based on gyro information[J]. Advances in Space Research, 2014, 53(11): 1657－1663.

[23] Liu M, Gao Y B, Li G C, et al. An improved alignment method for the strapdown inertial navigation system (SINS) [J]. Sensors, 2016, 16(5): 12－17.

[24] 嚴(yán)恭敏，秦永元，衛(wèi)育新，等. 一種適用于SINS動基座初始對準(zhǔn)的新算法[J]. 系統(tǒng)工程與電子技術(shù)，2009，31(3)：634－637. Yan Gongmin, Qin Yongyuan, Wei Yuxin, et al. New initial alignment algorithm for SINS on moving base[J]. Systems Engineering and Electronics, 2009, 31(3): 634－637. (in Chinese with English abstract)

[25] Tan Caiming, Zhu Xinhua, Su Yan, et al. A new analytic alignment method for a SINS[J]. Sensors, 2015, 15(11): 27930－27953.

[26] Chang L B, Li J S, Chen S Y. Initial alignment by attitude estimation for strapdown inertial navigation systems[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(3): 784－794.

[27] Yeon Fuh Jiang. Error analysis of analytic coarse alignment methods[J]. IEEE Transactions on Aerospace and Electronic Systems, 1998, 34(1): 334－337.

[28] Li Q, Ben Y Y, Yang J L. Coarse alignment for fiber optic gyro SINS with external velocity aid[J]. OPTIK, 2014, 125(16): 4241－4245.

[29] Silva F O, Hemerly E M, Filho W C L. Error analysis of analytical coarse alignment formulations for stationary SINS [J]. IEEE Transactions on Aerospace and Electronic Systems, 2016, 52(4): 1777－1796.

[30] 薛海建，王解，郭曉松，等. SINS非線性自對準(zhǔn)中的強(qiáng)跟蹤UKF算法設(shè)計[J]. 上海交通大學(xué)學(xué)報，2015，49(9)：1429－1434. Xue Haijian, Wang Jie, Guo Xiaosong, et al. Design of a strong tracing UKF for nonlinear self-alignment of SINS[J]. Journal of Shanghai Jiao Tong University, 2015, 49(9): 1429－1434. (in Chinese with English abstract)

Double-vector attitude determination algorithm improving coarse alignment accuracy of strapdown inertial navigation system for sea cucumber fishing device

Bao Jianhua1,2,3, Qiao Xi1,3, Li Daoliang1,3※

(1.100083,; 2.221116,; 3.100083,)

Autonomous navigation is one of the key technologies for an intelligent agricultural equipment, which has been widely used in modern agricultural production. Sea cucumber production and consumption are very large in China. At present, the sea cucumber harvest mainly relies on trawl or artificial fishing, which will cause a potential hazard to the health of divers and the marine ecology. Therefore, in order to overcome the shortcomings of traditional sea cucumber fishing methods, it is necessary to develop a sea cucumber fishing device with autonomous navigation and positioning functions. Strapdown inertial navigation system (SINS) is suitable for the navigation of the autonomous sea cucumber fishing device because of its autonomy, passivity and complete navigation parameters. For the SINS, initial alignment must be completed before starting a navigation mission. The purpose of the initial alignment is to determine the initial position of the carrier coordinate system relative to the navigation coordinate system, that is, to determine the initial value of the attitude matrix. Initial alignment is one of the key technologies of SINS, which is divided into coarse alignment and fine alignment, and the coarse alignment accuracy will directly determine the performance of the initial alignment. Therefore, it is of practical value to study a coarse alignment scheme with simple algorithm and high alignment precision. The traditional analytical coarse alignment algorithm directly utilizes the earth gravity vector and the earth rotation angular velocity vector to estimate the initial attitude matrix. In view of the problem that the horizontal alignment accuracy of SINS is affected by the measurement errors of gyroscope and accelerometer in the traditional analytical coarse alignment algorithm, a novel coarse alignment algorithm based on double-vector attitude determination is proposed. In general, the measurement error of the accelerometer is much smaller than that of the gyroscope, so the earth gravity vector in the proposed algorithm is chosen as the main reference vector. Then, 3 unit orthogonal vectors are constructed based on the earth gravity vector and the earth rotation angular velocity vector, and the resulting attitude matrix is a unit orthogonal matrix. The theoretical analysis shows that horizontal misalignment angles of SINS are only related to the accelerometer level measurement errors in the case of using the proposed algorithm, however, using the conventional algorithm, the horizontal misalignment angles are related to the accelerometer measurement errors and the gyroscope drift error. Therefore, the coarse alignment accuracy of SINS using the aforementioned algorithm is significantly improved.Based on the same measured data from an inertial measurement unit, the simulation experiment was carried out for 30 times using the conventional coarse alignment algorithm and the proposed algorithm, respectively. Simulation curves demonstrated that the variation of the horizontal attitude angles using the algorithm proposed was smoother. To further verify the effectiveness of the algorithm, the attitude angles calculated from the measured values of the inertial measurement unit were compared with the measured value of a high-precision three-dimensional electronic compass, and the experimental results showed that the horizontal error angles of the aforementioned coarse alignment algorithm did not exceed 1° and the azimuth error angle did not exceed 3°. The results can meet the accuracy requirement of coarse alignment and will provide an effective initial condition for the subsequent fine alignment using filtering methods.

fisheries; navigation; algorithms; strapdown inertial navigation; coarse alignment; double-vector attitude determination; stationary base; sea cucumber fishing

10.11975/j.issn.1002-6819.2017.12.037

TN966; TP391.9

A

1002-6819(2017)-12-0286-07

2016-12-29

2017-03-27

國家國際科技合作專項項目（2013DFA11320）；中央高校基本科研業(yè)務(wù)費專項資金資助項目（2016XD001）

包建華，男，安徽肥東人，副教授，博士生，主要從事慣性導(dǎo)航及組合導(dǎo)航研究。北京 中國農(nóng)業(yè)大學(xué)信息與電氣工程學(xué)院，100083。 Email：baojhxz@cau.edu.cn

李道亮，男，山東墾利人，教授，博士生導(dǎo)師，主要從事農(nóng)業(yè)信息先進(jìn)感知與智能處理研究。北京 中國農(nóng)業(yè)大學(xué)信息與電氣工程學(xué)院，100083。Email：dliangl@cau.edu.cn

包建華，喬 曦，李道亮. 雙矢量定姿算法提高海參捕撈裝置捷聯(lián)慣導(dǎo)系統(tǒng)粗對準(zhǔn)精度[J]. 農(nóng)業(yè)工程學(xué)報，2017，33(12)：286－292. doi：10.11975/j.issn.1002-6819.2017.12.037 http://www.tcsae.org

Bao Jianhua，Qiao Xi，Li Daoliang. Double-vector attitude determination algorithm improving coarse alignment accuracy of strapdown inertial navigation system for sea cucumber fishing device[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(12): 286－292. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2017.12.037 http://www.tcsae.org