杜曉瑋,侯冬冬,王紅都,3,黎 明,3

基于四元數(shù)反饋的UVMS滑?？刂破髟O計

杜曉瑋1,侯冬冬2,王紅都1,3,黎 明1,3

(1.中國海洋大學工程學院,山東 青島,266100; 2.河南省水下智能裝備重點實驗室,河南 鄭州,450015; 3.山東省海洋智能裝備技術(shù)工程研究中心,山東 青島,266100)

隨著水下機器人-機械手系統(tǒng)(UVMS)向輕量化、敏捷化方向發(fā)展,對高精度水下姿態(tài)控制的需求日益突出。傳統(tǒng)方法多使用歐拉角表示姿態(tài),其在特殊姿態(tài)下求運動學逆解會產(chǎn)生奇異,不能滿足特殊姿態(tài)作業(yè)的要求。因此,文中采用四元數(shù)表示水下機器人本體姿態(tài),建立基于四元數(shù)的運動學模型,避免了歐拉角的萬向鎖問題。同時,進一步構(gòu)建了UVMS的動力學模型,構(gòu)造基于四元數(shù)誤差的滑模面,設計了位置、姿態(tài)滑?？刂破?并通過廣義Lyapunov理論分析了系統(tǒng)的穩(wěn)定性。為了實現(xiàn)更好的控制效果,利用時間誤差絕對值函數(shù)設計適應度函數(shù),采用遺傳算法優(yōu)化了控制器參數(shù)。仿真結(jié)果驗證了控制器的有效性。

水下機器人-機械手系統(tǒng); 四元數(shù)反饋; 滑模控制; 遺傳算法; 參數(shù)優(yōu)化

0 引言

為了開發(fā)海洋資源,開展水下作業(yè)任務,人們對水下機器人-機械手系統(tǒng)(undersea vehicle manipulator system,UVMS)進行了大量研究,取得了許多成果。在上世紀90年代,UVMS本體的體型和質(zhì)量都比較大,如ALIVE[1]和SAUVIM[2]的機械手的運動就對本體影響較小。21世紀初期,對基于輕型自主水下航行器(autonomous undersea vehicle,AUV)本體的UVMS研究逐漸增多。GIRONA 500[3]是西班牙資助項目中開發(fā)的一種更輕便、更靈活的水下機器人,目前主要用于淺水作業(yè),已經(jīng)在許多項目中使用,如RAUVI、TRITON、TRIDENT、PANDORA和MERBOTS等項目[4]。

隨著自主水下航行器(autonomous undersea vehicle,AUV)輕型化的發(fā)展,UVMS本體和機械手的質(zhì)量比變小,導致系統(tǒng)的耦合性增強,控制難度增加,因此需要使用更先進的控制算法。Antonelli[5]詳細討論了UVMS的多種控制策略,如反饋線性化、前饋解耦控制、自適應控制、非線性控制、基于虛擬分解的控制等,并且分別在慣性坐標系和體坐標系下設計了多種控制方案。Dai等[6]提出了一種間接自適應控制方案,該方案由基于擴展卡爾曼濾波(extended Kalman filter,EKF)的補償系統(tǒng)、基于模型計算轉(zhuǎn)矩的控制器(a model-based computed torque controller,CTC)和魯棒補償跟蹤控制器三部分組成,該控制器具有良好的干擾抑制效果。Dai等[7-8]又進一步研究了基于快速模型預測控制(fast model predictive control,FTMPC)的方法。Ramezani等[9]討論了UVMS的自適應滑?？刂?充分發(fā)揮了滑?？刂瓶箙?shù)變化和外部干擾的能力。Dai等[10]提出將滑模阻抗控制(sliding mode impedance control,SMIC)方法應用于I-AUV(UVMS的一種)的位置/力控制中,以完成某些水下操作任務。

用歐拉角表示姿態(tài)具有直觀、易于理解等優(yōu)點,因此在上述文獻中,大多采用歐拉角來表示本體的姿態(tài),設計反饋進行控制,但是在俯仰角接近90°時,歐拉角方法無法處理,逆運動學求解會出現(xiàn)多解問題,產(chǎn)生姿態(tài)表示上的奇異。為了滿足一些特殊的操作任務或減少上升過程中的阻力,UVMS本體的姿態(tài)需要更加多樣化,同時水下航行器的小型化和輕量化也使得這種姿態(tài)成為可能,此時使用歐拉角表示姿態(tài),便會產(chǎn)生奇異。而使用四元數(shù)表示本體的姿態(tài)可以很好地解決此類問題。

