王 剛, 宋英杰,, 唐武生, 趙 強(qiáng)

(1. 白城師范學(xué)院 機(jī)械與控制工程學(xué)院, 吉林 白城 137000；2. 長(zhǎng)春大學(xué) 機(jī)械與車(chē)輛工程學(xué)院, 長(zhǎng)春 130022)

近年來(lái), 仿生機(jī)器魚(yú)在管道巡護(hù)和水質(zhì)監(jiān)測(cè)等領(lǐng)域應(yīng)用廣泛. 機(jī)器魚(yú)的位姿(位置和方向統(tǒng)稱(chēng)為位姿)控制要求機(jī)器魚(yú)在游動(dòng)過(guò)程中, 能克服各種干擾以到達(dá)指定目標(biāo)點(diǎn). 位姿控制的質(zhì)量直接影響仿生機(jī)器魚(yú)循跡、 避障、 搬運(yùn)和編隊(duì)的完成效果. 劉安全等[1]提出了基于改進(jìn)比例導(dǎo)引的機(jī)器魚(yú)位姿控制算法, 將機(jī)器魚(yú)位姿誤差中的方向角項(xiàng)和縱坐標(biāo)項(xiàng)用于機(jī)器魚(yú)角速度控制律, 橫坐標(biāo)項(xiàng)和縱坐標(biāo)項(xiàng)用于機(jī)器魚(yú)線(xiàn)速度控制律, 從而實(shí)現(xiàn)機(jī)器魚(yú)的位姿控制; 肖權(quán)等[2]提出了基于軌跡生成法的機(jī)器魚(yú)位姿控制算法, 以機(jī)器魚(yú)行為機(jī)制理論為基礎(chǔ), 構(gòu)建起始位姿到目標(biāo)位姿的距離和方向角與視線(xiàn)角差值組成的矢量矩陣, 以切線(xiàn)和圓弧長(zhǎng)度總和最小為最優(yōu)軌跡研究機(jī)器魚(yú)位姿控制; 韓虹飛[3]根據(jù)機(jī)器魚(yú)波動(dòng)推進(jìn)特性, 在目標(biāo)位姿圓切線(xiàn)基礎(chǔ)上添加10 cm假定后退距離, 利用切線(xiàn)圓與目標(biāo)方向信息尋找最佳目標(biāo)位置點(diǎn)并調(diào)整機(jī)器魚(yú)的方向?qū)崿F(xiàn)機(jī)器魚(yú)的位姿控制; 李慶春[4]以目標(biāo)位姿的方向線(xiàn)為滑模曲線(xiàn), 運(yùn)用模糊滑模軌跡控制法, 根據(jù)全局?jǐn)z像機(jī)反饋的機(jī)器魚(yú)位姿信息, 實(shí)時(shí)調(diào)整機(jī)器魚(yú)的運(yùn)動(dòng)方向和速度, 利用實(shí)驗(yàn)平臺(tái)原始的點(diǎn)到點(diǎn)模糊控制算法跟蹤滑模曲線(xiàn)上的一系列虛擬目標(biāo)點(diǎn), 使機(jī)器魚(yú)能夠在滑模曲線(xiàn)上穩(wěn)定運(yùn)動(dòng), 最終到達(dá)目標(biāo)位姿; 潘想[5]將輪式機(jī)器人穩(wěn)定位姿軌跡跟蹤的控制器----滑?？刂扑惴☉?yīng)用到機(jī)器魚(yú)位姿控制實(shí)驗(yàn), 有效解決了其誤差大、 用時(shí)長(zhǎng)的問(wèn)題； Li等[6]提出一種改進(jìn)的比例導(dǎo)引控制算法和自抗擾控制算法相結(jié)合的高精度控制器, 將其用于機(jī)器魚(yú)從任意位姿到目標(biāo)位姿的運(yùn)動(dòng); Yu等[7]利用基于無(wú)模型強(qiáng)化學(xué)習(xí)的控制方法實(shí)現(xiàn)了機(jī)器水母姿態(tài)的自主調(diào)節(jié), 提高了結(jié)構(gòu)靈活性和偏航機(jī)動(dòng)性; Zheng等[8]提出人工側(cè)線(xiàn)系統(tǒng)和慣性測(cè)量單元集成的雙傳感器融合方法, 利用人工神經(jīng)網(wǎng)絡(luò)實(shí)時(shí)感知流量信息, 實(shí)現(xiàn)機(jī)器魚(yú)在來(lái)流中的動(dòng)態(tài)運(yùn)動(dòng); Cafer等[9]用CPG(central pattern generator)和模糊邏輯控制器作為機(jī)器魚(yú)的決策機(jī)制, 提出有限狀態(tài)機(jī)(finite state machine, FSM)算法, 實(shí)現(xiàn)機(jī)器魚(yú)的多場(chǎng)景自主巡航和避障等游動(dòng)性能.

