基于Lyapunov直接法差動(dòng)驅(qū)動(dòng)機(jī)器人控制算法研究

，，

(上海工程技術(shù)大學(xué) 機(jī)械工程學(xué)院，上海 201620)

0 引言

近年來(lái)，非完整移動(dòng)機(jī)器人的運(yùn)動(dòng)控制技術(shù)已經(jīng)成為機(jī)器人領(lǐng)域的研究熱點(diǎn)，是實(shí)現(xiàn)輪式機(jī)器人智能化可靠行駛的重要保證，也是實(shí)現(xiàn)自主導(dǎo)航的前提和基礎(chǔ)，具有廣闊的應(yīng)用前景和市場(chǎng)價(jià)值。根據(jù)控制方法不同分為點(diǎn)鎮(zhèn)定控制[1]和跟蹤控制[2-3]，點(diǎn)鎮(zhèn)定控制是為機(jī)器人系統(tǒng)設(shè)計(jì)控制律，使其能夠到達(dá)運(yùn)動(dòng)平面上任意給定目標(biāo)點(diǎn)，并且能夠以預(yù)期的姿態(tài)穩(wěn)定在該目標(biāo)點(diǎn)；跟蹤控制所設(shè)計(jì)的控制律可以使機(jī)器人能夠到達(dá)并以一個(gè)給定的速度跟蹤平面上某條軌跡。鎮(zhèn)定控制與跟蹤控制原理不同，因此在實(shí)際應(yīng)用中常根據(jù)不同的問題來(lái)設(shè)計(jì)不同的控制器。

以步進(jìn)電機(jī)驅(qū)動(dòng)的差動(dòng)驅(qū)動(dòng)輪式機(jī)器人為研究對(duì)象，首先建立運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，設(shè)計(jì)了一種基于Lyapunov直接法的運(yùn)動(dòng)學(xué)軌跡跟蹤控制器；然后根據(jù)動(dòng)力學(xué)模型引入速度、加速度限制策略；仿真結(jié)果證明該控制器具有較快的收斂速度和較高的穩(wěn)定性，在軌跡發(fā)生變化時(shí)也能迅速做出調(diào)整；最后將控制算法的脈沖參數(shù)編譯下載到STM32F407進(jìn)行實(shí)驗(yàn)，獲得了較好的結(jié)果。

1 輪式機(jī)器人數(shù)學(xué)模型

1.1 運(yùn)動(dòng)學(xué)模型

圖1 運(yùn)動(dòng)學(xué)示意圖

(1)

由約束方程(1)可知，不能通過積分得到x，y與θ的關(guān)系，這是典型的非完整約束系統(tǒng)。

根據(jù)運(yùn)動(dòng)學(xué)原理還可以得到：

(2)

(3)

(4)

(5)

(6)

由式(2)～(6)可以建立非完整移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型：

(7)

(8)

結(jié)合公式(2)(3)(4)，經(jīng)過推導(dǎo)可得到機(jī)器人位姿誤差的微分方程：

(9)

1.2 動(dòng)力學(xué)模型

一般情況下，具有n維系統(tǒng)狀態(tài)移動(dòng)機(jī)器人模型可以用非完整約束的廣義力學(xué)系統(tǒng)來(lái)描述，根據(jù)拉格朗日力學(xué)分析方法，建立動(dòng)力學(xué)模型[4]：

(10)

(11)

(12)

設(shè)機(jī)器人的質(zhì)量為m，機(jī)器人相對(duì)于點(diǎn)OR的轉(zhuǎn)動(dòng)慣量為J，左右兩驅(qū)動(dòng)輪的驅(qū)動(dòng)力矩分別為TL、TR，令驅(qū)動(dòng)輪向機(jī)器人前進(jìn)方向轉(zhuǎn)動(dòng)時(shí)驅(qū)動(dòng)力矩為正，向機(jī)器人后退方向轉(zhuǎn)動(dòng)時(shí)驅(qū)動(dòng)力矩為負(fù)。對(duì)圖1進(jìn)行受力分析，得到點(diǎn)OR處的廣義驅(qū)動(dòng)力向量：

(13)