Fjellstad等[11]詳細介紹了如何使用四元數(shù)表示UVMS本體的姿態(tài),建立了對應的運動學模型,并設計了比例-積分-微分(proportional integral derivative,PID)控制器進行了控制仿真。Antonelli 等[12-13]基于四元數(shù)反饋設計了一種滑?？刂破?而后又進一步設計了自適應參數(shù)的控制器,并且在AUV的全方位智能導航器(omni-directional intelligent navigator,ODIN)上進行了實驗驗證。Arrichiello等[14]基于四元數(shù)反饋,研究了安裝單方向驅(qū)動器的AUV的位置和姿態(tài)控制,其控制策略基于有界單純形法。

滑?？刂破鲗朔蓴_和系統(tǒng)的不確定性有很強的魯棒性,尤其是對非線性系統(tǒng)具有良好的控制效果,且算法簡單,響應速度快。而結(jié)合滑?？刂婆c四元數(shù)反饋對UVMS進行控制的文獻比較少。因此,為了避免歐拉角的萬向鎖問題,建立了基于四元數(shù)的運動學模型。針對UVMS這一強耦合、非線性的復雜系統(tǒng),設計了基于四元數(shù)誤差的滑?？刂破?對UVMS的姿態(tài)進行控制,分析了控制器的穩(wěn)定性。為了得到更優(yōu)的控制器參數(shù),利用時間誤差絕對值函數(shù)(integ- rated time absolute error,ITAE)設計適應度函數(shù),將控制器參數(shù)選擇問題轉(zhuǎn)化成遺傳算法(genetic algorithm,GA)的優(yōu)化問題。仿真結(jié)果表明了控制器的有效性。

1 基于四元數(shù)的UVMS建模

1.1 正運動學模型

圖1 UVMS坐標系示意圖

基于四元數(shù)的表示方法,本體速度在體坐標系與慣性坐標系之間的變換關系如下[12]

其中

機械臂從肩部關節(jié)到末端手爪關節(jié)依次編號1～6,仿真模型是Smart3S機械臂[5],使用標準DH法建模后得到的DH參數(shù)如表1所示。

根據(jù)標準DH法對應的坐標系變換矩陣,可以利用本體位置、DH參數(shù)表和機械臂各關節(jié)角度計算末端手爪的位置。

1.2 逆運動學模型

表1 機械臂DH參數(shù)表

可以通過簡化的方式計算體坐標系下的參考速度[5](下標表示期望值),即

1.3 動力學模型

結(jié)合Fossen經(jīng)典六自由度動力學模型[15-17],并根據(jù)機械臂的自由度進行擴展,最終得到十二自由度的UVMS動力學模型[5]

動力學模型滿足以下3個重要的性質(zhì)[12],將模型中的質(zhì)量慣性矩陣、科氏力和向心力矩陣、阻尼系數(shù)矩陣簡化為、、,得

2 滑?？刂破髟O計

2.1 滑模面及誤差定義

姿態(tài)誤差的時間導數(shù)為

滑模面是速度誤差和位姿、關節(jié)角度誤差的線性組合

2.2 控制器設計

文中設計的滑?？刂破鳛?/p>

2.3 穩(wěn)定性證明

針對式(13),選用如下李雅普諾夫函數(shù)分析系統(tǒng)的穩(wěn)定性

其導數(shù)為

將滑模面式(13)的導數(shù)形式和動力學模型性質(zhì)式(9)代入式(17),得到

3 基于GA的控制器參數(shù)優(yōu)化

式中:是仿真總時長;()是時刻的誤差。式(22)是連續(xù)函數(shù)的積分形式,不便于仿真計算,需對其作離散化處理,得到離散條件下的ITAE,即

終止條件為多個條件的并集,包括期望的適應度值、最大的迭代次數(shù)、優(yōu)化停滯不前的次數(shù),滿足其中任何一個條件,即可結(jié)束當前的優(yōu)化過程。參數(shù)優(yōu)化流程圖如圖2所示。

圖2 基于遺傳算法的參數(shù)優(yōu)化流程圖

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

