基于位移PID-速度ADRC的軌跡跟隨控制

［摘 要］路徑跟蹤在移動(dòng)機(jī)器人控制中具有重要的作用，文章針對(duì)差速輪移動(dòng)機(jī)器人的軌跡跟蹤控制問(wèn)題，提出了一種綜合縱向控制和橫向控制的方法?？v向控制包括縱向位置控制和速度控制，其中縱向位置控制采用PID 控制器，通過(guò)計(jì)算實(shí)際位置與期望位置之間的差異，生成適當(dāng)?shù)目刂菩盘?hào)，從而實(shí)現(xiàn)位置的調(diào)節(jié)和速度補(bǔ)償。而速度控制采用自抗擾控制，引入擾動(dòng)觀(guān)測(cè)器和補(bǔ)償器，對(duì)外界擾動(dòng)進(jìn)行實(shí)時(shí)估計(jì)和補(bǔ)償，提高系統(tǒng)的魯棒性和抗干擾能力。橫向控制采用純跟蹤算法，具有簡(jiǎn)單易行、實(shí)時(shí)性好的特點(diǎn)，能夠?qū)崿F(xiàn)機(jī)器人與目標(biāo)軌跡的高精度跟蹤。試驗(yàn)結(jié)果表明，文章所提出的控制方法表現(xiàn)出優(yōu)秀的性能和魯棒性，適用于差速輪移動(dòng)機(jī)器人的軌跡跟蹤應(yīng)用。

［關(guān)鍵詞］移動(dòng)機(jī)器人；路徑跟隨；自抗擾控制；純跟蹤

［中圖分類(lèi)號(hào)］TP273 ［文獻(xiàn)標(biāo)志碼］A ［文章編號(hào)］2095–6487（2024）04–0152–03

1 概述

隨著人工智能、傳感技術(shù)和運(yùn)動(dòng)技術(shù)的進(jìn)步，移動(dòng)機(jī)器人能夠以越來(lái)越高的精度自主移動(dòng)。文章提出了一種位移PID– 速度自抗擾控制（ADRC），用于移動(dòng)機(jī)器人的軌跡跟蹤，該控制方法具有適應(yīng)環(huán)境變化的能力。同時(shí)，文章還采用純跟蹤算法來(lái)實(shí)現(xiàn)機(jī)器人的橫向控制，以確?？刂频膶?shí)時(shí)性。

2 模型建立

差速輪是一種常用于移動(dòng)機(jī)器人的驅(qū)動(dòng)系統(tǒng)，其具有簡(jiǎn)單且可靠的結(jié)構(gòu)。為了實(shí)現(xiàn)差速輪的精確控制和路徑規(guī)劃，需要建立準(zhǔn)確的差速輪運(yùn)動(dòng)學(xué)模型。差速輪移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型為：

式中，x、y為機(jī)器人在世界坐標(biāo)系下的位置坐標(biāo)，θ為機(jī)器人的航向角。

在實(shí)際應(yīng)用中，可以根據(jù)差速輪的幾何特性和運(yùn)動(dòng)約束，通過(guò)數(shù)學(xué)建模和分析來(lái)推導(dǎo)差速輪運(yùn)動(dòng)學(xué)模型，從而實(shí)現(xiàn)對(duì)差速輪運(yùn)動(dòng)的控制。

3 縱向位移PID–速度ADRC閉環(huán)控制

3.1 控制框架

在差速輪移動(dòng)機(jī)器人路徑跟蹤控制中，通常會(huì)在執(zhí)行控制前進(jìn)行規(guī)劃。規(guī)劃的目標(biāo)是生成路徑規(guī)劃和速度規(guī)劃，以實(shí)現(xiàn)差速輪移動(dòng)機(jī)器人的準(zhǔn)確導(dǎo)航。差速輪移動(dòng)機(jī)器人需對(duì)參考軌跡點(diǎn)和速度進(jìn)行跟蹤控制，控制分為縱向控制和橫向控制。文章設(shè)計(jì)的縱向控制結(jié)構(gòu)如圖1 所示。

在縱向誤差控制中，采用了基于PID 控制器的縱向誤差控制策略。該控制器的輸入是參考點(diǎn)與車(chē)輛位置之間的縱向誤差，輸出是基于參考點(diǎn)處目標(biāo)速度的速度補(bǔ)償。這個(gè)速度補(bǔ)償與目標(biāo)速度相加后，作為自抗擾控制器的輸入，進(jìn)而計(jì)算出用于控制對(duì)象的控制量。

