火災(zāi)預(yù)警巡檢機(jī)器人軌跡跟蹤控制*

2022-01-18 06:30:44車紅娟高學(xué)山郝亮超牛軍道

廣西科學(xué)院學(xué)報(bào) 2021年4期

車紅娟，高學(xué)山,2**，趙鵬，郝亮超，牛軍道

(1.廣西科技大學(xué)電氣電子與計(jì)算機(jī)科學(xué)學(xué)院，廣西柳州 545006；2.北京理工大學(xué)機(jī)電學(xué)院，北京 100081；3.廣西科技大學(xué)機(jī)械與汽車工程學(xué)院，廣西柳州 545616)

0 引言

在各種災(zāi)害中，火災(zāi)不僅毀壞物質(zhì)財(cái)產(chǎn)，造成社會(huì)秩序混亂，而且還直接或間接危害生命，及時(shí)發(fā)現(xiàn)火情并采取相應(yīng)措施可以有效降低火災(zāi)帶來的損失。火災(zāi)的主要特點(diǎn)有溫度高、煙霧濃、火勢猛和蔓延快。為及時(shí)監(jiān)測火情的發(fā)生，監(jiān)測數(shù)據(jù)的傳感器需要具有很高的靈敏度和準(zhǔn)確性。當(dāng)前應(yīng)用在監(jiān)測火情上的傳感器存在安裝位置固定、測量范圍有限等方面的局限性。將火災(zāi)監(jiān)測傳感器安裝在移動(dòng)機(jī)器人上，可以有效解決上述局限性，實(shí)現(xiàn)火災(zāi)監(jiān)測的無人化與準(zhǔn)確化。

由于存在滑動(dòng)、干擾、傳感器誤差等影響，輪式移動(dòng)機(jī)器人在設(shè)定好的路徑上進(jìn)行巡檢移動(dòng)的過程中，跟蹤誤差不可避免[1]。針對軌跡跟蹤控制問題，國內(nèi)外學(xué)者進(jìn)行大量的研究。Kanayama等[2]基于非完整移動(dòng)機(jī)器人模型對系統(tǒng)的微分方程進(jìn)行線性處理，并針對滑移問題引入速度/加速度限制。曹錦波等[3]基于機(jī)器人因外部擾動(dòng)和建模不準(zhǔn)確等造成的位姿誤差問題，用改進(jìn)型非線性擾動(dòng)觀測器對擾動(dòng)進(jìn)行估計(jì)，并用滑膜控制器對擾動(dòng)進(jìn)行補(bǔ)償。李衛(wèi)兵和吳瓊[4]通過機(jī)器人運(yùn)動(dòng)學(xué)模型，推導(dǎo)軌跡跟蹤誤差表達(dá)式，借助誤差表達(dá)式設(shè)計(jì)虛擬速度控制律，將該控制律作為機(jī)器人動(dòng)力學(xué)模型的輸入，控制機(jī)器人的力矩輸出，達(dá)到實(shí)際軌跡可以很好地跟蹤期望軌跡的預(yù)期。Zhang等[5]針對具有通信約束的非線性多智能體系統(tǒng)，采用分布式控制結(jié)構(gòu)中的“l(fā)eader-follower”模式，設(shè)計(jì)分布式自適應(yīng)反步控制協(xié)同跟蹤運(yùn)動(dòng)目標(biāo)。Yousuf等[6]對一個(gè)多用途機(jī)器人動(dòng)力學(xué)控制器采用Proportion Integral Differential (PID) 和反步控制器結(jié)合的方式，可以有效地限制超調(diào)量并提高瞬態(tài)反應(yīng)時(shí)間。宋立業(yè)、邢飛[7]和尤波等[8]分別將自適應(yīng)神經(jīng)滑膜和模糊滑膜控制引入到非完整移動(dòng)機(jī)器人的軌跡跟蹤上。吳運(yùn)雄和曾碧[9]將卷積神經(jīng)與強(qiáng)化學(xué)習(xí)結(jié)合在一起，解決了傳統(tǒng)算法存在的局部最優(yōu)問題。姜烽等[10]將差分進(jìn)化遺傳算法與PID結(jié)合構(gòu)建軌跡跟蹤控制器，提高了軌跡跟蹤的精度。沈智鵬和張曉玲[11]提出一種解決在跟蹤過程中速度跳變的擾動(dòng)補(bǔ)償模糊自適應(yīng)反演控制方法。崔明月等[12]將打滑程度用未知參數(shù)表示，設(shè)計(jì)一種補(bǔ)償縱向滑差的自適應(yīng)非線性控制律，并提出一種通過極點(diǎn)布置在線調(diào)整控制器增益的方法。目前的研究主要考慮運(yùn)動(dòng)控制算法在仿真方面的改進(jìn)與優(yōu)化，但未真正運(yùn)用到實(shí)際的場景中。為此，本文將從實(shí)際出發(fā)，提出火災(zāi)預(yù)警巡檢機(jī)器人在目標(biāo)軌跡跟蹤過程中的跟蹤控制。首先根據(jù)火災(zāi)預(yù)警巡檢機(jī)器人建立運(yùn)動(dòng)學(xué)模型，并依據(jù)該模型推導(dǎo)出跟蹤誤差方程，建立跟蹤控制器，在控制器中引入虛擬反饋控制量，利用Lyapunov穩(wěn)定性理論判斷控制器的穩(wěn)定性，最后借助仿真實(shí)驗(yàn)和實(shí)物驗(yàn)證算法的有效性。

