基于HJI的移動(dòng)機(jī)器人軌跡跟蹤控制

劉 鑫，陳昌忠，王 蓉，唐榮嘉

（1.四川輕化工大學(xué)自動(dòng)化與信息工程學(xué)院，四川 宜賓 644000；2.人工智能四川省重點(diǎn)實(shí)驗(yàn)室，四川 宜賓 644000）

引 言

移動(dòng)機(jī)器人由于其靈活性高和實(shí)用性強(qiáng)，現(xiàn)已廣泛應(yīng)用于工業(yè)領(lǐng)域和社會(huì)場(chǎng)所[1]。但是，由于其本身具有非完整約束、強(qiáng)非線性、強(qiáng)耦合性等不確定因素，要對(duì)其進(jìn)行良好的控制具有一定困難。文獻(xiàn)[2]對(duì)跟蹤誤差進(jìn)行泰勒處理，利用Lyapunov 理論設(shè)計(jì)了具有局部漸進(jìn)穩(wěn)定的跟蹤控制器。文獻(xiàn)[3]采用動(dòng)態(tài)線性化反饋，提出了具有指數(shù)收斂的全局跟蹤控制器。雖然以上研究實(shí)現(xiàn)了軌跡跟蹤，但是忽略了非線性因素的影響。文獻(xiàn)[4]利用反演法設(shè)計(jì)了具有全局指數(shù)漸進(jìn)穩(wěn)定的跟蹤控制器。針對(duì)傳統(tǒng)滑?？刂频娜秉c(diǎn)，文獻(xiàn)[5]將全局穩(wěn)定定理與指數(shù)趨近率相結(jié)合，文獻(xiàn)[6]將模糊控制與滑?？刂七M(jìn)行結(jié)合，文獻(xiàn)[7]通過(guò)自適應(yīng)技術(shù)與滑?？刂葡嘟Y(jié)合，均有效地減少了滑模抖振。文獻(xiàn)[8]針對(duì)重復(fù)運(yùn)動(dòng)的移動(dòng)機(jī)器人系統(tǒng)，提出了一種基于數(shù)據(jù)驅(qū)動(dòng)的高階迭代學(xué)習(xí)控制算法。文獻(xiàn)[9]針對(duì)移動(dòng)機(jī)器人左右輪驅(qū)動(dòng)控制系統(tǒng)，把雙輸入雙輸出控制系統(tǒng)解耦成單輸入單輸出的控制系統(tǒng)，并對(duì)單輸入單輸出系統(tǒng)分別設(shè)計(jì)控制器。以上控制方法雖然都能夠?qū)σ苿?dòng)機(jī)器人進(jìn)行良好的軌跡跟蹤控制，但是并未考慮移動(dòng)機(jī)器人的動(dòng)力學(xué)模型。文獻(xiàn)[10]結(jié)合動(dòng)力學(xué)模型提出了基于慣性檢測(cè)單元的跟蹤控制方法。在具有外部擾動(dòng)的情況下，文獻(xiàn)[11]設(shè)計(jì)了具有干擾觀測(cè)器補(bǔ)償?shù)牧乜刂破鳎墨I(xiàn)[12]設(shè)計(jì)了一種直接模糊自適應(yīng)輸出反饋控制器，都取得了良好的跟蹤效果。

本文針對(duì)動(dòng)力學(xué)具有外部擾動(dòng)的移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，采用反步法設(shè)計(jì)了輔助速度控制率；針對(duì)其動(dòng)力學(xué)模型，基于哈密頓-雅可比不等式（Hamilton-Jacobi Inequality，HJI）設(shè)計(jì)了前饋控制和反饋控制相結(jié)合的控制方法，最后利用MATLAB將計(jì)算力矩控制與本文提出的控制方法進(jìn)行數(shù)值仿真對(duì)比。

1 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)及其模型

