基于串級PID的機器魚位姿控制算法

王 剛, 宋英杰,, 唐武生, 趙 強

(1. 白城師范學院 機械與控制工程學院, 吉林 白城 137000；2. 長春大學 機械與車輛工程學院, 長春 130022)

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

仿生機器魚依靠魚體和魚鰭等與水相互作用產生流體動力, 由于水環境的干擾和動力學的非線性, 因此仿生機器魚位姿控制的精準度較低. 本文以北京大學智能仿生實驗室開發的仿鲹科三關節機器魚為研究對象, 研究仿生機器魚的位姿控制問題.

1 三關節仿生機器魚

三關節仿生機器魚包括剛性頭部(含靜止橡膠材質胸鰭)、 柔性魚體(3個舵機串聯組成)和具有尾柄的尾鰭3個部分, 如圖1所示. 機器魚的剛性頭部無動力, 可有效抵御池壁的碰撞, 內部配重片可保證機器魚的魚體在水中直立. 柔性魚體由舵機、 串聯金屬骨架和橡膠魚皮組成, 柔性魚體和尾鰭通過各舵機驅動所對應的金屬骨架周期性擺動使機器魚擬合魚體波曲線運動, 模仿真魚借助擊水產生有效的反作用力推動機器魚前進.

圖1 仿生機器魚Fig.1 Biomimetic robotic fish

仿生機器魚采用CPG運動控制, 通過在線調整參數, 實現其多模態游動之間的平滑過渡[10-11]. 圖2為機器魚的CPG運動控制框圖[12].

圖2 機器魚的CPG運動控制框圖Fig.2 CPG motion control block diagram of robotic fish

CPG控制器的數學描述[12]如下：

(1)

(2)

(3)

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

(4)

其中i=1,2,3, 運動控制參數ri(t),xi(t)和φi(t)分別為第i個振蕩器的振幅、 偏移和相位,Ri,Xi和ν分別為第i個振蕩器的期望幅值、 期望偏移和期望頻率,φij為振蕩器i和振蕩器j之間的期望相位差,θi為第i個振蕩器對應關節的輸出角,α′,β′,μ為系統結構參數, 決定系統的收斂速度,N為振蕩器的總數, 對應機器魚的3個擺動關節. CPG模型的輸出為各關節對應的周期性變化擺角值, 經STM32核心板解析為驅動舵機的脈沖寬度調制(pulse width modulation, PWM)波信號. 在線調節CPG網絡模型間的連接權值參數以實現各關節之間的協調運動, 使機器魚自然協調游動.

2 位姿誤差模型

圖3 機器魚位姿誤差模型坐標系Fig.3 Posture error model coordinate of robotic fish

(5)

圖4 機器魚位姿控制模型流程Fig.4 Flow chart of posture control model of robotic fish

3 串級PID控制算法

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

圖5 串級PID控制系統結構Fig.5 Cascade PID control system structure

機器魚系統對基本控制通道的控制存在非線性因素且機器魚作業環境具有時變性, 由于全局攝像機采集的機器魚信息與實際狀態信息存在實時性誤差, 因此, 單級PID控制無法滿足機器魚位姿的控制精度, 采用串級PID可提高對機器魚的位姿控制, 即將響應速度快的線速度控制器和角速度控制器作為內環, 響應速度慢的距離控制器和方向角角度控制器作為外環[17]. 基于串級PID的仿生機器魚位姿控制原理如圖6所示.

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

3.1 角速度控制器設計

角速度控制器的外環角度PID控制器和內環角速度PID控制器均采用增量式PID算法, 角速度控制器的外環角度PID控制器的輸入為實際方向角與期望方向角的角度誤差和視線角之和, 外環角度PID控制器的輸出為

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

以外環角度PID控制器的輸出為內環角速度PID控制器的輸入, 內環角速度PID控制器的輸出為

其中:ew(k)=wr(k)-wc(k),wc為多水下機器人協作控制系統平臺的全局攝像機采集的機器魚角速度信息;kpi1,kii1和kdi1分別為角速度控制器的內環角速度PID控制器的比例、 積分和微分系數,k為采樣時刻.

3.2 線速度控制器設計

線速度控制器的外環距離PID控制器和內環線速度PID控制器均采用增量式PID算法, 線速度控制器的外環距離PID控制器的輸入為實際位置與期望位置的距離誤差, 外環距離PID控制器的輸出為

以外環距離PID控制器的輸出為內環線速度PID控制器的輸入, 線速度控制器的內環線速度PID控制器的輸出為