整個控制系統(tǒng)仿真框圖如圖3所示。設計了滑?？刂破鞑?yōu)化了參數(shù),最終實現(xiàn)了末端手爪的定點控制。

圖3 系統(tǒng)仿真框圖

文中使用的UVMS模型參數(shù)可以參考文獻[5]。使用的開源函數(shù)庫Simurv4.0也可以參考文獻[5]。Simurv4.0不是一個一鍵式的模擬器,而是一個通用函數(shù)庫,包含常用的坐標轉(zhuǎn)換矩陣和雅可比矩陣等,可以用它來測試UVMS的運動學和動力學控制算法。

為了減少滑?？刂破鞯亩墩?仿真過程中使用飽和函數(shù)代替符號函數(shù)式(15)

式中,是邊界層,仿真中=2。

控制器更新為

仿真分為2個階段進行: 第1階段,進行定點控制,將本體姿態(tài)的俯仰角設定為90°,驗證四元數(shù)姿態(tài)表示的無奇異性,同時利用MATLAB的GA工具箱對滑?？刂破鲄?shù)進行優(yōu)化,控制目標是使UVMS機械臂的末端手爪接近目標點; 第2階段,加入軌跡規(guī)劃和逆運動學求解,使機械臂末端手爪移動到指定位置,并通過運動學正向計算驗證,最后繪制UVMS的狀態(tài)圖和各仿真曲線?？刂破鞯姆抡娌介L均為0.005 s,仿真總時長均為20 s。

階段1: 首先對UVMS實現(xiàn)定點控制,本體的期望位置為[12 2 4]。后續(xù)沒有特殊說明的情況下,位置坐標的單位都是m,即UVMS體坐標系的原點需要從慣性坐標系下的[0 0 0]點移動到 [12 2 4]點; 本體的期望姿態(tài)角為[0 90 0]°,俯仰角設定為90°,來驗證四元數(shù)表示的無奇異性; 關節(jié)角的設定值為[0 -70 -30 -30 -170 -90]°,處于一種伸展的狀態(tài)。

圖4 GA優(yōu)化結(jié)果圖

將GA尋優(yōu)得到的最佳個體,作為控制器的參數(shù),繪制UVMS的仿真效果圖。其中,UVMS的終止狀態(tài)和運行軌跡如圖5(a)所示,所繪制的軌跡是本體的重心軌跡; 剛體的位置和姿態(tài)跟蹤性能如圖5(b)所示,可知俯仰角最終達到88.8°,雖然存在1.2°的穩(wěn)態(tài)誤差,但并沒有產(chǎn)生歐拉角表示的奇異問題; 剛體的位置誤差如圖5(c)所示; 機械臂關節(jié)角度的時間變化曲線如圖5(d)所示。由圖5可知,系統(tǒng)響應速度較快,基本在6 s左右即可達到穩(wěn)定狀態(tài),且穩(wěn)態(tài)誤差較小,表明了控制器良好的控制效果。

階段2: 在階段1之后,UVMS機械臂的末端手爪已經(jīng)到達目標點[3 3.5 14]附近。此時,使用梯形速度曲線規(guī)劃末端手爪從當前位置到指定位置的軌跡,通過式(6)進行逆運動學求解,可得到體坐標系下的參考速度,進而通過坐標變換,轉(zhuǎn)換到慣性坐標系下并進行積分,可得到慣性坐標系下的參考位置,即控制器的設定值。

圖5 階段1仿真曲線

在仿真過程中,UVMS抓取前、后的狀態(tài)分別如圖6(a)～(b)所示。注意抓取的初始狀態(tài)是階段1的結(jié)束狀態(tài)。機械臂末端手爪的實際位置和參考位置如圖7所示,可知該控制器可以精確地到達目標位置,并且抖振較小,具有較好的控制效果。

圖6 抓取前后狀態(tài)圖

圖7 末端手爪實際與參考位置曲線

5 結(jié)束語