公式(13)中fx為驅(qū)動(dòng)力在X軸向分量，fy為驅(qū)動(dòng)力在Y軸向分量，fθ為使機(jī)器人旋轉(zhuǎn)的力矩。當(dāng)ω>0時(shí)，機(jī)器人由于離心力的作用有向軸負(fù)向ORYR發(fā)生側(cè)滑的趨勢(shì)，因此約束力λ指向ORYR軸正方向，根據(jù)牛頓第二定律建立系統(tǒng)動(dòng)力學(xué)方程：

(14)

(15)

(16)

對(duì)公式(16)求導(dǎo)可得：

(17)

將公式(17)代入公式(12)可得：

(18)

公式(18)等號(hào)兩邊同時(shí)左乘ST(q)，可以消去AT(q)λ項(xiàng)，經(jīng)過推導(dǎo)可得：

(S(q)TM(q)S(q))-1S(q)TB(q)τ

(19)

將S(q)，M(q)，V，B(q)τ的結(jié)果代入公式(19)最終可得：

(20)

2 Lyapunov直接法控制器設(shè)計(jì)

輪式移動(dòng)機(jī)器人控制系統(tǒng)原理結(jié)構(gòu)框圖如圖2所示，其中控制輸入量Vc=[vc,ωc]T，P表示機(jī)器人實(shí)際位姿，Pd表示機(jī)器人期望位姿，Pd-P為機(jī)器人在絕對(duì)坐標(biāo)系XOY下的位姿誤差，經(jīng)過旋轉(zhuǎn)矩陣的作用轉(zhuǎn)化為在機(jī)器人局部坐標(biāo)系XRORYR下的位姿誤差Pe。采用Lyapunov直接法設(shè)計(jì)運(yùn)動(dòng)學(xué)控制器，構(gòu)造能量函數(shù)[5]：

(21)

圖2 機(jī)器人控制結(jié)構(gòu)原理圖

其中：k1>0且有界，則公式(21)為正定函數(shù)，L對(duì)時(shí)間t求導(dǎo)并結(jié)合公式(7)、(8)、(9)可得：

(22)

取控制律:

(23)

其中：k2,k3>0有界。

聯(lián)立公式(5)、(6)、(23)得：

(24)

將控制律(23)代入公式(22)可得：

(25)

通過控制器的作用最終會(huì)使Pe趨于0，則有θe→0，將sinθe泰勒級(jí)數(shù)展開，公式(25)可轉(zhuǎn)化為：

(26)

控制律(23)中vc主要是為了消除移動(dòng)機(jī)器人前進(jìn)方向上的位置誤差xe，其中vdcosθe為機(jī)器人前進(jìn)方向上因期望線速度vd產(chǎn)生的位移；k3θe項(xiàng)對(duì)θe的導(dǎo)數(shù)要大于等于k3sinθe對(duì)θe的導(dǎo)數(shù)，因此該控制律要比使用kesinθe作為參數(shù)的控制律對(duì)姿態(tài)角誤差θe調(diào)節(jié)更加迅速。

3 仿真分析

圖3 軌跡跟蹤情況

圖4 位姿誤差

圖5 線速度和角速度的變化曲線

圖6 線加速度和角加速度變化曲線

仿真中期望軌跡為直線軌跡和曲線軌跡的結(jié)合，使得機(jī)器人在期望軌跡上有角速度的變化，在這種情況下機(jī)器人也能穩(wěn)定地跟蹤運(yùn)動(dòng)，并且在軌跡的變化處能快速做出響應(yīng)。通過仿真結(jié)果可以看出，初始位姿誤差在4 s內(nèi)完成收斂，調(diào)節(jié)速度快，在收斂的過程中沒有發(fā)生速度突變，且收斂后有良好的穩(wěn)定性，除第一次轉(zhuǎn)彎處由于線速度v>0.5 m/s使得xd-x出現(xiàn)一次負(fù)向跳動(dòng)，之后的轉(zhuǎn)彎處線速度穩(wěn)定在0.5 m/s，所以位姿誤差基本穩(wěn)定在0值沒有再次發(fā)生跳變。由于加入速度和加速度限制策略，使得速度、加速度在有限范圍內(nèi)變化，通過圖6可以得到角加速度始終被限制在[-8.428 8.428]范圍內(nèi)，保證了步進(jìn)電機(jī)較高的工作性能和控制精度。