仿生機(jī)器魚(yú)依靠魚(yú)體和魚(yú)鰭等與水相互作用產(chǎn)生流體動(dòng)力, 由于水環(huán)境的干擾和動(dòng)力學(xué)的非線(xiàn)性, 因此仿生機(jī)器魚(yú)位姿控制的精準(zhǔn)度較低. 本文以北京大學(xué)智能仿生實(shí)驗(yàn)室開(kāi)發(fā)的仿鲹科三關(guān)節(jié)機(jī)器魚(yú)為研究對(duì)象, 研究仿生機(jī)器魚(yú)的位姿控制問(wèn)題.

1 三關(guān)節(jié)仿生機(jī)器魚(yú)

三關(guān)節(jié)仿生機(jī)器魚(yú)包括剛性頭部(含靜止橡膠材質(zhì)胸鰭)、 柔性魚(yú)體(3個(gè)舵機(jī)串聯(lián)組成)和具有尾柄的尾鰭3個(gè)部分, 如圖1所示. 機(jī)器魚(yú)的剛性頭部無(wú)動(dòng)力, 可有效抵御池壁的碰撞, 內(nèi)部配重片可保證機(jī)器魚(yú)的魚(yú)體在水中直立. 柔性魚(yú)體由舵機(jī)、 串聯(lián)金屬骨架和橡膠魚(yú)皮組成, 柔性魚(yú)體和尾鰭通過(guò)各舵機(jī)驅(qū)動(dòng)所對(duì)應(yīng)的金屬骨架周期性擺動(dòng)使機(jī)器魚(yú)擬合魚(yú)體波曲線(xiàn)運(yùn)動(dòng), 模仿真魚(yú)借助擊水產(chǎn)生有效的反作用力推動(dòng)機(jī)器魚(yú)前進(jìn).

圖1 仿生機(jī)器魚(yú)Fig.1 Biomimetic robotic fish

仿生機(jī)器魚(yú)采用CPG運(yùn)動(dòng)控制, 通過(guò)在線(xiàn)調(diào)整參數(shù), 實(shí)現(xiàn)其多模態(tài)游動(dòng)之間的平滑過(guò)渡[10-11]. 圖2為機(jī)器魚(yú)的CPG運(yùn)動(dòng)控制框圖[12].

圖2 機(jī)器魚(yú)的CPG運(yùn)動(dòng)控制框圖Fig.2 CPG motion control block diagram of robotic fish

CPG控制器的數(shù)學(xué)描述[12]如下：

(1)

(2)

(3)

θi(t)=xi(t)+ri(t)sin(φi(t)),

(4)