3.2 縱向位置PID控制

定義差速輪移動(dòng)機(jī)器人與參考軌跡的縱向位置誤差為e，離散化的PID 控制器表達(dá)式如下：

式中，Kp、Ki、Kd分別為PID控制器的比例系數(shù)、積分系數(shù)、微分系數(shù)。

在實(shí)際應(yīng)用中，移動(dòng)機(jī)器人的運(yùn)動(dòng)受到驅(qū)動(dòng)力和外界環(huán)境的影響，導(dǎo)致其速度和加速度存在一定的限制，因此需要對(duì)速度和加速度進(jìn)行約束。

4 縱向速度自抗擾控制

自抗擾控制能夠?qū)崟r(shí)估計(jì)和補(bǔ)償系統(tǒng)中的干擾，從而提升系統(tǒng)的魯棒性和抗干擾能力。ADRC 的基本結(jié)構(gòu)包括以下3 個(gè)主要組成部分。

4.1 跟蹤微分器

根據(jù)位移誤差PID 控制輸出的u（k）作為速度輸入存在一些問(wèn)題，如果直接將其作為速度控制的輸入并采用PID 控制，可能會(huì)導(dǎo)致系統(tǒng)出現(xiàn)震蕩或超調(diào)現(xiàn)象。因此，在輸入速度時(shí)需要增加過(guò)渡過(guò)程來(lái)解決這些問(wèn)題，跟蹤微分器可提供這個(gè)過(guò)渡過(guò)程。

定義跟蹤微分器為以下離散形式的系統(tǒng)：

式中，v（t）為目標(biāo)速度；v1為跟蹤速度；v2為跟蹤加速度，是v（t）的近似微分；r為收斂速度；h為采樣步長(zhǎng)。

該系統(tǒng)的輸入為v（t），輸出為v1 和v2。

其中fhan（.）函數(shù)的計(jì)算方式為：

4.2 擴(kuò)張狀態(tài)觀(guān)測(cè)器

ADRC 中的擴(kuò)張狀態(tài)觀(guān)測(cè)器通過(guò)引入擴(kuò)張狀態(tài)變量來(lái)描述系統(tǒng)中未建模的動(dòng)態(tài)特性和未知干擾，從而實(shí)現(xiàn)對(duì)這些因素的補(bǔ)償和抑制。設(shè)計(jì)的擴(kuò)張狀態(tài)觀(guān)測(cè)器為以下離散形式：

式中，z1為觀(guān)測(cè)速度，z2為觀(guān)測(cè)加速度，z3為擴(kuò)張狀態(tài)，y為實(shí)際速度輸出，h0為采用步長(zhǎng)，β01、β02、β03、α01、α02、b0、δ1均為可調(diào)參數(shù)。

其中函數(shù)fal（.）為：

觀(guān)測(cè)器觀(guān)測(cè)的狀態(tài)量是z1、z2、z3，其中，調(diào)整相應(yīng)的系數(shù)可得到一個(gè)觀(guān)測(cè)效果較好的觀(guān)測(cè)器。

4.3 非線(xiàn)性狀態(tài)誤差反饋

ADRC 中的非線(xiàn)性狀態(tài)誤差反饋（NLSEF）通過(guò)比較實(shí)際系統(tǒng)狀態(tài)與期望系統(tǒng)狀態(tài)之間的誤差，生成非線(xiàn)性反饋信號(hào)，從而修正控制器的輸出。對(duì)于NLSEF，先將z1 和v1 作差得跟蹤速度誤差e1，再將z2 和v2 作差得跟蹤加速度誤差e2，然后將得出的誤差采用非線(xiàn)性組合的方式計(jì)算為控制量：

式中，k1、k2為權(quán)重系數(shù)，α1、α2、δ2均為可調(diào)參數(shù)。

最后，由u0 結(jié)合觀(guān)測(cè)擾動(dòng)z3（k）形成的實(shí)際控制量為u，將u 輸入到移動(dòng)機(jī)器人執(zhí)行器中從而實(shí)現(xiàn)縱向控制：

5 純跟蹤算法橫向控制