1 火災(zāi)預(yù)警巡檢機(jī)器人的系統(tǒng)框架

火災(zāi)預(yù)警巡檢機(jī)器人系統(tǒng)主要由3個(gè)部分組成：

①環(huán)境感知。涉及多種傳感器，包括煙霧、火焰、測距和溫度檢測傳感器等，在巡檢過程中實(shí)現(xiàn)對無人化倉庫煙霧和溫度的實(shí)時(shí)監(jiān)測。

②執(zhí)行機(jī)構(gòu)。為了方便后期的機(jī)器人維護(hù)，上層控制器部分與底層執(zhí)行機(jī)構(gòu)部分采用模塊化設(shè)計(jì)，執(zhí)行機(jī)構(gòu)部分采用Arduino作為底層處理器，主要控制機(jī)器人底層電機(jī)的驅(qū)動(dòng)。針對環(huán)境感知系統(tǒng)中存在的導(dǎo)航、傳感器數(shù)據(jù)復(fù)雜和信息量較大等問題，上層控制器的處理器采用樹莓派作為主控。

③通信模塊。傳感器將檢測到的數(shù)據(jù)通過協(xié)議的方式傳遞給樹莓派，執(zhí)行機(jī)構(gòu)部分與樹莓派之間、導(dǎo)航與樹莓派之間采用USB通信方式，為了方便管理者在后端管理平臺(tái)查看機(jī)器人的位置和無人化倉庫的情況，機(jī)器人平臺(tái)與終端采用無線的方式實(shí)現(xiàn)通信。

為了能夠讓機(jī)器人按照一定的路徑進(jìn)行火災(zāi)監(jiān)測，需規(guī)劃出機(jī)器人從此刻位置到某一位置的移動(dòng)路徑，使得機(jī)器人從當(dāng)前位姿到達(dá)目標(biāo)位姿。移動(dòng)機(jī)器人的速度與位置、位姿存在緊密聯(lián)系，要對機(jī)器人進(jìn)行控制，需要對其位姿進(jìn)行準(zhǔn)確表達(dá)。首先建立合適的坐標(biāo)系，對主要信息量進(jìn)行表達(dá)，然后分析各坐標(biāo)系之間存在的位置關(guān)系，最后確定速度與位姿之間的數(shù)學(xué)關(guān)系表達(dá)式?；馂?zāi)預(yù)警巡檢機(jī)器人的系統(tǒng)整體框架如圖1所示。

圖1 火災(zāi)預(yù)警巡檢機(jī)器人系統(tǒng)整體框架Fig.1 Overall framework of fire warning and inspection robot system

1.1 火災(zāi)預(yù)警巡檢機(jī)器人運(yùn)動(dòng)學(xué)模型

圖2 火災(zāi)預(yù)警巡檢機(jī)器人位姿圖Fig.2 Position map of fire warning and inspection robot

圖2所示的巡檢機(jī)器人需滿足純滾動(dòng)和無滑動(dòng)情況，即

(1)

化簡后得到移動(dòng)機(jī)器人的約束方程：

(2)

(3)

其中S(q)是一個(gè)雅可比矩陣，w表示巡檢機(jī)器人的角速度，則運(yùn)動(dòng)學(xué)方程可表示為

(4)

巡檢機(jī)器人平臺(tái)幾何中心P的線速度v和角速度w與左右輪驅(qū)動(dòng)角速度wR和wL的關(guān)系如式(5)和式(6)所示：

(5)

(6)

式(5)、式(6)中r為車輪的半徑，將式(5)、式(6)用矩陣表示，如式(7)所示：

(7)

巡檢機(jī)器人的運(yùn)動(dòng)學(xué)方程改寫為式(8)中：

(8)

1.2 火災(zāi)預(yù)警巡檢機(jī)器人軌跡誤差模型