文中采用四元數(shù)反饋的方法為UVMS設計了滑?？刂破?避免了俯仰角接近90°時,歐拉角進行姿態(tài)表示的多解問題,即避免了姿態(tài)表示的奇異性; 并在仿真中設計特殊的作業(yè)情況,驗證四元數(shù)姿態(tài)表示的有效性。同時為了得到更好的控制效果,利用MATLAB的GA工具箱對控制器參數(shù)進行優(yōu)化,設計適應度函數(shù),取最小適應度值對應的種群個體作為控制器的參數(shù)。仿真試驗驗證了控制器的有效性。最后通過運動學逆解、坐標變換和積分,實現(xiàn)了機械臂末端手爪的定點控制。后續(xù)需要進一步研究存在障礙物的情況下,如何進行路徑規(guī)劃,以及進一步考慮末端手爪的姿態(tài),而非僅實現(xiàn)末端手爪的位置控制。

[1] Evans J,Redmond P,Plakas C,et al. Autonomous Docking for Intervention-AUVs Using Sonar and Video-based real-time 3D Pose Estimation[C]//Oceans. San Diego,CA,USA: IEEE,2003.

[2] Marani G,Choi S K,Yuh J. Underwater Autonomous Manipulation for Intervention Missions AUVs[J]. Ocean Engineering,2009,36(1): 15-23.

[3] Ribas D,Palomeras N,Ridao P,et al. Girona 500 AUV: From Survey to Intervention[J]. IEEE/ASME Transactions on Mechatronics,2012,17(1): 46-53.

[4] Zereik,Bibuli E,Marco,et al. Challenges and Future Trends in Marine Robotics[J]. Annual Reviews in Control,2018,46: 350-368.

[5] Antonelli G. Underwater Robots[M]. Berlin Heidelberg: Springer,2014.

[6] Dai Y,Yu S. Design of an Indirect Adaptive Controller for the Trajectory Tracking of UVMS[J]. Ocean Engineering,2018,151: 234-245.

[7] Dai Y,Yu S,Yan,et al. An EKF-Based Fast Tube MPC Scheme for Moving Target Tracking of a Redundant Underwater Vehicle-Manipulator System[J]. IEEE/ASME Transactions on Mechatronics,2020,24(6): 2803-2814.

[8] Dai Y,Yu S,Yan Y. An Adaptive EKF-FMPC for the Trajectory Tracking of UVMS[J]. IEEE Journal of Oceanic Engineering,2019,45(3): 1-15.

[9] Ramezani-Al M R,Sereshki Z T. A Novel Adaptive Sliding Mode Controller Design for Tracking Problem of an AUV in the Horizontal Plane[J]. International Journal of Dynamics and Control,2019,7(2): 679-689.

[10] Dai P,Lu W,Le K,et al. Sliding Mode Impedance Control for Contact Intervention of an I-AUV: Simulation and Experimental Validation[J]. Ocean Engineering,2020,196: 106855.

[11] Fjellstad O E,Fossen T I. Position and Attitude Tracking of AUV’s: a Quaternion Feedback Approach[J]. IEEE Journal of Oceanic Engineering,2002,19(4): 512-518.

[12] Antonelli G,Chiaverini S. Singularity-free Regulation of Underwater Vehicle-manipulator Systems[C]//Proceedings of the 1998 American Control Conference. ACC(IEEE Cat. No.98CH36207). Philadelphia,USA: IEEE,1998,1: 399-403.

[13] Antonelli G,Chiaverini S,Sarkar N,et al. Adaptive Control of an Autonomous Underwater Vehicle: Experimental Results on ODIN[J]. IEEE Transactions on Control Systems Technology,2001,9(5): 756-765.

[14] Arrichiello V,Bartolini G,Pisano A,et al. Attitude and Position Tracking of Autonomous 6 DOF Vehicles with Mono-directional Actuators[C]//2016 14th International Workshop on Variable Structure Systems(VSS). Nanjing,China: IEEE,2016: 80-85.

[15] Thor I F. Guidance and Control of Ocean Vehicles[M]. Chichester,UK: John Wiley & Sons,1994.

[16] 魏延輝. UVMS系統(tǒng)控制技術(shù)[M]. 哈爾濱: 哈爾濱工程大學出版社,2017: 19-69.

[17] 李小崗,王紅都,黎明. 自主水下航行器機械臂系統(tǒng)非線性PD控制器設計[J]. 水下無人系統(tǒng)學報,2020,28(1): 24-32.

Li Xiao-gang,Wang Hong-du,Li Ming. Nonlinear PD Controller Design for Autonomous Undersea Vehicle-Manipulator System[J]. Journal of Unmanned Undersea Systems,2020,28(1): 24-32.

