劉冠喜 張永順

摘要：考慮到自主研發(fā)的雙半球膠囊機器人系統(tǒng)不確定性以及外界擾動等非線性特征問題，提出了一種基于 Hamilton-Jacobi不等式（HJI）理論的雙半球膠囊機器人滑模魯棒非線性控制策略。通過拉格朗日動力學原理建立雙半球膠囊機器人被動模態(tài)的動力學模型，并將 Hamilton-Jacobi不等式理論應用于設計滑模魯棒的控制律來確?？刂葡到y(tǒng)的穩(wěn)定性。此外，通過 Lyapunov方程驗證了該雙半球膠囊機器人基于 HJI理論的滑模魯棒控制系統(tǒng)的有效性，并對所設計的控制器進行了仿真實驗，仿真和實驗結果表明所設計的控制策略能有效地抑制外部干擾對控制誤差的影響，提高了雙半球膠囊機器人系統(tǒng)的控制精度。

關鍵詞：雙半球膠囊機器人;拉格朗日動力學;HJI理論;滑模魯棒控制

中圖分類號：TP242文獻標志碼：A

文章編號：1009-9492（2021）11-0026-04開放科學（資源服務）標識碼（OSID）：

Nonlinear Posture Control Method of Dual Hemisphere Capsule Robot Based on HJI Theory

Liu Guanxi ，Zhang Yongshun

（Key Laboratory for Precision & Non-traditional Machining of Ministry of Education， Dalian University of Technology， Dalian，Liaoning 116024， China）

Abstract： Considering the nonlinear characteristics of self-developed dual hemisphere capsule robots（DHCRs） such as indeterminate and external disturbances， a sliding mode robust control strategy for the DHCR based on Hamilton-Jacobi inequality（HJI） theory was proposed. The dynamic model of the passive mode of the DHCR was established by Lagrangian dynamics principle， and the HJI inequality theory was applied to design the sliding mode robust control law to ensure the stability of the control system. In addition， the effectiveness of the control system based on the HJI theory was verified through the Lyapunov equation， and simulation experiments on the designed controller was conducted. Simulation and experimental results show that the control strategy can effectively suppress the influence of external interference on the control error， and improve the control accuracy of the DHCR.

Key words： dual hemisphere capsule robot; Lagrange dynamics; HJI theory; sliding mode robust control

0 引言

目前，可代替內窺鏡檢查的膠囊內窺鏡機器人取得了較快的發(fā)展[1-4]，與繁瑣的傳統(tǒng)內窺鏡相比，膠囊內窺鏡由于具有安全，無痛等特征，可望在復雜環(huán)境中執(zhí)行醫(yī)學診療任務。按照驅動方式，膠囊內窺鏡可分為被動式和主動式膠囊內窺鏡。主動式膠囊內窺鏡也稱為膠囊機器人，主要包括電驅動式和磁驅動式膠囊機器人。由于磁驅動膠囊機器人可實現(xiàn)無纜驅動，并避免了復雜的行走機構，因此，磁驅動膠囊機器人具有良好的應用前景，并迅速成為國內外的研究熱點。大連理工大學膠囊機器人課題組提出了一種雙半球型膠囊機器人，在空間萬向旋轉磁場驅動隨動效應驅動下，在腸胃寬裕環(huán)境下可以實現(xiàn)主動模態(tài)下的滾動行走和被動模態(tài)下的定點調姿[5]，通過滾動行走接近可疑目標，通過定點調姿實現(xiàn)對可疑目標的全景觀察與篩查?？臻g萬向旋轉磁場是在三軸正交嵌套方形亥姆霍茲線圈中通入滿足一定關系的三相交變電流，在三軸亥姆霍茲線圈的內部空間疊加產(chǎn)生的，能夠消除磁力，解決磁力與磁矩相互耦合的問題，從而通過力矩的隨動效應實現(xiàn)雙半球型膠囊機器人的精確驅動[6-7]。與一般典型球形機器人采用配重塊實現(xiàn)姿態(tài)穩(wěn)定控制方式不同[8]，雙半球膠囊依賴于磁場隨動效應實現(xiàn)姿態(tài)的穩(wěn)定控制，即雙半球膠囊軸線與旋轉磁場軸線具有跟隨特性，可通過旋轉磁場控制膠囊軸線在空間的位置，進而實現(xiàn)膠囊主動模態(tài)下滾動行進和被動模態(tài)下定點調姿兩種功能。