其中i=1,2,3, 運(yùn)動(dòng)控制參數(shù)ri(t),xi(t)和φi(t)分別為第i個(gè)振蕩器的振幅、 偏移和相位,Ri,Xi和ν分別為第i個(gè)振蕩器的期望幅值、 期望偏移和期望頻率,φij為振蕩器i和振蕩器j之間的期望相位差,θi為第i個(gè)振蕩器對(duì)應(yīng)關(guān)節(jié)的輸出角,α′,β′,μ為系統(tǒng)結(jié)構(gòu)參數(shù), 決定系統(tǒng)的收斂速度,N為振蕩器的總數(shù), 對(duì)應(yīng)機(jī)器魚(yú)的3個(gè)擺動(dòng)關(guān)節(jié). CPG模型的輸出為各關(guān)節(jié)對(duì)應(yīng)的周期性變化擺角值, 經(jīng)STM32核心板解析為驅(qū)動(dòng)舵機(jī)的脈沖寬度調(diào)制(pulse width modulation, PWM)波信號(hào). 在線(xiàn)調(diào)節(jié)CPG網(wǎng)絡(luò)模型間的連接權(quán)值參數(shù)以實(shí)現(xiàn)各關(guān)節(jié)之間的協(xié)調(diào)運(yùn)動(dòng), 使機(jī)器魚(yú)自然協(xié)調(diào)游動(dòng).

2 位姿誤差模型

圖3 機(jī)器魚(yú)位姿誤差模型坐標(biāo)系Fig.3 Posture error model coordinate of robotic fish

(5)

圖4 機(jī)器魚(yú)位姿控制模型流程Fig.4 Flow chart of posture control model of robotic fish

3 串級(jí)PID控制算法

位姿控制算法具有一定的局限性, 需假設(shè)機(jī)器魚(yú)在前進(jìn)過(guò)程中無(wú)障礙及目標(biāo)位姿在水環(huán)境的干擾下相對(duì)固定等. 為減少水環(huán)境干擾及時(shí)延對(duì)機(jī)器魚(yú)位姿控制精度的影響, 提高機(jī)器魚(yú)的適應(yīng)能力, 對(duì)其位姿控制采用串級(jí)控制系統(tǒng), 串級(jí)比例-積分-微分(PID)控制系統(tǒng)結(jié)構(gòu)如圖5所示. 串級(jí)PID控制系統(tǒng)由兩個(gè)PID控制器串聯(lián)組成, 將外環(huán)PID控制器的輸出作為內(nèi)環(huán)PID控制器的輸入, 內(nèi)環(huán)PID控制器的輸出作為控制量[16].Gc1(s)和Gc2(s)分別為主調(diào)節(jié)器傳遞函數(shù)和副調(diào)節(jié)器傳遞函數(shù),G1(s)和G2(s)分別為主對(duì)象特性和副對(duì)象特性, 所處外環(huán)和內(nèi)環(huán)分別作為主回路(PID1)和副回路(PID2),D(s)為系統(tǒng)中存在的干擾. 先計(jì)算PID1, 再計(jì)算PID2, 控制參數(shù)的整定順序相反.

圖5 串級(jí)PID控制系統(tǒng)結(jié)構(gòu)Fig.5 Cascade PID control system structure

機(jī)器魚(yú)系統(tǒng)對(duì)基本控制通道的控制存在非線(xiàn)性因素且機(jī)器魚(yú)作業(yè)環(huán)境具有時(shí)變性, 由于全局?jǐn)z像機(jī)采集的機(jī)器魚(yú)信息與實(shí)際狀態(tài)信息存在實(shí)時(shí)性誤差, 因此, 單級(jí)PID控制無(wú)法滿(mǎn)足機(jī)器魚(yú)位姿的控制精度, 采用串級(jí)PID可提高對(duì)機(jī)器魚(yú)的位姿控制, 即將響應(yīng)速度快的線(xiàn)速度控制器和角速度控制器作為內(nèi)環(huán), 響應(yīng)速度慢的距離控制器和方向角角度控制器作為外環(huán)[17]. 基于串級(jí)PID的仿生機(jī)器魚(yú)位姿控制原理如圖6所示.

圖6 基于串級(jí)PID的仿生機(jī)器魚(yú)位姿控制原理Fig.6 Posture control principle of biomimetic robotic fish based on cascade PID

3.1 角速度控制器設(shè)計(jì)