考慮一類(lèi)受非完整約束的三輪移動(dòng)機(jī)器人，它有兩個(gè)驅(qū)動(dòng)輪和一個(gè)萬(wàn)向輪，如圖1 所示。萬(wàn)向輪無(wú)動(dòng)力裝置，僅起支撐作用。兩驅(qū)動(dòng)輪軸距為2a，驅(qū)動(dòng)輪半徑為r，S為兩驅(qū)動(dòng)輪的軸心。XOY表示笛卡爾坐標(biāo)系，X0SY0表示機(jī)器人坐標(biāo)系，用向量q=[x y θ]T，V=[v ω]T來(lái)表示移動(dòng)機(jī)器人的狀態(tài)，[x y]表示移動(dòng)機(jī)器人在笛卡爾坐標(biāo)系中的位置，θ表示其運(yùn)動(dòng)方向與笛卡爾坐標(biāo)系X軸正方向的夾角。v和ω分別表示移動(dòng)機(jī)器人的線速度和角速度。

圖1 移動(dòng)機(jī)器人結(jié)構(gòu)示意圖

移動(dòng)機(jī)器人在純滾動(dòng)、無(wú)滑動(dòng)條件下的非完整約束[13]為：

將其改寫(xiě)成如下形式：

選擇一滿秩轉(zhuǎn)換矩陣S(q)，滿足

通過(guò)矩陣S(q)把機(jī)器人坐標(biāo)系下的速度V轉(zhuǎn)換為笛卡爾坐標(biāo)系下的速度q?，即

則移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程為：

移動(dòng)機(jī)器人動(dòng)力學(xué)方程[14]通常可以表示為：

其 中：M(q)∈R3×3為 系 統(tǒng) 正 定 慣 性 矩 陣；C(q,q? )∈R3×3為 與 位 置 和 速 度 有 關(guān) 的 矩 陣；G(q)∈R3×1為重力矩陣；F(q? )∈R3×1為摩擦力項(xiàng)；τd∈R3×1為有界的未知擾動(dòng)；B(q)∈R3×2為輸入變換矩陣；τ∈R2×1為控制力矩；A(q)為約束矩陣；λ為約束力。

對(duì)式（4）進(jìn)行求導(dǎo)得：

將式（4）和式（7）代入移動(dòng)機(jī)器人的動(dòng)力學(xué)方程式（6），左乘ST( )q，整理后得：

其中：

假設(shè)機(jī)器人在水平面上運(yùn)動(dòng)，且不考慮摩擦因素。即G(q)= 0，F(xiàn)(q? )= 0，則式（8）可寫(xiě)為：

式（10）為變形后的移動(dòng)機(jī)器人一階動(dòng)力學(xué)模型，根據(jù)文獻(xiàn)[16]，可知其具有以下性質(zhì)：

性質(zhì)1：對(duì)于任意的q，參數(shù)矩陣( )q為對(duì)稱(chēng)矩陣。

2 哈密頓-雅可比不等式理論

在模有界條件下，非線性不確定奇異系統(tǒng)的魯棒可鎮(zhèn)定性與一個(gè)擴(kuò)展的HJI理論微分不等式存在正解等價(jià)[17]。本文基于HJI 來(lái)設(shè)計(jì)動(dòng)力學(xué)反饋控制器，并用HJI 理論證明動(dòng)力學(xué)系統(tǒng)的穩(wěn)定性。假設(shè)一非線性系統(tǒng)如下：

其中：d為外界干擾，z為系統(tǒng)的評(píng)判指標(biāo)。

為了評(píng)判系統(tǒng)的抗干擾能力，定義如下性能指標(biāo)：

其中J為系統(tǒng)的L2增益，它與系統(tǒng)的魯棒性負(fù)相關(guān)。J越小，系統(tǒng)的魯棒性能越好。

根據(jù)文獻(xiàn)[18]，HJI 理論描述為：對(duì)任意給定一個(gè)正數(shù)γ，如果存在一個(gè)正定且可微的函數(shù)L(x)≥0，并且

則J≤γ。

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

為了實(shí)現(xiàn)移動(dòng)機(jī)器人對(duì)參考軌跡的跟蹤，設(shè)計(jì)如下控制方案：

（1）為了實(shí)現(xiàn)移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)位置跟蹤，基于反步法設(shè)計(jì)運(yùn)動(dòng)學(xué)輔助速度控制器。

（2）為了實(shí)現(xiàn)移動(dòng)機(jī)器人動(dòng)力學(xué)速度跟蹤，基于HJI設(shè)計(jì)動(dòng)力學(xué)前饋控制器和反饋控制器。

3.1 運(yùn)動(dòng)學(xué)控制器設(shè)計(jì)