在橫向控制中，文章采用純跟蹤算法，其是一種基于幾何關(guān)系確定曲率的方法，該曲率將驅(qū)動(dòng)差速輪移動(dòng)機(jī)器人到達(dá)選定的路徑點(diǎn)。目標(biāo)點(diǎn)是已知軌跡上與當(dāng)前機(jī)器人位置偏離一定距離ld 的點(diǎn)。假設(shè)差速輪移動(dòng)機(jī)器人中心點(diǎn)可以按照一定的轉(zhuǎn)彎半徑r 到達(dá)該預(yù)瞄點(diǎn)，然后根據(jù)視距l(xiāng)d、轉(zhuǎn)彎半徑r 和差速輪移動(dòng)機(jī)器人坐標(biāo)系下預(yù)瞄點(diǎn)的朝向角α 之間的幾何關(guān)系，確定差速輪移動(dòng)機(jī)器人的轉(zhuǎn)向角度，得到圓弧曲率γ與視距l(xiāng)d 之間的關(guān)系和角速度表達(dá)式為：

式中，x為目標(biāo)點(diǎn)相對(duì)于機(jī)器人的橫向誤差。

6 仿真試驗(yàn)

文章對(duì)跟蹤微分器的響應(yīng)、擴(kuò)張狀態(tài)觀(guān)測(cè)器的響應(yīng)和雙移線(xiàn)的軌跡跟蹤進(jìn)行仿真試驗(yàn)。

跟蹤微分器及擴(kuò)張狀態(tài)觀(guān)測(cè)器的響應(yīng)試驗(yàn)如圖2所示。由圖2 可知，跟蹤微分器具有較好的速度跟蹤效果，并且沒(méi)有高頻顫振，以及擴(kuò)張狀態(tài)觀(guān)測(cè)器對(duì)系統(tǒng)狀態(tài)量觀(guān)測(cè)有非常好的效果。

雙移線(xiàn)的軌跡跟蹤效果如圖3 所示，具有較好的跟蹤效果，圖中不間斷曲線(xiàn)為跟蹤的雙移線(xiàn)軌跡，“*”曲線(xiàn)為離散的跟蹤軌跡點(diǎn)。

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

文章提出了一種基于純跟蹤算法的橫向控制和位路徑曲率的變化具有較好的適應(yīng)性，能夠保持良好的軌跡跟蹤性能。在縱向控制方面，位移PID控制能夠?qū)崿F(xiàn)位置誤差的精確控制，而速度ADRC控制則能夠提供較好的魯棒性和抗干擾能力。試驗(yàn)結(jié)果表明，這種組合控制策略使得移動(dòng)機(jī)器人能夠穩(wěn)定地跟蹤軌跡，具有較高的實(shí)用性。

參考文獻(xiàn)

[1] DIANAYANAKIEV，IOANNISKANELLAKOPOULOS.Speed Tracking and Vehicle Follower Control Design forHeavy-Duty Vehicles[J].Vehicle System Dynamics，2007，25（4）：251-276.

[2] 李以農(nóng)，鄭玲，郝奕，等. 基于參數(shù)自整定模糊 PID 的汽車(chē)縱向控制[J]. 江蘇大學(xué)學(xué)報(bào)（自然科學(xué)版），2006，27（1）：22-26.

[3] RIVERA D E，MORARI M，SKOGESTAD S .Internalmodel control ：PID controller design[J].Industrial &Engineering Chemistry Process Design & Development，1986，25（1）：2163.

[4] 曹杰華. 一種基于純跟蹤算法的單舵輪AGV 路徑跟隨方法及系統(tǒng)：CN202210562076.8[P].2022-09-02.

[5] 靳欣宇，張軍，劉元盛，等. 基于Stanley 算法的自適應(yīng)最優(yōu)預(yù)瞄模型研究[J]. 計(jì)算機(jī)工程，2018，44（7）：42-46.

[6] 許萬(wàn)，曹松，羅西，等. 雙輪差速移動(dòng)機(jī)器人軌跡跟蹤混合控制算法研究[J]. 組合機(jī)床與自動(dòng)化加工技術(shù)，2018（3）：78-83.

[7] 韓京清. 從PID 技術(shù)到“自抗擾控制”技術(shù)[J]. 控制工程，2002（3）：13-18.