水下采捕裝置運動姿態(tài)控制方法研究

蔡衛(wèi)國，武立波，李明智，魏鴻磊

（1.大連海洋大學 機械與動力工程學院，大連 116023；2.大連海洋大學 航海與船舶工程學院，大連 116023）

0 引言

隨著近海養(yǎng)殖業(yè)的蓬勃發(fā)展，近海捕撈所面臨的問題日益突出。近海養(yǎng)殖的海珍品主要有扇貝、海參、鮑魚等，傳統(tǒng)的近海捕撈方式以人工下潛作業(yè)為主，輔以拖網(wǎng)和定置網(wǎng)，不但勞動強度大、效率低、成本高，而且對海底的生態(tài)環(huán)境造成了嚴重破壞。近幾年，我國的水下機器人研究發(fā)展迅猛，如我國自行研制的“蛟龍?zhí)枴陛d人潛水器，可以實施深?？碧健⒑５鬃鳂I(yè)等任務(wù)[1]。水下機器人技術(shù)的迅速發(fā)展為海珍品的生態(tài)采捕提供了技術(shù)支持和可能，復(fù)雜的水下環(huán)境對機器人性能提出了更高的要求，防傾覆控制是其完成特殊任務(wù)的前提和保障，是水下機器人關(guān)鍵技術(shù)之一[2～4]。本文首先確定水下采捕裝置的運動姿態(tài)控制系統(tǒng)方案，通過單片機對采捕裝置上三軸陀螺儀和三軸加速度計傳感器的姿態(tài)數(shù)據(jù)進行采樣，經(jīng)過卡爾曼濾波處理、PID調(diào)節(jié)控制電機驅(qū)動器實現(xiàn)對螺旋槳電機的速度控制，從而實現(xiàn)了采捕裝置的平衡控制[5～7]。

1 總體方案設(shè)計

水下采捕裝置的試驗?zāi)Ｐ腿鐖D1所示，其運動由四個螺旋槳控制，可以實現(xiàn)裝置的上浮和下潛。 三軸加速度計和三軸陀螺儀與水下采捕裝置的三軸重合并固定在水下采捕裝置內(nèi)部。 當水下采捕裝置的姿態(tài)發(fā)生變化時，加速度計能夠讀出重力加速度在X,Y,Z三軸的分量，通過計算可得到每一軸與原始坐標系的夾角，同時可以通過陀螺儀讀出各軸的角加速度，經(jīng)過卡爾曼濾波融合就可以得到精確的水下采捕裝置的姿態(tài)。再將姿態(tài)角度作為變量通過PID編程控制，將角度的變化量轉(zhuǎn)化為控制水下采捕裝置平衡電機的轉(zhuǎn)速，從而糾正它在水中的姿態(tài)。通過加速度陀螺儀實時采集數(shù)據(jù)不斷改變電機轉(zhuǎn)速，這樣就實現(xiàn)了水下采捕裝置的平衡控制。

圖1 試驗?zāi)Ｐ?/p>

2 控制系統(tǒng)方案

控制系統(tǒng)總體方案如圖2所示。

圖2 控制系統(tǒng)方案

2.1 硬件結(jié)構(gòu)

水下采捕裝置運動姿態(tài)控制主要以單片機ATmega328P為控制核心，主要采用了Arduino Pro Nimi微處理模塊、三軸加速度和三軸陀螺儀MPU6050模塊、L298電機驅(qū)動器模塊和Arduino IDE編程軟件。主要輔助功能包括自檢、校準、低電壓報警等，硬件電路接線如圖3所示。

圖3 硬件電路接線圖

2.2 控制電路設(shè)計

水下采捕裝置運動姿態(tài)控制電路采用模塊化設(shè)計，主要包括Arduino Pro Nimi微處理模塊、三軸加速度+三軸陀螺儀MPU6050模塊、L298電機驅(qū)動器模塊和Arduino IDE編程軟件。

2.2.1 Arduino Pro Mini微處理器模塊

水下采捕裝置通過以單片機ATmega328P為控制核心的Arduino Pro Nimi模塊來實現(xiàn)平衡控制功能。因為它的尺寸小、重量輕、價格便宜，適合安裝在空間較小的環(huán)境，唯一的缺點是它缺少用于編程的USB接口，需要一塊TTL/USB適配器模塊來進行編程。ATmega328單片機可以提供32k的儲存空間、2k RAM和1k的EEPROM。

2.2.2 三軸加速度+三軸陀螺儀MPU6050模塊