假設(shè)移動(dòng)機(jī)器人在笛卡爾坐標(biāo)系中的參考位姿為qr=[xr yr θr]T，根據(jù)參考機(jī)器人和移動(dòng)機(jī)器人在笛卡爾坐標(biāo)系中的位置關(guān)系，可以得到位姿跟蹤誤差：

其中：ex為移動(dòng)機(jī)器人坐標(biāo)系X0方向的偏差；ey為移動(dòng)機(jī)器人坐標(biāo)系Y0方向的偏差；eθ為移動(dòng)機(jī)器人轉(zhuǎn)向角的偏差。

對(duì)式（14）求導(dǎo)，得跟蹤誤差變化率為：

其中：vr≥0、ωr≥0 分別為給定的線速度和角速度。參照文獻(xiàn)[19-20]，采用反步法設(shè)計(jì)輔助速度控制率為：

其中：β1、β2、β3均為大于零的可調(diào)節(jié)的控制器參數(shù)。

3.2 動(dòng)力學(xué)控制器設(shè)計(jì)

定義速度跟蹤誤差：

根據(jù)式（10），設(shè)計(jì)前饋控制率為：

其中u為反饋控制率。

將式（18）代入式（10）得動(dòng)力學(xué)誤差模型：

設(shè)動(dòng)力學(xué)系統(tǒng)的評(píng)判指標(biāo)z=ec，則式（19）可寫(xiě)為：

為了保證動(dòng)力學(xué)控制系統(tǒng)的穩(wěn)定，基于HJI 理論設(shè)計(jì)反饋控制器為：

根據(jù)以上分析，設(shè)計(jì)出移動(dòng)機(jī)器人的控制結(jié)構(gòu)如圖2所示。

圖2 移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)系統(tǒng)控制結(jié)構(gòu)圖

4 系統(tǒng)穩(wěn)定性

定義移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)系統(tǒng)和動(dòng)力學(xué)系統(tǒng)的Lyapunov函數(shù)分別為L(zhǎng)1和L2：

顯然，L1≥0，L2≥0。只有當(dāng)e1、e2、e3都為0 時(shí)，L1= 0；當(dāng)ec= 0時(shí)，L2= 0。

對(duì)L1求導(dǎo)，結(jié)合式（14）～（15）可得：

顯然，L?1≤0。

對(duì)L2進(jìn)行求導(dǎo)，可得：

結(jié)合式（19）可得：

定義

將式（26）代入式（27）可得：

結(jié)合式（27）可知：

由HJI 理論可知，動(dòng)力學(xué)系統(tǒng)的L2增益J≤γ，結(jié)合式（12）可知‖ ‖z滿足性能指標(biāo)，即速度跟蹤誤差ec收斂，動(dòng)力學(xué)系統(tǒng)穩(wěn)定，且γ越小，誤差ec越小，系統(tǒng)的魯棒性越好。

5 仿真驗(yàn)證

采用MATLAB 軟件，以圖1所示的機(jī)器人為例，運(yùn)動(dòng)學(xué)控制系統(tǒng)采用相同的控制器，且動(dòng)力學(xué)控制系統(tǒng)采用計(jì)算力矩控制法與本文所設(shè)計(jì)的控制方法進(jìn)行對(duì)比仿真實(shí)驗(yàn)。

移動(dòng)機(jī)器人的質(zhì)量m為10 kg，轉(zhuǎn)動(dòng)慣量I為5 kg·m2，軸距2a為0.40 m，驅(qū)動(dòng)輪半徑r為0.05 m，設(shè)參考軌跡為圓心在原點(diǎn)、半徑為2 m 的圓，參考速度vr= 1 m/s，ωr= 0.5 rad/s。參考機(jī)器人的初始位姿為qr0=[2 0 π/2]T，受控移動(dòng)機(jī)器人的初始位姿為q0=[1 0.5 π/6]T。選擇輔助速度控制器參數(shù)β1= 4.0、β2= 6.0、β3= 5.5，根據(jù)文獻(xiàn)[21]，采用計(jì)算力矩控制法設(shè)計(jì)力矩控制器為:

其中kp為2 × 2的參數(shù)矩陣，選取kp=。選取本文所設(shè)計(jì)的力矩控制器參數(shù)γ= 0.06。仿真時(shí)，設(shè)外界不確定干擾為采樣時(shí)間（0.02 s）的隨機(jī)高斯白噪聲，如圖3所示。