其中:ev(k)=vr(k)-vc(k),vc為多水下機器人協作控制系統平臺的全局攝像機采集的機器魚速度信息;kpi2,kii2和kdi2分別為線速度控制器的內環線速度PID控制器的比例、 積分和微分系數.

受舵機和魚皮等硬件結構的約束, 實驗中所用的機器魚巡游和機動能力有限, 僅能完成左右轉彎和向前直游等動作, 不能向后倒游和達到目標位姿后立即靜止. 基于串級PID的仿生機器魚位姿控制算法中, 角速度指令和線速度指令連續, 由于在多水下機器人協作控制系統平臺中, 基本控制通道已固化為有限的檔位, 因此需將角速度指令和線速度指令進行離散化處理, 即分別將某個區間內的角速度指令和線速度指令變換為對應的速度檔和方向檔.

4 仿真結果與分析

4.1 仿真結果

在保證驗證算法的有效性前提下, 盡可能減少仿生機器魚下水次數, 避免機械疲勞以完成機器魚位姿控制算法中控制參數的整定. 以URWPGSim2D環境下的非對抗性二維仿真花樣游泳競賽項目的6號仿真機器魚為驗證位姿控制算法的模型. 為便于區分, 將6號仿真機器魚的顏色設置為綠色, 其中1號黃色仿真機器魚可隨機在場地游動, 不受策略控制. 為防止其他仿真機器魚對6號機器魚運動干擾, 將剩余紅色仿真機器魚置于左上角, 且不賦予任何動作, 即除碰撞外處于靜止狀態, 如圖7所示. 坐標系定義為： 以仿真場地中心為原點,x軸向右,z軸向下, 負x軸順時針轉回負x軸角度范圍為[-π,π].策略編寫系統和仿真機器魚的運動參數以毫米為單位.

圖7 URWPGSim2D環境下的仿真機器魚模型Fig.7 Robotic fish model in URWPGSim2D simulation environment

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

表1 仿真實驗控制參數

4.2 仿真分析

圖8為URWPGSim2D環境下基于串級PID控制律以5 s為間隔的仿真程序視頻序列圖. 由圖8可見, 到達目標位姿所用時間為31 s. URWPGSim2D中的PoseToPose函數稱為位姿到位姿控制函數, 采用的時變反饋控制律為

圖8 URWPGSim2D環境下的視頻序列Fig.8 Video sequences in URWPGSim2D simulation environment

(10)

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

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

圖9 位置誤差對比Fig.9 Comparison of position error

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

5 結果與討論

在多水下機器人協作控制系統平臺下, 導入根據串級PID控制律編寫的策略進行仿生機器魚位姿控制實驗, 策略編寫系統和仿生機器魚信息采集系統以像素為單位, 即用像素單位描述位置. 仿生機器魚從初始位姿(100像素,330像素,-π/2 rad)游到目標位姿(560像素,120像素,0)作為實驗任務, 以兩個直徑為95像素的半圓形黑色橡膠為初始點和目標點的標記點, 假設初始位姿至目標位姿途中無障礙, 機器魚方向與x軸方向相同, 沿x軸向y軸旋轉為0～π rad, 反方向旋轉為0～-π rad. 多水下機器人協作控制系統中的仿生機器魚與URWPGSim2D中的仿真機器魚存在差異, 通過實驗確定仿生機器魚的實體實驗控制參數列于表2.

表2 實體實驗控制參數

目標位姿的方向角為0, 僅計算仿生機器魚從初始位姿到達目標位姿過程中實際位姿與目標位姿之間的絕對誤差. 圖11為多水下機器人協作控制系統平臺環境下以2 s為間隔的視頻序列圖.

圖11 多水下機器人協作控制系統平臺環境下的視頻序列Fig.11 Video sequences in multi-underwater robots cooperative control system platform environment

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

圖12 橫坐標誤差Fig.12 Abscissa error

圖13 縱坐標誤差Fig.13 Ordinate error

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

綜上, 本文基于期望位姿建立坐標系構建機器魚的位姿誤差模型, 針對機器魚的位姿控制問題, 利用串級PID控制算法與時變反饋控制算法在URWPGSim2D環境下進行仿真實驗, 仿真結果表明, 在一定的誤差范圍內, 與時變反饋控制律相比, 串級PID控制律達到期望位姿所用時間增加, 且存在方向角角度變化跳動現象, 但位置誤差和方向誤差均減小, 提高了位姿控制精度. 利用Microsoft Visual Studio 2010編寫相應策略, 將其分別應用到URWPGSim2D軟件的仿真平臺和多水下機器人協作控制系統平臺上, 證明本文算法合理且有效.