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

高繼勛， 黃全振， 高振東， 趙媛媛

(1. 河南工程學(xué)院計(jì)算機(jī)學(xué)院，河南 鄭州 451191; 2. 河南工程學(xué)院電氣信息工程學(xué)院，河南 鄭州 451191;3. 鄭州工程技術(shù)學(xué)院，河南 鄭州 450044)

0 引 言

隨著人工智能迅速發(fā)展，機(jī)器人技術(shù)也隨之成為當(dāng)今研究的熱點(diǎn)[1]。輪式移動(dòng)機(jī)器人作為機(jī)器人的一部分，因其靈活、承載能力大等特點(diǎn)，已被普遍應(yīng)用于軍事、工業(yè)、農(nóng)業(yè)等方面[2-3]。

輪式機(jī)器人作為一個(gè)典型的非線性強(qiáng)耦合控制系統(tǒng)[4]，其運(yùn)動(dòng)控制模型的建模需要從運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)兩方面進(jìn)行綜合分析與設(shè)計(jì)。軌跡跟蹤作為移動(dòng)機(jī)器人運(yùn)動(dòng)控制的核心問題,已有很多研究成果，包括反演控制[5]、反饋線性化[6]、滑?？刂芠7]等，都是基于模型的非線性控制設(shè)計(jì)的軌跡跟蹤控制系統(tǒng)，并且取得了廣泛的應(yīng)用。Kanayama在Lyapunov第二方法的基礎(chǔ)上設(shè)計(jì)了局部穩(wěn)定的軌跡跟蹤控制器。在此基礎(chǔ)上，吳衛(wèi)國[8]利用反演控制法，設(shè)計(jì)了全局漸進(jìn)穩(wěn)定的軌跡跟蹤控制器，但該控制器無法同時(shí)完成軌跡跟蹤與鎮(zhèn)定控制。陳罡[9]在此研究基礎(chǔ)上，通過滑?？刂婆c反演控制相結(jié)合的方式，完成軌跡跟蹤控制。目前，移動(dòng)機(jī)器人的軌跡跟蹤控制以多種控制方法相結(jié)合為發(fā)展趨勢，以期望能獲得更好的穩(wěn)定性和魯棒性。在實(shí)際應(yīng)用過程中，大多數(shù)學(xué)者的研究是在運(yùn)動(dòng)學(xué)基礎(chǔ)上展開[10]，運(yùn)動(dòng)學(xué)模型在面對(duì)高速、重載的情況控制精度無法達(dá)到要求，并且系統(tǒng)穩(wěn)定性無法保證，因此動(dòng)力學(xué)模型的研究對(duì)于移動(dòng)機(jī)器人應(yīng)用具有重要的學(xué)術(shù)意義和工程價(jià)值[11]。

本文基于移動(dòng)機(jī)器人的動(dòng)力學(xué)模型，通過對(duì)模型的解耦和分析，利用反演控制方法設(shè)計(jì)控制律，依據(jù)Lyapunov證明系統(tǒng)的穩(wěn)定和跟蹤誤差的收斂最終完成移動(dòng)機(jī)器人對(duì)理想軌跡速度、角度的跟蹤任務(wù)。

1 模型建立推導(dǎo)

本文以兩輪差速移動(dòng)機(jī)器人為研究對(duì)象，且在二維平面內(nèi)運(yùn)動(dòng)時(shí)只有滾動(dòng)、無滑動(dòng)。移動(dòng)機(jī)器人的模型如圖1所示。

圖1 兩輪差速移動(dòng)機(jī)器人模型

2l為車身寬度，r為輪胎半徑，P為移動(dòng)機(jī)器人的質(zhì)心。為航向角，的方向?yàn)橐苿?dòng)機(jī)器人線速度方向，始終垂直于車輪之間連線，、為左輪和右輪轉(zhuǎn)角，速度矢量可表示為。由文獻(xiàn)[12]可知，在o-xy絕對(duì)坐標(biāo)系內(nèi)，可用動(dòng)力學(xué)方程描述移動(dòng)機(jī)器人動(dòng)態(tài)特性。

根據(jù)力矩平衡原理和牛頓定律，對(duì)移動(dòng)機(jī)器人及車輪進(jìn)行分析。

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