然而，由于雙半球膠囊機器人的動力學模型是一個多變量非線性系統(tǒng)，存在不確定性因素以及外界干擾，例如非結構化環(huán)境摩擦、腸蠕動干擾和質心位移等，這些因素對定點姿態(tài)調整的準確性具有一定影響。為了探索機器人姿態(tài)控制的動態(tài)特性，需要對非線性系統(tǒng)進行深入分析[9-11]。隨著膠囊機器人技術的深入研究和應用場景的擴展，各種智能控制策略的應用使得膠囊機器人控制系統(tǒng)的非線性特性得到了較好的解決。例如，模糊控制，神經(jīng)網(wǎng)絡和專家系統(tǒng)以及其他非線性自適應控制方法可以通過多個簡單關聯(lián)來實現(xiàn)復雜的非線性函數(shù)，使其成為探索非線性系統(tǒng)的有效方法[12]。因此，本文提出了一種基于 Hamilton-Jacobi不等式（HJI）理論的滑模魯棒控制策略，該方法解決了線性化的局限性，考慮了雙半球膠囊機器人模型的不確定性以及外界擾動，使系統(tǒng)具有更強的穩(wěn)定性和抗擾動能力。膠囊機器人非線性自適應控制的研究對于膠囊機器人的發(fā)展具有十分重大的理論意義。

1 雙半球型膠囊機器人的系統(tǒng)原理

1.1 雙半球膠囊機器人主要結構

雙半球型膠囊機器人的樣機及結構如圖1所示，由主動半球和被動半球體兩部分組成，其中主動半球體包括殼體、套筒、LED 電路、軸承、圖像采集元件和徑向充磁磁環(huán)等組件;被動半球體包括殼體、電池、軸及射頻電路等組件。

膠囊機器人的主、被動半球體由軸承懸浮連接，在空間萬向旋轉磁場中，主動半球體在耦合磁矩的作用下跟隨旋轉磁矢量轉動。當磁場法向量水平時，機器人處于主動模態(tài)下整機滾動移位模式;當磁場法向量處于豎直方位時，被動半球體在摩擦力的約束下靜止，膠囊處于被動模態(tài)下定點觀察模式，通過磁場方位變換可實現(xiàn)雙半球型膠囊調姿與滾動行走雙重模態(tài)分離與轉換。

1.2 雙半球型膠囊機器人的動力學模型

為了便于建立膠囊機器人的動力學特性的分析，建立雙半球膠囊機器人的固定坐標系 Ox0y0z0和賴柴坐標系 Ox2y2z2，賴柴坐標系 Ox2y2z2表示機器人的軸線方位，賴柴坐標系 Ox2y2z2初始時與固定坐標系 Ox0y0z0重合，兩個坐標系之間的相對位置及雙半球膠囊機器人的運動可用俯仰角α和側擺角β描述，先繞 Oy0軸轉過α角度，得到中間動坐標系 Ox1y1z1;再將動坐標系 Ox1y1z1繞 Ox1軸旋轉β角度，其坐標變換關系如圖2所示。

可以得到賴柴坐標系到固定坐標系的齊次變換矩陣為：

采用拉格朗日方程建立膠囊機器人被動模態(tài)動力學模型，具有廣義坐標 q 的拉格朗日機械系統(tǒng)的動力學方程表示為[13]：

式中：L 為機器人被動模態(tài)時系統(tǒng)勢能和動能之和;qk為系統(tǒng)的廣義坐標;Qk為廣義力。

其動能可表示為：

L = J1（ωz 1）2+ J2（ωz 11）2+ Je （ωx 11）2+（ωy11）2

式中：ωx 1、ωy1、ωz 1分別為角速度ω′在 Ox1、Oy1、Oz1軸的分量;ωx 11、ωy11、ωz 11分別為角速度ω"在 Ox1、Oy1、Oz1軸的分量。

將式（3） 代入式（2），得到雙半球膠囊被動模態(tài)動力學模型：

J （θ）θ?+ C （θ， θ? ）θ? + G （θ）+ df +Δ（ q， q? ） = T（4）

式中： T 為輸入控制;df為系統(tǒng)外界干擾; Δ（ q， q? ） 為建模不確定部分; G（θ）為質心偏移力矩。

慣性矩陣J（θ）和離心力與哥氏力矩陣 C（θ， θ? ） 為：

式中：c11=（J1+ J2- Je ）β? sin β cosβ; c12= J1ω cosβ+（J1+ J2- Je ）α? sin β cosβ;c21=-J1ω cosβ-（J1+ J2- Je ）α? sin β cosβ;c22=0;J1、J2和 Je 分別為雙半球膠囊機器人在xyz坐標系3個軸向上的主轉動慣量。

2 基于 HJI理論的控制器設計

2.1 控制器設計

雙半球型膠囊機器人控制系統(tǒng)的設計十分重要。膠囊在運行過程中，由于存在不確定性因素以及外界干擾，因此，精確建模困難。為了減少模型不確定性對系統(tǒng)的影響， 本文設計了一個基于 HJI理論的魯棒控制器，將 e 設為系統(tǒng)的位置誤差：

e（t）=θd （t）-θ（t）（7）

式中：θd （t）=[αdβd ]為雙半球膠囊機器人的期望角軌跡。

設計控制律為：

T =J （θ）θ?d + C （θ， θ? ）θ?d + G （θ）μ

