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

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

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

0 引言

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

1 總體方案設(shè)計(jì)

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

圖1 試驗(yàn)?zāi)Ｐ?/p>

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

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

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

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

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

圖3 硬件電路接線圖

2.2 控制電路設(shè)計(jì)

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

2.2.1 Arduino Pro Mini微處理器模塊

水下采捕裝置通過以單片機(jī)ATmega328P為控制核心的Arduino Pro Nimi模塊來實(shí)現(xiàn)平衡控制功能。因?yàn)樗某叽缧　⒅亓枯p、價格便宜，適合安裝在空間較小的環(huán)境，唯一的缺點(diǎn)是它缺少用于編程的USB接口，需要一塊TTL/USB適配器模塊來進(jìn)行編程。ATmega328單片機(jī)可以提供32k的儲存空間、2k RAM和1k的EEPROM。

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

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

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

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

3 實(shí)驗(yàn)結(jié)果

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

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

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

圖4 MPU6050模塊連接電路

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

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

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

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

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

4 結(jié)論

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

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

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

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

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

[5] 魏延輝,彭富國,盛超,周衛(wèi)祥.自主式水下機(jī)器人運(yùn)動穩(wěn)定性控制方法[J].華中科技大學(xué)學(xué)報（自然科學(xué)版）,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軸機(jī)器人通用運(yùn)動規(guī)劃方法[J].制造業(yè)自動化,2015,37(5):72-74.