1. 薛乃耀,劉鯤,王冬姣,等. 考慮模型誤差的ROV抗飽和控制設計. 2021,29(3).

2. 黃博倫,楊啟. 基于super-twisting二階滑模算法的作業(yè)型ROV路徑跟蹤控制方法. 2021,29(1).

3. 施文煜,梁霄,曲星儒,等. 基于RBF積分滑模的無人艇集群協(xié)同路徑跟蹤控制. 2020,28(6).

4. 徐鵬程,曾慶軍,陳堯偉,等. 基于導管螺旋槳負載的ROV推進器滑?？刂? 2020,28(5).

5. 王香,張永林. 基于RBF神經(jīng)網(wǎng)絡的AUV路徑跟蹤分數(shù)階滑?？刂? 2020,28(3).

6. 霍宇彤,郭晨,于浩淼. 欠驅(qū)動AUV三維路徑跟蹤RBF神經(jīng)網(wǎng)絡積分滑?？刂? 2020,28(2).

7. 姜曉奇,劉維亭,魏海峰,等. 基于全階狀態(tài)滑模觀測器的混合永磁記憶電機磁鏈觀測. 2020,28(2).

8. 李小崗,王紅都,黎明. 自主水下航行器機械臂系統(tǒng)非線性PD控制器設計. 2020,28(1).

9. 李鑫,黃茹楠,丁寧. 輸入受限的自主水下航行器自適應反演控制. 2019,27(6).

10. 張明明,劉維亭,魏海峰,等. 基于滑模觀測器的PMSM單相霍爾位置傳感器故障容錯控制算法. 2019,27(6).

11. 桑宏強,于佩元,孫秀軍. 基于航向補償?shù)乃禄铏C路徑跟蹤控制方法. 2019,27(5).

12. 林樹鋒,王冬姣,葉家瑋,等. 基于蟻群算法的水下機器人機械臂工作路徑優(yōu)化. 2019,27(1).

13. 齊貝貝,嚴衛(wèi)生,高劍. 基于偶極勢場的移動回收站空間回塢導引算法. 2018,26(3).

14. 嚴浙平,李響,宋育武,等. 參數(shù)攝動下基于積分滑模的欠驅(qū)動UUV軌跡跟蹤控制方法. 2018,26(3).

Sliding Mode Controller Design Based on Quaternion Feedback for UVMS

1,2,1,3,1,3

(1. College of Engineering,Ocean University of China,Qingdao 266100,China; 2. Henan Key Laboratory of Underwater Intelligent Equipment,Zhengzhou 450015,China; 3. Shandong Marine Intelligent Equipment Technology Engineering Research Center,Qingdao 266100,China)

With the development of lightweight and agile undersea vehicle manipulator systems(UVMSs),the demand for high-precision underwater attitude control is becoming increasingly prominent. Euler angle is traditionally used to represent the attitude; however,an inverse kinematic solution under a special attitude results in a singularity when this representation is employed. As a result,the requirements of special operations cannot be fulfilled through this representation. In this study,therefore,the quaternion is used to represent the posture of the rigid body of an undersea vehicle,and a quaternion-based kinematics model is established to avoid the gimbal lock. Furthermore,a dynamic model for the UVMS,sliding mode surface based on quaternion error,and position and attitude sliding mode controller are established. The stability of the system is analyzed by the generalized Lyapunov theory. In order to achieve a better control effect,this study uses the function of integrate time absolute error(ITAE) to design the fitness function and uses the genetic algorithm(GA) to optimize the controller parameters. The simulation results verify the effectiveness of the controller.

undersea vehicle manipulator system; quaternion feedback; sliding mode control; genetic algorithm; parameters optimization

TP241; TJ630.32

A

2096-3920(2021)04-0407-08

10.11993/j.issn.2096-3920.2021.04.006

杜曉瑋,侯冬冬,王紅都,等. 基于四元數(shù)反饋的UVMS滑模控制器設計[J]. 水下無人系統(tǒng)學報,2021,29(4): 407-414.

2020-10-20;

2020-11-12.

河南省水下智能裝備重點實驗室開放基金(KL02A1802); 中央高?；究蒲袠I(yè)務費專項(201964012).

杜曉瑋(1995-),女,在讀碩士,主要研究方向為水下機器人-機械手系統(tǒng)建模與運動控制.

(責任編輯: 許 妍)