在軌跡跟蹤過程中，需要將機(jī)器人期望的運(yùn)動(dòng)位姿與實(shí)際的位姿進(jìn)行比較，計(jì)算并修正偏差。在圖3誤差示意圖中，qr表示巡檢機(jī)器人的期望位姿，q表示巡檢機(jī)器人的實(shí)際位姿。

圖3 火災(zāi)預(yù)警巡檢機(jī)器人軌跡跟蹤誤差示意圖Fig.3 Schematic diagram of fire warning and inspection robot trajectory tracking error

由圖3可知，非完整巡檢機(jī)器人系統(tǒng)位姿跟蹤誤差方程如式(9)-(11)：

xe=(xr-x)cosθ+(yr-y)sinθ,

(9)

ye=(yr-y)cosθ-(xr-x)sinθ,

(10)

θe=θr-θ,

(11)

式(9)-(11)中xe為x方向的誤差，ye為y方向的誤差，θe為角度方向的誤差,(xr,yr,θr)代表巡檢機(jī)器人在x軸、y軸、角度方向的期望值。將式(9)-(11)寫成矩陣形式，如式(12)所示：

(12)

下面推導(dǎo)位姿誤差微分方程，對式(9)進(jìn)行求微分，可得式(13)：

(13)

對式(10)進(jìn)行求微分，可得式(14)：

(14)

對式(11)進(jìn)行求微分，可得式(15)：

(15)

對式(13)-(15)進(jìn)行整理，得到非完整巡檢機(jī)器人系統(tǒng)的誤差微分方程：

(16)

根據(jù)位姿誤差的微分方程，即式(16)設(shè)計(jì)合適的控制律，使機(jī)器人在軌跡跟蹤過程中可以按照預(yù)定的軌跡運(yùn)行，或者在出現(xiàn)誤差時(shí)快速消除偏差。軌跡跟蹤控制的目標(biāo)就是設(shè)計(jì)合適的控制器v、w，使得跟蹤誤差趨于0：

2 火災(zāi)預(yù)警巡檢機(jī)器人跟蹤控制器設(shè)計(jì)

機(jī)器人軌跡跟蹤流程如圖4所示。將期望位姿qr和實(shí)際位姿q進(jìn)行對比，作為誤差微分方程的輸入，得到誤差微分方程的輸出，即位姿誤差qe；位姿誤差qe和期望速度(vc,wc)作為控制器的輸入，得到控制器的輸出，即控制速度(v,w)；機(jī)器人的實(shí)際位姿q是將控制速度(v,w)作為運(yùn)動(dòng)學(xué)模型的輸入，再經(jīng)過積分運(yùn)算得到的。

圖4 火災(zāi)預(yù)警巡檢機(jī)器人軌跡跟蹤流程Fig.4 Frajectory tracking process of fire warning and inspection robot

2.1 虛擬反饋量的設(shè)計(jì)

為找到合適的虛擬狀態(tài)反饋函數(shù)xed，使得ye→0。借鑒文獻(xiàn)[13-15]的思想，設(shè)xe為虛擬控制輸入，設(shè)計(jì)控制量v和w使得xe→xed，θe→0，且整個(gè)系統(tǒng)漸進(jìn)穩(wěn)定。定義誤差變量如式(17)：

(17)

“中國風(fēng)”歌曲善于以修辭的運(yùn)用，尤其是“用典”手法的使用來堆砌“中國式”的骨肉鮮活的歌謠。但在使用的程度上態(tài)度有所不同，主觀依賴修辭的一類“中國風(fēng)”歌曲走向或素雅或華美，節(jié)制使用修辭的則走向或瘦削或清虛。

選用一個(gè)局部Lyapunov函數(shù)V1：

(18)

對式(18)進(jìn)行求導(dǎo)：

(19)

在θe→0時(shí)，V1單調(diào)遞減，并且可以使得ye收斂到0。

2.2 基于Lyapunov穩(wěn)定性分析

基于李雅普諾夫穩(wěn)定性準(zhǔn)則，構(gòu)造全局Lyapunov函數(shù)V如式(20)：

(20)

由式(16)和式(17)可得：

(21)

同時(shí)對Lyapunov函數(shù)V求導(dǎo)得式(22)：

(22)

取系統(tǒng)的控制律為式(23)：

(23)

式(23)中，k1，k2，k3，ky均為大于0的數(shù)，則

(24)

3 結(jié)果與分析

為驗(yàn)證本文控制器的控制效果，通過Matlab對機(jī)器人系統(tǒng)進(jìn)行經(jīng)典控制器和本文控制器的仿真對比。經(jīng)典控制器如式(25)所示：

(25)

式(25)中，k11，k22，k33均為增益系數(shù)且大于0。

3.1 直線軌跡跟蹤仿真