角速度控制器的外環(huán)角度PID控制器和內(nèi)環(huán)角速度PID控制器均采用增量式PID算法, 角速度控制器的外環(huán)角度PID控制器的輸入為實(shí)際方向角與期望方向角的角度誤差和視線(xiàn)角之和, 外環(huán)角度PID控制器的輸出為

其中:eθ(k)=θr-θc(k)+θs(k),θr-θc(k)為機(jī)器魚(yú)期望方向角與實(shí)際方向角之間的差值,θs(k)=arctan[(yr-yc(k))/(xr-xc(k))]為機(jī)器魚(yú)的視線(xiàn)角；kpo1,kio1和kdo1分別為角速度控制器的外環(huán)角度PID控制器的比例、 積分和微分系數(shù)；k為采樣時(shí)刻.

以外環(huán)角度PID控制器的輸出為內(nèi)環(huán)角速度PID控制器的輸入, 內(nèi)環(huán)角速度PID控制器的輸出為

其中:ew(k)=wr(k)-wc(k),wc為多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)的全局?jǐn)z像機(jī)采集的機(jī)器魚(yú)角速度信息;kpi1,kii1和kdi1分別為角速度控制器的內(nèi)環(huán)角速度PID控制器的比例、 積分和微分系數(shù),k為采樣時(shí)刻.

3.2 線(xiàn)速度控制器設(shè)計(jì)

線(xiàn)速度控制器的外環(huán)距離PID控制器和內(nèi)環(huán)線(xiàn)速度PID控制器均采用增量式PID算法, 線(xiàn)速度控制器的外環(huán)距離PID控制器的輸入為實(shí)際位置與期望位置的距離誤差, 外環(huán)距離PID控制器的輸出為

以外環(huán)距離PID控制器的輸出為內(nèi)環(huán)線(xiàn)速度PID控制器的輸入, 線(xiàn)速度控制器的內(nèi)環(huán)線(xiàn)速度PID控制器的輸出為

其中:ev(k)=vr(k)-vc(k),vc為多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)的全局?jǐn)z像機(jī)采集的機(jī)器魚(yú)速度信息;kpi2,kii2和kdi2分別為線(xiàn)速度控制器的內(nèi)環(huán)線(xiàn)速度PID控制器的比例、 積分和微分系數(shù).

受舵機(jī)和魚(yú)皮等硬件結(jié)構(gòu)的約束, 實(shí)驗(yàn)中所用的機(jī)器魚(yú)巡游和機(jī)動(dòng)能力有限, 僅能完成左右轉(zhuǎn)彎和向前直游等動(dòng)作, 不能向后倒游和達(dá)到目標(biāo)位姿后立即靜止. 基于串級(jí)PID的仿生機(jī)器魚(yú)位姿控制算法中, 角速度指令和線(xiàn)速度指令連續(xù), 由于在多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)中, 基本控制通道已固化為有限的檔位, 因此需將角速度指令和線(xiàn)速度指令進(jìn)行離散化處理, 即分別將某個(gè)區(qū)間內(nèi)的角速度指令和線(xiàn)速度指令變換為對(duì)應(yīng)的速度檔和方向檔.

4 仿真結(jié)果與分析

4.1 仿真結(jié)果

在保證驗(yàn)證算法的有效性前提下, 盡可能減少仿生機(jī)器魚(yú)下水次數(shù), 避免機(jī)械疲勞以完成機(jī)器魚(yú)位姿控制算法中控制參數(shù)的整定. 以URWPGSim2D環(huán)境下的非對(duì)抗性二維仿真花樣游泳競(jìng)賽項(xiàng)目的6號(hào)仿真機(jī)器魚(yú)為驗(yàn)證位姿控制算法的模型. 為便于區(qū)分, 將6號(hào)仿真機(jī)器魚(yú)的顏色設(shè)置為綠色, 其中1號(hào)黃色仿真機(jī)器魚(yú)可隨機(jī)在場(chǎng)地游動(dòng), 不受策略控制. 為防止其他仿真機(jī)器魚(yú)對(duì)6號(hào)機(jī)器魚(yú)運(yùn)動(dòng)干擾, 將剩余紅色仿真機(jī)器魚(yú)置于左上角, 且不賦予任何動(dòng)作, 即除碰撞外處于靜止?fàn)顟B(tài), 如圖7所示. 坐標(biāo)系定義為： 以仿真場(chǎng)地中心為原點(diǎn),x軸向右,z軸向下, 負(fù)x軸順時(shí)針轉(zhuǎn)回負(fù)x軸角度范圍為[-π,π].策略編寫(xiě)系統(tǒng)和仿真機(jī)器魚(yú)的運(yùn)動(dòng)參數(shù)以毫米為單位.