4 實(shí)驗(yàn)驗(yàn)證與分析

結(jié)合支持Simulink的第三方硬件庫(kù)，如圖7所示，采用基于模型的程序設(shè)計(jì)方法，將脈沖數(shù)據(jù)編譯下載到STM32F407微控芯片中。由于機(jī)械裝配存在誤差，機(jī)器人轉(zhuǎn)動(dòng)慣量無(wú)法精確計(jì)算等因素，編譯下載前需要將控制算法生成的脈沖數(shù)據(jù)結(jié)合實(shí)際情況進(jìn)行小范圍的修正。利用ULN2003驅(qū)動(dòng)板將STM32F407對(duì)應(yīng)引腳輸出的3.3 V信號(hào)電壓轉(zhuǎn)換為步進(jìn)電機(jī)驅(qū)動(dòng)器額定信號(hào)電壓5 V，通過相應(yīng)的電路連接在機(jī)器人平臺(tái)上進(jìn)行實(shí)驗(yàn)。

取圖3中(2,0),(1,1),(2.5,1),(4,1),(5,2),(4,3),(2.5,3),(1,3),(0,2)等9個(gè)坐標(biāo)點(diǎn)為標(biāo)記點(diǎn)分別標(biāo)號(hào)0-8，如圖8所示布置實(shí)驗(yàn)場(chǎng)地。經(jīng)過實(shí)驗(yàn)，機(jī)器人運(yùn)行狀態(tài)良好，在期望時(shí)刻與對(duì)應(yīng)標(biāo)記點(diǎn)的位姿誤差較小，控制算法生成的脈沖數(shù)據(jù)準(zhǔn)確度高，而且可以較方便地將控制算法計(jì)算結(jié)果應(yīng)用于微控制芯片。

圖7 程序模型

圖8 標(biāo)記點(diǎn)示意圖1

5 結(jié)論

以步進(jìn)電機(jī)差動(dòng)驅(qū)動(dòng)輪式機(jī)器人為研究對(duì)象，通過建立運(yùn)動(dòng)學(xué)模型，利用Lyapunov直接法設(shè)計(jì)了運(yùn)動(dòng)學(xué)控制器；通過建立動(dòng)力學(xué)模型并結(jié)合步進(jìn)電機(jī)工作特點(diǎn)，設(shè)計(jì)了對(duì)運(yùn)動(dòng)學(xué)控制器中線速度、角速度、線加速度、加速度的限制策略，最終實(shí)現(xiàn)了對(duì)機(jī)器人的運(yùn)動(dòng)控制；經(jīng)過仿真和實(shí)驗(yàn)，證明所設(shè)計(jì)的控制器具有較高的跟蹤精度和穩(wěn)定性，機(jī)器人對(duì)初始位姿誤差矯正時(shí)間短，在軌跡變化處能做出較快響應(yīng)；結(jié)合matlab硬件支持包，采用基于模型的程序設(shè)計(jì)方法可以將理論分析計(jì)算結(jié)果應(yīng)用于實(shí)踐，縮短了開發(fā)周期。

參考文獻(xiàn)：

[1] 曹政才，趙應(yīng)濤，吳啟迪. 基于Bcakstepping和神經(jīng)動(dòng)力學(xué)的非完整移動(dòng)機(jī)器人點(diǎn)鎮(zhèn)定[J]. 電子學(xué)報(bào)，2011，39(3)：591-595.

[2] 馬建偉，石佳玉. 非完整約束移動(dòng)機(jī)器人的軌跡跟蹤控制[J]. 計(jì)算機(jī)測(cè)量與控制，2017，25(3): 77-80.

[3] 金 娟，王耀南. 基于模糊CMAC的移動(dòng)機(jī)器人軌跡跟蹤控制[J]. 計(jì)算機(jī)工程與應(yīng)用，2015，51(1): 54-58.

[4] 楊 敏. 輪式移動(dòng)機(jī)器人控制算法研究及其伺服系統(tǒng)設(shè)計(jì)[D]. 南京：南京航空航天大學(xué)，2014

[5] 李 平，王建鋒，李 娜，等. 基于Lyapunov直接法的移動(dòng)機(jī)器人運(yùn)動(dòng)控制研究[J]. 中國(guó)科技論文，2016，11(2)：160-163.