仿真的參數(shù)設(shè)定如下，期望的角速度和線速度分別為wr=0 rad/s，vr=0.8 m/s，初始角速度和線速度分別為w=0.3 rad/s，v=0.4 m/s，初始位姿為[1.2 0 π/2]，軌跡跟蹤的參考位姿

qr=(xryrθr)T為

(26)

其中，0≤t≤100；參數(shù)值：k1=1，k2=4.5，k3=4，ky=5，k11=8，k22=3，k33=3。仿真結(jié)果如圖5所示。

從圖5可以看出，機(jī)器人在經(jīng)典控制器與本文控制器在初始值相同的跟蹤過程中，都與目標(biāo)軌跡重合。對比x軸方向的偏差、y軸方向的偏差和航向角θ的偏差可以看出，本文控制器存在較大的波動(dòng)但提高了跟蹤的收斂速度，本文控制器在3 s左右誤差趨于0，經(jīng)典控制器在5 s左右趨于0。

圖5 經(jīng)典控制器(a-c)和本文控制器(d-f)直線軌跡跟蹤仿真圖Fig.5 Simulation diagram of linear trajectory tracking for the classical controller (a-c) and the controller (d-f) in this article

3.2 圓形軌跡跟蹤

仿真的參數(shù)設(shè)定如下，取軌跡跟蹤過程中的期望角速度和線速度分別為wr=1 rad/s，vr=1 m/s，初始角速度和線速度分別為w=0.3 rad/s，v=0.4 m/s。初始位姿為[1.2 0 2π/3]，軌跡跟蹤的參考位姿qr=(xryrθr)T為

(27)

其中，0≤t≤300，跟蹤軌跡為圓心位于原點(diǎn)、半徑為1的圓形；參數(shù)值：k1=1，k2=4，k3=5，ky=4，k11=3，k22=3，k33=2。仿真結(jié)果如圖6所示。

從圖6可以看出，兩種控制器在圓形軌跡跟蹤上可以重合。對比兩種控制器的誤差圖，可以看出本文控制器的誤差波動(dòng)比經(jīng)典控制器的誤差波動(dòng)小，本文控制器角度誤差5 s左右趨于0，經(jīng)典控制器角度誤差8 s左右趨于0。對比兩種控制器的速度曲線，可以看出本文控制器在4 s左右達(dá)到穩(wěn)定狀態(tài)，經(jīng)典控制器在7 s左右達(dá)到穩(wěn)定狀態(tài)。

圖6 經(jīng)典控制器(a-c)和本文控制器(d-f)圓形軌跡跟蹤仿真圖Fig.6 Simulation diagram of circular trajectory tracking for the classical controller (a-c) and the controller (d-f) in this article

3.3 實(shí)物驗(yàn)證

為進(jìn)一步驗(yàn)證軌跡跟蹤控制器的正確性和有效性，進(jìn)行巡檢機(jī)器人的實(shí)物實(shí)驗(yàn)。參考軌跡可以用公式(28)表示：

(28)

實(shí)驗(yàn)環(huán)境包括一臺(tái)巡檢機(jī)器人，一個(gè)OpenMV攝像頭，一臺(tái)PC主機(jī)。PC主機(jī)采用無線形式與巡檢機(jī)器人進(jìn)行通信，巡檢機(jī)器人將自身位置實(shí)時(shí)發(fā)送給PC機(jī)，PC機(jī)通過ROS平臺(tái)將機(jī)器人反饋的數(shù)據(jù)錄制下來并保存成 .txt文件形式，在Matlab中繪制出實(shí)際與期望軌跡。

巡檢機(jī)器人跟蹤軌跡如圖7所示，機(jī)器人從A點(diǎn)(1.5，0.6)沿直線到B點(diǎn)(6.5，1)，從B點(diǎn)沿直線到C點(diǎn)(6.5，4.8)，從C點(diǎn)沿直線到D點(diǎn)(1.5，4.8)，從D點(diǎn)沿直線到A點(diǎn)。圖8為機(jī)器人在運(yùn)動(dòng)過程中的位姿誤差、角速度和線速度控制律，圖9為機(jī)器人的實(shí)物圖。

從圖7可以看出，剛開始機(jī)器人不在設(shè)定的初始位置，在設(shè)計(jì)控制器作用下，經(jīng)過一段時(shí)間逐漸靠近目標(biāo)軌跡。從圖8可以看出，航向角θ的偏差出現(xiàn)3次較大的波動(dòng)，是機(jī)器人在3次方向轉(zhuǎn)換中角度誤差過大導(dǎo)致的；從角速度和線速度的曲線可以看出，機(jī)器人的輸出較為穩(wěn)定，由于存在未知擾動(dòng)，產(chǎn)生速度抖動(dòng)的情況。