圖7 URWPGSim2D環(huán)境下的仿真機(jī)器魚(yú)模型Fig.7 Robotic fish model in URWPGSim2D simulation environment

在機(jī)器魚(yú)的位姿控制中, 控制參數(shù)的選擇較復(fù)雜, 由于機(jī)器魚(yú)一直處于游動(dòng)狀態(tài), 系統(tǒng)穩(wěn)態(tài)誤差影響較小, 因此最后確定積分參數(shù).先將微分參數(shù)固定為零, 從小到大改變比例參數(shù)至滿(mǎn)足響應(yīng)速度要求, 再固定比例參數(shù), 從小到大改變微分參數(shù), 使機(jī)器魚(yú)穩(wěn)定游向目標(biāo)位姿, 利用“控制變量法”經(jīng)多次仿真實(shí)驗(yàn)整定得到串級(jí)PID算法的控制參數(shù), 結(jié)果列于表1. 仿真實(shí)驗(yàn)任務(wù)為仿真機(jī)器魚(yú)從初始位姿(-800 mm,500 mm,-π/2 rad)游到目標(biāo)位姿(600 mm,-600 mm,π/6 rad).

表1 仿真實(shí)驗(yàn)控制參數(shù)

4.2 仿真分析

圖8為URWPGSim2D環(huán)境下基于串級(jí)PID控制律以5 s為間隔的仿真程序視頻序列圖. 由圖8可見(jiàn), 到達(dá)目標(biāo)位姿所用時(shí)間為31 s. URWPGSim2D中的PoseToPose函數(shù)稱(chēng)為位姿到位姿控制函數(shù), 采用的時(shí)變反饋控制律為

圖8 URWPGSim2D環(huán)境下的視頻序列Fig.8 Video sequences in URWPGSim2D simulation environment

(10)

其中k1,k2和k3為時(shí)變反饋控制律的控制參數(shù),t為時(shí)間變量.

計(jì)算相同仿真任務(wù)下時(shí)變反饋控制律與串級(jí)PID控制律仿真機(jī)器魚(yú)位姿的絕對(duì)誤差. 圖9和圖10分別為時(shí)變反饋控制律與串級(jí)PID控制律的位置和方向角誤差對(duì)比結(jié)果. 由圖9和圖10可見(jiàn)： 仿真機(jī)器魚(yú)采用時(shí)變反饋控制算法的誤差為(29 mm,18 mm,0.111 1 rad), 到達(dá)目標(biāo)位姿所用時(shí)間為27 s； 仿真機(jī)器魚(yú)采用串級(jí)PID控制算法的誤差為(7 mm,5 mm,-0.035 8 rad), 到達(dá)目標(biāo)位姿所用時(shí)間為31 s. 相對(duì)時(shí)變反饋控制算法, 本文算法到達(dá)目標(biāo)位姿所用時(shí)間增加, 存在角度變化跳動(dòng)現(xiàn)象, 但位置誤差和方向誤差均減小, 提高了位姿控制精度, 在某些實(shí)際場(chǎng)合更符合應(yīng)用需求.

圖9 位置誤差對(duì)比Fig.9 Comparison of position error

圖10 方向角誤差對(duì)比Fig.10 Comparison of direction angle error

