劉冠喜 張永順

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

關(guān)鍵詞：雙半球膠囊機(jī)器人;拉格朗日動力學(xué);HJI理論;滑模魯棒控制

中圖分類號：TP242文獻(xiàn)標(biāo)志碼：A

文章編號：1009-9492（2021）11-0026-04開放科學(xué)（資源服務(wù)）標(biāo)識碼（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 引言

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

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

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

1.1 雙半球膠囊機(jī)器人主要結(jié)構(gòu)

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

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

1.2 雙半球型膠囊機(jī)器人的動力學(xué)模型

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

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

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

式中：L 為機(jī)器人被動模態(tài)時系統(tǒng)勢能和動能之和;qk為系統(tǒng)的廣義坐標(biāo);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)動力學(xué)模型：

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

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

慣性矩陣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 分別為雙半球膠囊機(jī)器人在xyz坐標(biāo)系3個軸向上的主轉(zhuǎn)動慣量。

2 基于 HJI理論的控制器設(shè)計(jì)

2.1 控制器設(shè)計(jì)

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

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

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

設(shè)計(jì)控制律為：

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

式中：μ為反饋控制律。

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

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

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

r =e? +Λe （10）

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

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

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

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

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

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

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

則魯棒條件 H ≤γ成立。

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

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

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

對上式求導(dǎo)，則：

定義：

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

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

3 系統(tǒng)仿真

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

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

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

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