MPU-6050模塊作為整合性六軸運動處理組件，與多組件方案相比消除了組合陀螺儀與加速度計的軸間差問題，以單一數(shù)據(jù)流的形式向應(yīng)用端輸出完整的9軸融合的運動處理資料庫，可對運動感測的復(fù)雜數(shù)據(jù)進行處理，從而降低了運動處理運算對操作系統(tǒng)的負荷。連接電路如圖4所示。

2.2.3 L298N直流電機/步進電機驅(qū)動模塊

L298N是ST公司生產(chǎn)的一種高電壓、大電流電機驅(qū)動芯片。該芯片采用了15腳封裝，它的主要特點是：工作電壓高、輸出電流大，內(nèi)部含有兩個H橋的高電壓大電流全橋式驅(qū)動器，用來驅(qū)動直流電動機或步進電動機。L298N芯片驅(qū)動電機可以驅(qū)動一臺兩相步進電機或四相步進電機或兩臺直流電機。連接電路如圖5所示。

3 實驗結(jié)果

本次實驗主要測試卡爾曼濾波效果，測試對象為加速度計、陀螺儀采集角度曲線，實驗過程以水上測試為主，通過串行口監(jiān)視窗口數(shù)據(jù)，對數(shù)據(jù)稍作調(diào)試即可進行水下調(diào)試。測試指標為：

1）通過串行口輸出的角度無漂移且變化平滑；

2）通過串行口輸出的電機轉(zhuǎn)速控制量與角度的關(guān)系滿足預(yù)期PID控制輸出量與角度關(guān)系，不斷調(diào)整Kp、Kd、Ki參數(shù)是電機轉(zhuǎn)速與角度的變化與所處的環(huán)境相適應(yīng)。

圖4 MPU6050模塊連接電路

圖5 L298N驅(qū)動模塊連接電路

加速度傳感器要矯正偏移，先讀取若干數(shù)值，然后將數(shù)值取平均值，該平均值就是傳感器當前狀態(tài)下的偏移，在后面的角度積分中要先用原始加速度值減掉對應(yīng)的偏移。加速度計經(jīng)過卡爾曼濾波后采集的角度曲線變得平滑，如圖6所示。

圖6 加速度計濾除偏移量后的角度曲線

如圖7所示，經(jīng)過卡爾曼濾波后的陀螺儀三軸角度變化曲線，漂移消除且曲線變化比較平滑。

圖7 陀螺儀+卡爾曼濾波后采集角度曲線

4 結(jié)論

針對水下采捕裝置的運動穩(wěn)定性問題，通過單片機對采捕裝置上陀螺儀和加速度計傳感器的姿態(tài)數(shù)據(jù)進行采樣，經(jīng)過卡爾曼濾波處理、PID調(diào)節(jié)控制電機驅(qū)動器，從而對螺旋槳電機的速度控制，實現(xiàn)了采捕裝置的平衡控制。實驗結(jié)果初步實現(xiàn)了傾斜超過設(shè)定的平衡角度范圍時，四個螺旋槳轉(zhuǎn)動產(chǎn)生扶正力矩，而且隨著傾斜角度的增加轉(zhuǎn)速也會增加，從而增大扶正力矩。尚需解決的問題是：所選電機啟動轉(zhuǎn)矩小，導致在傾斜時電機不能同一時間啟動，且它們的轉(zhuǎn)速也與程序控制量有偏差，電機轉(zhuǎn)速的代數(shù)和不為零會使其產(chǎn)生旋轉(zhuǎn)力矩。

[1] 徐玉如,李彭超,周俊.水下機器人發(fā)展趨勢[J].自然雜志,2011,33(3):125-131.

[2] 蔣新松,封錫盛,王棣棠.水下機器人[M].沈陽:遼寧科學技術(shù)出版社,2002.

[3] 徐玉如,龐永杰,甘永,孫玉山.智能水下機器人技術(shù)展望[J].智能系統(tǒng)學報,2006,1(1):9-16.

[4] 朱玉華,袁瑞琴.基于嵌入式系統(tǒng)的水下機器人航向控制[J].自動化應(yīng)用,2013,(9):8-10,39.

[5] 魏延輝,彭富國,盛超,周衛(wèi)祥.自主式水下機器人運動穩(wěn)定性控制方法[J].華中科技大學學報（自然科學版）,2014,42(2):127-132.

[6] Weiguo Cai,Zhenguo Shang,Libo Wu. Optimal Design of a Four-legged Walking Mechanism Based on Bionics[A].Applied Mechanics and Materials Vols[C].496-500(2014) pp719-723.

[7] 程晗,王內(nèi),壽曉波.基于姿態(tài)角遞增算法的6軸機器人通用運動規(guī)劃方法[J].制造業(yè)自動化,2015,37(5):72-74.