式中：μ為反饋控制律。

將式（8） 代入式（4） 中，可得：

J （θ）+ C （θ， θ?）e? + d = T

根據(jù)雙半球膠囊機器人動力學方程，滑模構控制器滑模表面定義為：

r =e? +Λe （10）

式中：Λ= diag{Λ1，Λ2 ， … ， Λm}>0，為正定對角陣。

將式（10）代入式（9） 中，可以得到：

式中： w =JΛe? +CΛe。

根據(jù) HJI 理論，將滑模函數(shù)作為評價信號 z ，式（11）可以改寫為：

為了表示系統(tǒng)的抗干擾能力，定義性能指標為：

根據(jù)式（11）， Hamilton-Jacobi 不等式理論可描述為：給定任意一個正數(shù)γ，如果存在一個正定且可微的函數(shù) L（x）≥0，同時滿足：

L? ≤懇γ2|d |2-|z |2}（?d）（14）

則魯棒條件 H ≤γ成立。

為了使得膠囊機器人控制系統(tǒng)滿足魯棒條件，將設計控制律為：

2.2 控制系統(tǒng)的穩(wěn)定性分析

前文所述，通過 HJI 理論設計了一個滑模控制律，為驗證其控制系統(tǒng)的穩(wěn)定性，利用李雅普諾夫定律來分析系統(tǒng)的穩(wěn)定性及收斂性，定義李雅普諾夫函數(shù)：

對上式求導，則：

定義：

將式（17）代入式（18）中，可以得到：

則有 I ≤0，即，滿足 Hamilton-Jacobi不等式理論以及李雅普諾夫穩(wěn)定性的理論條件。因此，所設計的控制系統(tǒng)是有效的。

3 系統(tǒng)仿真

本文雙半球膠囊基于 HJI理論的非線性控制系統(tǒng)如圖3所示，根據(jù)動力學模型進行運動仿真實驗，以驗證所提出的自適應跟蹤控制方案的性能和效率。

在仿真中，系統(tǒng)參數(shù)配置如表1所示，給定俯仰以及側擺運動為正弦信號，設計了以下仿真實驗。

控制器中狀態(tài)變量的初始值為0，滑膜控制器的控制參數(shù)Λ=diag （2020），γ=0.05，采樣時間為0.001。

仿真結果如圖4所示，結果顯示了實際軌跡角度能夠穩(wěn)定跟蹤預期軌跡角度，實線是 DHCR的姿態(tài)角和側擺角的預期軌跡，虛線是膠囊機器人的姿態(tài)角和側擺角的跟蹤軌跡。

從圖5所示的仿真結果可以看出，在不確定性以及干擾存在的條件下，機器人的姿態(tài)角度跟蹤誤差小于±2%，充分證明所提方法的有效性。

4 實驗驗證

實驗平臺包括雙半球膠囊機器人和控制系統(tǒng)，如圖6所示，控制系統(tǒng)由 PC和一臺 DSP控制器組成，PC主要負責控制算法指令的生成，并將所需的電流通過串行端口傳輸?shù)?DSP控制器，通過更改電流的幅值來改變磁矢量的方向，從而實現(xiàn) DHCR的姿態(tài)調整。機器人配置了攝像頭、 LED 光源、圖像無線發(fā)射模塊和高能量電池等集成硬件，不僅能夠保證膠囊在胃腸道中穩(wěn)定驅動，還能將膠囊在胃腸道中的圖像以視頻流的方式無線傳輸?shù)絇C 機上。

為了進一步驗證 HJI 理論滑?？刂葡到y(tǒng)的有效性，設計了相應實驗來測試控制系統(tǒng)的性能。將機器人放置在人體腸道模型中，腸道模型上部鋪設豬大腸，用來模擬真實腸道粘彈性環(huán)境，并將模型放在三軸亥姆霍茨線圈當中，通過 SURMF調整旋轉磁矢量方向驅動機器人進行姿態(tài)調整。在實驗過程中，分別對膠囊機器人的俯仰和側擺的姿態(tài)位置進行了測試。

采用激光檢測方法得到機器人的姿態(tài)信息，對不同位置的姿態(tài)數(shù)據(jù)進行記錄，從圖7所示的實驗數(shù)據(jù)可以看出，系統(tǒng)的控制誤差仍然存在。然而，由于膠囊機器人采用了更大的廣角（140°）來實現(xiàn)腸胃內部的可視化，該誤差范圍能夠滿足機器人實現(xiàn)胃腸道的全景觀察要求，因此，本文提出的控制策略可行。同時對比仿真和試驗角度跟蹤誤差可知，試驗角度跟蹤依然存在一定誤差，誤差范圍在±5%。該現(xiàn)象的原因，一方面是由于仿真建模時膠囊機器人模型理想化，另一方面是加工出來的膠囊機器人質量分布不均勻導致的，其他更深層次的原因則需要繼續(xù)探究。