基于 HJI理論的雙半球膠囊機器人姿態非線性控制方法

劉冠喜 張永順

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

關鍵詞：雙半球膠囊機器人;拉格朗日動力學;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 引言

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

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

1 雙半球型膠囊機器人的系統原理

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

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

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

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

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

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

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

式中：L 為機器人被動模態時系統勢能和動能之和;qk為系統的廣義坐標;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），得到雙半球膠囊被動模態動力學模型：

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

式中： T 為輸入控制;df為系統外界干擾; Δ（ 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 控制器設計

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

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

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

設計控制律為：

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

式中：μ為反饋控制律。

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

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

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

r =e? +Λe （10）

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

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

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

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

為了表示系統的抗干擾能力，定義性能指標為：

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

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

則魯棒條件 H ≤γ成立。

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

2.2 控制系統的穩定性分析

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

對上式求導，則：

定義：

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

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

3 系統仿真

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

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

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

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

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

4 實驗驗證

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

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

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