反演控制的設(shè)計(jì)思路是從一個(gè)高階系統(tǒng)開始，在保證系統(tǒng)性能的前提下，將一個(gè)非線性系統(tǒng)分解為相應(yīng)的低階子系統(tǒng)，然后在子系統(tǒng)中設(shè)計(jì)相應(yīng)的控制量。本文通過反演控制方法，分別對(duì)線速度和角度進(jìn)行控制律設(shè)計(jì)。

定義Lyapunov函數(shù)V0：

3 仿真實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證控制算法的有效性，分別使用計(jì)算機(jī)仿真軟件Matlab數(shù)值仿真和硬件平臺(tái)構(gòu)建模擬/物理實(shí)驗(yàn)系統(tǒng)進(jìn)行驗(yàn)證。

3.1 數(shù)值仿真

系統(tǒng)狀態(tài)方程如式(12)所示，選取控制對(duì)象為式 (13)、(14)，選取系統(tǒng)參數(shù)從而可得到系統(tǒng)參數(shù)

取參考軌跡，理想線速度vd=1.0 m/s, 理想角度φd=sintrad 。系統(tǒng)參數(shù)k1=k2=k3=10，仿真結(jié)果如圖2、圖3所示。

圖2 線速度跟蹤和角度跟蹤曲線

圖3 仿真軌跡曲線

圖2中，紅色實(shí)線分別表示理想線速度和角度，藍(lán)色實(shí)線分別表示實(shí)際線速度和角度。圖3為仿真軌跡曲線。由圖2可以知，在一定時(shí)間內(nèi)，實(shí)際線速度與角度逐漸趨近于理想線速度和角度，并最終保持一致，完成跟蹤任務(wù)。系統(tǒng)在控制律約束下，仿真軌跡如圖3所示。

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

硬件實(shí)驗(yàn)平臺(tái)部分主要由雙輪差速驅(qū)動(dòng)底盤Turtlebot2、IMU模塊、GPS差分定位模塊(RTK)和主計(jì)算機(jī)（Jetson Nano）構(gòu)成，如圖4、圖5所示。

圖4 實(shí)驗(yàn)平臺(tái)實(shí)物圖

圖5 實(shí)驗(yàn)平臺(tái)示意圖

Turtlebot2底盤包括ST微控制器，霍爾編碼器(500線)和兩個(gè)直流電機(jī)，電機(jī)通過微控制器的兩個(gè)脈寬調(diào)制通道獨(dú)立控制，同時(shí)為左、右電機(jī)產(chǎn)生控制信號(hào)。IMU模塊為九軸姿態(tài)傳感器，可完成對(duì)移動(dòng)機(jī)器人航向角、加速度數(shù)據(jù)的采集，采樣頻率為20 Hz。GPS差分定位模塊(RTK)定位準(zhǔn)確度為5 cm，采樣頻率為5 Hz，并且通過主計(jì)算機(jī)控制程序?qū)崟r(shí)完成位置記錄。主計(jì)算機(jī)（Jetson Nano）運(yùn)行Ubuntu操作系統(tǒng)，通過串口與微控制器通信，包含左、右電機(jī)速度的信息、移動(dòng)機(jī)器人姿態(tài)信息和定位信息。

主計(jì)算機(jī)通過傳感器RTK和IMU完成對(duì)移動(dòng)機(jī)器人位姿數(shù)據(jù)的采集?？刂葡到y(tǒng)通過傳感器采集到的數(shù)據(jù)，對(duì)移動(dòng)機(jī)器人下一時(shí)刻運(yùn)動(dòng)狀態(tài)進(jìn)行計(jì)算，之后輸出對(duì)應(yīng)控制參數(shù)給電機(jī)驅(qū)動(dòng)，完成對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)狀態(tài)的控制，流程圖如圖6所示。

圖6 移動(dòng)機(jī)器人系統(tǒng)控制流程圖