圖3 外界白噪聲干擾曲線

圖4(a)所示為移動(dòng)機(jī)器人在兩種方法下的軌跡跟蹤仿真曲線圖，圖4(b)所示為兩種方法下的位姿跟蹤誤差曲線圖。由圖4 可知，傳統(tǒng)的計(jì)算力矩控制下的移動(dòng)機(jī)器人在5 s以后逐漸跟蹤上參考軌跡，但是軌跡跟蹤誤差始終不能收斂到0，存在抖動(dòng)。而在本文方法下的移動(dòng)機(jī)器人在2 s 以后便能夠逐漸跟蹤上參考軌跡，并且軌跡跟蹤誤差幾乎為0。這說(shuō)明本文所設(shè)計(jì)的控制方法控制性能好，系統(tǒng)具有良好的抗擾性。

圖4 不同方法下軌跡跟蹤仿真與位姿跟蹤誤差曲線

圖5所示為采用計(jì)算力矩控制法和采用本文控制方法時(shí)，移動(dòng)機(jī)器人動(dòng)力學(xué)系統(tǒng)實(shí)際速度輸出對(duì)輔助速度控制輸入的跟蹤曲線圖。由圖5(a)可知，計(jì)算力矩控制下的移動(dòng)機(jī)器人的線速度和角速度在將近5 s時(shí)跟蹤上運(yùn)動(dòng)學(xué)輔助速度輸入，跟蹤效果較差；而圖5(b)中本文方法下的移動(dòng)機(jī)器人的線速度和角速度在0.5 s以?xún)?nèi)就能跟蹤上運(yùn)動(dòng)學(xué)輔助速度輸入，跟蹤效果良好。圖6 所示為兩種方法的速度跟蹤誤差圖。由圖6 可以看出，采用本文的控制方法時(shí)，移動(dòng)機(jī)器人速度跟蹤誤差的收斂速度很快，能夠在0.5 s之前迅速收斂到幾乎為0。而采用計(jì)算力矩控制時(shí)移動(dòng)機(jī)器人的速度跟蹤誤差在5 s 左右收斂，且收斂誤差始終在0 附近來(lái)回抖動(dòng)。根據(jù)仿真的結(jié)果可知，計(jì)算力矩控制由于抗干擾能力弱，在面對(duì)外界干擾時(shí)，移動(dòng)機(jī)器人的跟蹤性能差。而本文基于HJI理論所設(shè)計(jì)前饋控制加反饋控制的方法，在面對(duì)外界干擾時(shí)，移動(dòng)機(jī)器人能夠在極短的時(shí)間內(nèi)實(shí)現(xiàn)速度的完美跟蹤，且誤差幾乎無(wú)抖動(dòng)。這說(shuō)明本文所設(shè)計(jì)的動(dòng)力學(xué)控制方法具有良好的跟蹤性能，控制系統(tǒng)具有良好的抗擾性。

圖5 不同方法下速度跟蹤曲線

圖6 速度跟蹤誤差曲線

圖7 所示為本文方法下的力矩控制器輸出的左、右力矩曲線圖。由圖7 可知，移動(dòng)機(jī)器人左、右輪控制力矩輸出均在2 s之后逐漸穩(wěn)定。

圖7 左右輪控制力矩曲線

6 結(jié)束語(yǔ)

通過(guò)考慮一類(lèi)受外界干擾的非完整移動(dòng)機(jī)器人，針對(duì)其運(yùn)動(dòng)學(xué)模型基于反步法設(shè)計(jì)了運(yùn)動(dòng)學(xué)控制器。針對(duì)其動(dòng)力學(xué)模型設(shè)計(jì)了前饋控制器，并把動(dòng)力學(xué)誤差模型轉(zhuǎn)換為HJI 所需的非線性模型，在此基礎(chǔ)上設(shè)計(jì)了反饋控制器。該控制方法設(shè)計(jì)簡(jiǎn)單，需調(diào)節(jié)的參數(shù)少。仿真結(jié)果證明本文所提出的控制方法是有效的，在具有外部不確定干擾的情況下，移動(dòng)機(jī)器人能夠快速、準(zhǔn)確、穩(wěn)定地跟蹤上參考軌跡，控制系統(tǒng)具有良好的抗擾性。