5 結(jié)果與討論

在多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)下, 導(dǎo)入根據(jù)串級(jí)PID控制律編寫(xiě)的策略進(jìn)行仿生機(jī)器魚(yú)位姿控制實(shí)驗(yàn), 策略編寫(xiě)系統(tǒng)和仿生機(jī)器魚(yú)信息采集系統(tǒng)以像素為單位, 即用像素單位描述位置. 仿生機(jī)器魚(yú)從初始位姿(100像素,330像素,-π/2 rad)游到目標(biāo)位姿(560像素,120像素,0)作為實(shí)驗(yàn)任務(wù), 以?xún)蓚€(gè)直徑為95像素的半圓形黑色橡膠為初始點(diǎn)和目標(biāo)點(diǎn)的標(biāo)記點(diǎn), 假設(shè)初始位姿至目標(biāo)位姿途中無(wú)障礙, 機(jī)器魚(yú)方向與x軸方向相同, 沿x軸向y軸旋轉(zhuǎn)為0～π rad, 反方向旋轉(zhuǎn)為0～-π rad. 多水下機(jī)器人協(xié)作控制系統(tǒng)中的仿生機(jī)器魚(yú)與URWPGSim2D中的仿真機(jī)器魚(yú)存在差異, 通過(guò)實(shí)驗(yàn)確定仿生機(jī)器魚(yú)的實(shí)體實(shí)驗(yàn)控制參數(shù)列于表2.

表2 實(shí)體實(shí)驗(yàn)控制參數(shù)

目標(biāo)位姿的方向角為0, 僅計(jì)算仿生機(jī)器魚(yú)從初始位姿到達(dá)目標(biāo)位姿過(guò)程中實(shí)際位姿與目標(biāo)位姿之間的絕對(duì)誤差. 圖11為多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)環(huán)境下以2 s為間隔的視頻序列圖.

圖11 多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)環(huán)境下的視頻序列Fig.11 Video sequences in multi-underwater robots cooperative control system platform environment

圖12～圖14分別為仿生機(jī)器魚(yú)在到達(dá)目標(biāo)位姿過(guò)程中的橫坐標(biāo)誤差、 縱坐標(biāo)誤差和方向角誤差. 由圖11～圖14可見(jiàn), 仿生機(jī)器魚(yú)到達(dá)目標(biāo)位姿所用時(shí)間為14.4 s, 誤差為(-3像素,-4像素,0.062 rad), 基本滿(mǎn)足仿生機(jī)器魚(yú)的循跡和搬運(yùn)等實(shí)際應(yīng)用要求, 表明了算法的有效性, 由于采用固定的比例、 積分和微分參數(shù), 因此仿生機(jī)器魚(yú)的方向角易跳動(dòng).

圖12 橫坐標(biāo)誤差Fig.12 Abscissa error

圖13 縱坐標(biāo)誤差Fig.13 Ordinate error

圖14 方向角誤差Fig.14 Directional angle error

綜上, 本文基于期望位姿建立坐標(biāo)系構(gòu)建機(jī)器魚(yú)的位姿誤差模型, 針對(duì)機(jī)器魚(yú)的位姿控制問(wèn)題, 利用串級(jí)PID控制算法與時(shí)變反饋控制算法在URWPGSim2D環(huán)境下進(jìn)行仿真實(shí)驗(yàn), 仿真結(jié)果表明, 在一定的誤差范圍內(nèi), 與時(shí)變反饋控制律相比, 串級(jí)PID控制律達(dá)到期望位姿所用時(shí)間增加, 且存在方向角角度變化跳動(dòng)現(xiàn)象, 但位置誤差和方向誤差均減小, 提高了位姿控制精度. 利用Microsoft Visual Studio 2010編寫(xiě)相應(yīng)策略, 將其分別應(yīng)用到URWPGSim2D軟件的仿真平臺(tái)和多水下機(jī)器人協(xié)作控制系統(tǒng)平臺(tái)上, 證明本文算法合理且有效.