首先，使用移動(dòng)機(jī)器人完成對(duì)期望軌跡數(shù)據(jù)的采集，離線處理后保存到移動(dòng)機(jī)器人主計(jì)算機(jī)。實(shí)驗(yàn)過程中，移動(dòng)機(jī)器人通過對(duì)期望軌跡與當(dāng)前位置誤差的計(jì)算，得到所需要的控制輸入。在移動(dòng)機(jī)器人運(yùn)動(dòng)的同時(shí)，主計(jì)算機(jī)會(huì)實(shí)時(shí)將數(shù)據(jù)保存在本地。由于Nano(2 GB內(nèi)存)硬件內(nèi)存不足的原因無法完成位姿在線顯示的任務(wù)，因此只能通過離線的方式對(duì)保存在本地的數(shù)據(jù)進(jìn)行處理。

圖7 動(dòng)力學(xué)模型實(shí)驗(yàn)軌跡跟蹤曲線

圖8 動(dòng)力學(xué)模型位置誤差曲線

圖9 動(dòng)力學(xué)模型速度曲線

由圖7可知，紅色實(shí)線為理想軌跡，藍(lán)色實(shí)線為實(shí)際軌跡，在一定時(shí)間內(nèi)，移動(dòng)機(jī)器人可完成對(duì)理想軌跡的跟蹤任務(wù)。圖8為移動(dòng)機(jī)器人實(shí)際運(yùn)動(dòng)軌跡與理想軌跡隨時(shí)間變化的誤差曲線，誤差由初始2 m逐漸減小，最終穩(wěn)定在0.2 m左右。

為驗(yàn)證本文方法的有效性，使用文獻(xiàn) [12]中以運(yùn)動(dòng)學(xué)模型為對(duì)象的控制方法。同樣選取軌跡為半徑的圓，令理想線速度，理想角度。移動(dòng)機(jī)器人初始位置為[7 10.5]。實(shí)驗(yàn)結(jié)果如圖10、圖11、圖12所示。

圖10 運(yùn)動(dòng)學(xué)模型軌跡跟蹤曲線

圖11 運(yùn)動(dòng)學(xué)模型位置誤差曲線

圖12 運(yùn)動(dòng)學(xué)模型速度曲線

在圖10中，紅色實(shí)線為理想軌跡，藍(lán)色實(shí)線為實(shí)際軌跡。圖11為移動(dòng)機(jī)器人實(shí)際運(yùn)動(dòng)軌跡與理想軌跡隨時(shí)間變化的誤差曲線，最終穩(wěn)定在左右。圖9所示為移動(dòng)機(jī)器人速度曲線。

將圖7、圖8與圖10、圖11對(duì)比可知，動(dòng)力學(xué)模型下，移動(dòng)機(jī)器人誤差收斂迅速，并且達(dá)到收斂速度后較為穩(wěn)定、誤差較小，同時(shí)運(yùn)動(dòng)軌跡較為平滑。對(duì)比圖9與圖12，動(dòng)力學(xué)模型下，移動(dòng)機(jī)器人響應(yīng)速度更快，并且控制精度更高；運(yùn)動(dòng)學(xué)模型下，速度曲線波動(dòng)較大。

以相同實(shí)驗(yàn)對(duì)象為前提，對(duì)比不同控制模型的實(shí)驗(yàn)結(jié)果，動(dòng)力學(xué)模型下的移動(dòng)機(jī)器人跟蹤誤差由0.5 m減小到0.2 m左右，提高了控制精度和準(zhǔn)確度更高，同時(shí)減小了結(jié)構(gòu)非線性特性的影響，使系統(tǒng)具有更好的穩(wěn)定性和魯棒性。

4 結(jié)束語

本文根據(jù)兩輪差速移動(dòng)機(jī)器人模型的運(yùn)動(dòng)特征和受力情況，建立動(dòng)力學(xué)模型。針對(duì)實(shí)際應(yīng)用問題，對(duì)模型進(jìn)行解耦和分析，利用反演控制方法將控制系統(tǒng)分解為線速度控制和角度控制，分別對(duì)移動(dòng)機(jī)器人的速度和角度跟蹤進(jìn)行控制律設(shè)計(jì)，并利用Lyapunov穩(wěn)定性理論證明系統(tǒng)的穩(wěn)定。

通過數(shù)值仿真算例，驗(yàn)證了該系統(tǒng)的有效性與穩(wěn)定性，并且能夠保證速度和角度跟蹤的精度、收斂速度。最后通過實(shí)驗(yàn)，表明了該控制方法在實(shí)際應(yīng)用中的可行性，為以后的研究工作提供了一定的參考和借鑒。