基于 HJI理論的雙半球膠囊機(jī)器人姿態(tài)非線性控制方法

劉冠喜 張永順

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

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

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

文章編號(hào)：1009-9492（2021）11-0026-04開放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（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ū)動(dòng)方式，膠囊內(nèi)窺鏡可分為被動(dòng)式和主動(dòng)式膠囊內(nèi)窺鏡。主動(dòng)式膠囊內(nèi)窺鏡也稱為膠囊機(jī)器人，主要包括電驅(qū)動(dòng)式和磁驅(qū)動(dòng)式膠囊機(jī)器人。由于磁驅(qū)動(dòng)膠囊機(jī)器人可實(shí)現(xiàn)無纜驅(qū)動(dòng)，并避免了復(fù)雜的行走機(jī)構(gòu)，因此，磁驅(qū)動(dòng)膠囊機(jī)器人具有良好的應(yīng)用前景，并迅速成為國內(nèi)外的研究熱點(diǎn)。大連理工大學(xué)膠囊機(jī)器人課題組提出了一種雙半球型膠囊機(jī)器人，在空間萬向旋轉(zhuǎn)磁場(chǎng)驅(qū)動(dòng)隨動(dòng)效應(yīng)驅(qū)動(dòng)下，在腸胃寬裕環(huán)境下可以實(shí)現(xiàn)主動(dòng)模態(tài)下的滾動(dòng)行走和被動(dòng)模態(tài)下的定點(diǎn)調(diào)姿[5]，通過滾動(dòng)行走接近可疑目標(biāo)，通過定點(diǎn)調(diào)姿實(shí)現(xiàn)對(duì)可疑目標(biāo)的全景觀察與篩查?？臻g萬向旋轉(zhuǎn)磁場(chǎng)是在三軸正交嵌套方形亥姆霍茲線圈中通入滿足一定關(guān)系的三相交變電流，在三軸亥姆霍茲線圈的內(nèi)部空間疊加產(chǎn)生的，能夠消除磁力，解決磁力與磁矩相互耦合的問題，從而通過力矩的隨動(dòng)效應(yīng)實(shí)現(xiàn)雙半球型膠囊機(jī)器人的精確驅(qū)動(dòng)[6-7]。與一般典型球形機(jī)器人采用配重塊實(shí)現(xiàn)姿態(tài)穩(wěn)定控制方式不同[8]，雙半球膠囊依賴于磁場(chǎng)隨動(dòng)效應(yīng)實(shí)現(xiàn)姿態(tài)的穩(wěn)定控制，即雙半球膠囊軸線與旋轉(zhuǎn)磁場(chǎng)軸線具有跟隨特性，可通過旋轉(zhuǎn)磁場(chǎng)控制膠囊軸線在空間的位置，進(jìn)而實(shí)現(xiàn)膠囊主動(dòng)模態(tài)下滾動(dòng)行進(jìn)和被動(dòng)模態(tài)下定點(diǎn)調(diào)姿兩種功能。

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

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

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

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

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

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

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

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

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

式中：L 為機(jī)器人被動(dòng)模態(tài)時(shí)系統(tǒng)勢(shì)能和動(dòng)能之和;qk為系統(tǒng)的廣義坐標(biāo);Qk為廣義力。

其動(dòng)能可表示為：

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），得到雙半球膠囊被動(dòng)模態(tài)動(dòng)力學(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個(gè)軸向上的主轉(zhuǎn)動(dòng)慣量。

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

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

雙半球型膠囊機(jī)器人控制系統(tǒng)的設(shè)計(jì)十分重要。膠囊在運(yùn)行過程中，由于存在不確定性因素以及外界干擾，因此，精確建模困難。為了減少模型不確定性對(duì)系統(tǒng)的影響， 本文設(shè)計(jì)了一個(gè)基于 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ī)器人動(dòng)力學(xué)方程，滑模構(gòu)控制器滑模表面定義為：

r =e? +Λe （10）

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

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

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

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

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

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

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

則魯棒條件 H ≤γ成立。

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

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

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

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

定義：

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

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

3 系統(tǒng)仿真

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

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

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

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

采用激光檢測(cè)方法得到機(jī)器人的姿態(tài)信息，對(duì)不同位置的姿態(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)胃腸道的全景觀察要求，因此，本文提出的控制策略可行。同時(shí)對(duì)比仿真和試驗(yàn)角度跟蹤誤差可知，試驗(yàn)角度跟蹤依然存在一定誤差，誤差范圍在±5%。該現(xiàn)象的原因，一方面是由于仿真建模時(shí)膠囊機(jī)器人模型理想化，另一方面是加工出來的膠囊機(jī)器人質(zhì)量分布不均勻?qū)е碌?，其他更深層次的原因則需要繼續(xù)探究。