基于改進(jìn)的動(dòng)態(tài)窗口法的雙舵輪軌跡跟蹤研究

任文強(qiáng)，張秋菊

（江南大學(xué) 機(jī)械學(xué)院，無(wú)錫 214000）

0 引言

我國(guó)經(jīng)過(guò)多年的發(fā)展與科技革新，自主移動(dòng)平臺(tái)和相關(guān)運(yùn)動(dòng)控制技術(shù)取得飛速發(fā)展，其應(yīng)用更是遍布各個(gè)行業(yè)，在工業(yè)生產(chǎn)中顯得尤為突出，為生產(chǎn)與運(yùn)輸帶來(lái)了極大的便利。經(jīng)過(guò)多年的研究，精準(zhǔn)優(yōu)化的路徑跟蹤依然存在一定的困難，其相關(guān)技術(shù)仍需要不斷提高，相關(guān)研究還需繼續(xù)進(jìn)行。Huang JS等提出一種可以保證誤差趨近于任意小的自適應(yīng)輸出反饋跟蹤控制器，該種控制方法主要是針對(duì)于非完整移動(dòng)機(jī)器人的應(yīng)用[1]；Chen DD等通過(guò)研究AGV小車(chē)的前進(jìn)與后退的相似之處，提出一種新的控制規(guī)則，但是對(duì)于一般性的AGV小車(chē)存在不適應(yīng)性，有待進(jìn)一步提高[2]；文獻(xiàn)利用Backstepping的思想，通過(guò)構(gòu)造中間變量，使系統(tǒng)趨近于全局穩(wěn)定[3]。對(duì)于局部路徑的規(guī)劃工作更是投入許多，人工勢(shì)場(chǎng)法[4]、動(dòng)態(tài)窗口法[5]和A*算法[6]等得到大量的應(yīng)用。但是依然存在問(wèn)題，相關(guān)研究主要是針對(duì)于單舵輪驅(qū)動(dòng)的情況，劉媛媛等利用單舵輪的形式進(jìn)行Gmapping Slam相關(guān)導(dǎo)航算法研究[7]；山東大學(xué)的劉林煒等以單舵輪的AGV為研究對(duì)象，進(jìn)行基于模糊控制的避障系統(tǒng)研究[8]等，對(duì)于雙舵輪驅(qū)動(dòng)的相關(guān)研究很少，但是隨著工業(yè)化發(fā)展，對(duì)移動(dòng)平臺(tái)的性能和負(fù)載提出了更高的要求，單一的舵輪往往無(wú)法滿足，就需要雙舵輪的驅(qū)動(dòng)形式。并且以上方法存在一定的設(shè)計(jì)復(fù)雜性和適用特殊性，往往需要投入大量的工作成本。

針對(duì)于以上問(wèn)題，本研究以雙舵輪驅(qū)動(dòng)為研究對(duì)象，研究與分析常用的三種雙舵輪驅(qū)動(dòng)形式的優(yōu)劣，通過(guò)借助專(zhuān)用軟件進(jìn)行仿真和搭建樣機(jī)進(jìn)行驗(yàn)證，得出第一種驅(qū)動(dòng)形式更適合應(yīng)用。并且借鑒動(dòng)態(tài)窗口法，在傳統(tǒng)的動(dòng)態(tài)窗口法基礎(chǔ)之上進(jìn)行適應(yīng)性改進(jìn)，改進(jìn)其速度域搜索范圍和最后的評(píng)價(jià)函數(shù)指標(biāo)，通過(guò)軟件仿真與樣機(jī)搭建驗(yàn)證該方法的可行性。

1 運(yùn)動(dòng)學(xué)建模

1.1 第一種驅(qū)動(dòng)方式

圖1 第一種驅(qū)動(dòng)模式示意簡(jiǎn)圖

假設(shè)：α1與α2分別表示前后輪與中心方向線的夾角；R、R1和R2分別表示中心點(diǎn)、前輪和后輪的旋轉(zhuǎn)半徑；v、v1和v2分別表示中心點(diǎn)、前輪與后輪的驅(qū)動(dòng)速度。因此可以得到如下關(guān)系：

其中：vx，表示在x軸方向上的分速度；vy，表示在y方向上的分速度；θ，表示整體旋轉(zhuǎn)角度；ω，表示其旋轉(zhuǎn)角速度。

線速度與角速度是整個(gè)移動(dòng)平臺(tái)運(yùn)動(dòng)的參數(shù)指標(biāo)，通過(guò)控制前后舵輪的運(yùn)動(dòng)速度與方向來(lái)控制整體的線速度與角速度。由示意圖可以得到如下的簡(jiǎn)化關(guān)系式：

從而得到理論路徑：

后舵輪的轉(zhuǎn)動(dòng)角度和速度的調(diào)整和前輪的運(yùn)動(dòng)模型一樣的，只是其中的個(gè)別參數(shù)會(huì)發(fā)生變化。

1.2 第二種驅(qū)動(dòng)方式

圖2 第二種驅(qū)動(dòng)模式示意簡(jiǎn)圖

該種驅(qū)動(dòng)方式是以后輪為中心，作為中心驅(qū)動(dòng)輪，前輪作為轉(zhuǎn)向輪沿著預(yù)定軌跡進(jìn)行移動(dòng)。得到其在參考坐標(biāo)系下的方程如下：

經(jīng)過(guò)計(jì)算，得到如下運(yùn)動(dòng)學(xué)模型：

1.3 第三種驅(qū)動(dòng)方式

第三種驅(qū)動(dòng)方式和第一種在形式上具有類(lèi)似性，主要是以后輪作為轉(zhuǎn)向輪，前輪作為驅(qū)動(dòng)輪。將整體的中心定義在前輪處，假設(shè)前輪與預(yù)定軌跡的行走路線是完全重合的，后輪作為改變方向的輪子。如下所示：

圖3 第三種驅(qū)動(dòng)模式示意簡(jiǎn)圖

2 誤差

根據(jù)相關(guān)文獻(xiàn)中定義的理論姿態(tài)與實(shí)際姿態(tài)的相關(guān)概念[9]，設(shè)實(shí)際姿態(tài)為pr（xryrθr），理論姿態(tài)為pt（xtytθt）。因此可以得到誤差為：

在利用運(yùn)動(dòng)模型進(jìn)行運(yùn)動(dòng)控制過(guò)程中，軌跡跟蹤的最終目的就是通過(guò)控制相關(guān)參數(shù)間接控制誤差[10]，即pe=0。得到：

3 求最優(yōu)覆蓋面積

其主要思想是：移動(dòng)平臺(tái)在實(shí)際運(yùn)行過(guò)程中，中心點(diǎn)沿著預(yù)設(shè)路徑進(jìn)行移動(dòng)，但是實(shí)際情況下前后輪或者移動(dòng)平臺(tái)整體的移動(dòng)路徑是覆蓋在移動(dòng)平臺(tái)的預(yù)設(shè)路徑上的，會(huì)出現(xiàn)類(lèi)似于圖中陰影部分的軌跡圖，可以根據(jù)陰影部分面積的大小確定移動(dòng)平臺(tái)的軌跡與預(yù)設(shè)軌跡之間的重合度。且在實(shí)際使用過(guò)程中，最關(guān)心的并不是中心點(diǎn)的軌跡路徑，而是移動(dòng)平臺(tái)的整體的運(yùn)動(dòng)路徑，這樣方便于避障處理和多機(jī)位中的最優(yōu)路徑規(guī)劃?；谝陨显?，本研究提出采用最優(yōu)覆蓋面積法確定移動(dòng)平臺(tái)的軌跡。

圖4 示意簡(jiǎn)圖

針對(duì)于上述第一種驅(qū)動(dòng)方式，以移動(dòng)平臺(tái)的中心作為預(yù)定軌跡跟蹤點(diǎn)，前后輪中心的連線作為移動(dòng)平臺(tái)簡(jiǎn)化輪廓，則得到圖5示意圖。

圖5 第一種驅(qū)動(dòng)模式行進(jìn)示意簡(jiǎn)圖

在已知輪廓中心點(diǎn)的情況下，求前后輪中心點(diǎn)的坐標(biāo)如下：

對(duì)于第二種和第三種驅(qū)動(dòng)方式采用類(lèi)似上述第一種的方式計(jì)算前后輪中心的坐標(biāo)。然后計(jì)算出陰影部分的面積S，該面積即為評(píng)價(jià)實(shí)際軌跡與預(yù)定軌跡的重合度的指標(biāo)，也為評(píng)價(jià)上述三種驅(qū)動(dòng)方式優(yōu)劣的指標(biāo)，最重要的是作為動(dòng)態(tài)窗口法速度選擇的一個(gè)限制條件。

4 動(dòng)態(tài)窗口法

4.1 速度搜索

動(dòng)態(tài)窗口法是用于單車(chē)體移動(dòng)局部路徑規(guī)劃普遍適用的方法，可以計(jì)算出車(chē)體到達(dá)預(yù)定位置所需要的最合適的速度[11]。其主要思想為根據(jù)當(dāng)前運(yùn)行的速度在速度域中搜索下一時(shí)間的合適速度，即：根據(jù)目前的線速度或者角速度，在速度空間中計(jì)算出多組速度，考慮移動(dòng)平臺(tái)的狀態(tài)模擬移動(dòng)平臺(tái)在速度下的運(yùn)行軌跡，然后根據(jù)評(píng)價(jià)指標(biāo)函數(shù)選擇下一個(gè)時(shí)間間隔的速度，驅(qū)動(dòng)移動(dòng)平臺(tái)移動(dòng)[12]。傳統(tǒng)的動(dòng)態(tài)窗口算法主要是針對(duì)于局部坐標(biāo)系下的路徑，忽視了整體路徑[13]，本研究將受整體軌跡覆蓋面積影響的前后輪速度作為一項(xiàng)限定指標(biāo)，將覆蓋面積作為一項(xiàng)評(píng)價(jià)指標(biāo)。速度搜索的限制因素可分為三種：移動(dòng)平臺(tái)速度約束、移動(dòng)平臺(tái)加速度約束、障礙物約束[14]，但是本研究拓展了一項(xiàng)約束——前后輪速度約束。

1）移動(dòng)平臺(tái)速度約束

式中：vmin，vmax表示最小和最大線速度約束；ωmin，ωmax表示最小和最大角速度約束。

2）移動(dòng)平臺(tái)加速度約束該項(xiàng)約束是由于移動(dòng)平臺(tái)的電機(jī)性能和外界環(huán)境影響決定的。

式中：βmin，βmax表示最小和最大加速度；

3）障礙物約束

考慮障礙物，主要是由于移動(dòng)平臺(tái)在運(yùn)動(dòng)過(guò)程中會(huì)遇到障礙，當(dāng)遇到障礙時(shí)，要求其必須緊急停止。

式中：dist（v，ω），表示所選速度軌跡中（v，ω）對(duì)應(yīng)距離障礙物的最近距離。

4）前后輪速度約束：

該項(xiàng)約束主要是考慮前后輪的速度。對(duì)于第一種驅(qū)動(dòng)方式，其中心位置的速度才是速度域中所選定的速度，所以需要根據(jù)前后輪的速度對(duì)其進(jìn)行相應(yīng)的約束；對(duì)于第二，三種驅(qū)動(dòng)方式，主要是考慮非中心位置輪的舵輪處的速度，該處以第一種驅(qū)動(dòng)方式為例。

4.2 評(píng)價(jià)函數(shù)

設(shè)計(jì)評(píng)價(jià)函數(shù)的目的是作為速度空間選擇的指標(biāo)或者標(biāo)準(zhǔn)。根據(jù)比較評(píng)價(jià)函數(shù)，選擇其中最合適的速度。其設(shè)計(jì)原則：根據(jù)方位角偏差、距離障礙物最短距離、速度評(píng)價(jià)函數(shù)和相應(yīng)的權(quán)重，規(guī)劃路徑使其最快、最準(zhǔn)確的到達(dá)目標(biāo)位置。

引入面積評(píng)價(jià)函數(shù)：

引入路徑偏差評(píng)價(jià)函數(shù)：

綜上得到評(píng)價(jià)函數(shù)如下：

式中：head(v，ω)，用于評(píng)價(jià)方位角偏差的評(píng)價(jià)函數(shù)，該方位角偏差不是與目標(biāo)點(diǎn)的方位角偏差，而是與下一時(shí)刻的預(yù)定軌跡之間的偏差；vel(v，ω)，用于評(píng)價(jià)當(dāng)前速度的評(píng)價(jià)函數(shù)；σ，表示平滑函數(shù)；α，β，γ和μ，分別表示對(duì)應(yīng)的權(quán)重。其整體算法流程如圖6所示。

圖6 算法流程圖

5 軌跡跟蹤仿真

在實(shí)際應(yīng)用中，多采用直線和標(biāo)準(zhǔn)圓弧組合的方式，在直線與圓弧的交點(diǎn)處會(huì)發(fā)生曲率突變的情況，本研究假設(shè)不發(fā)生突變。首先借助MATLAB軟件進(jìn)行仿真，比較上述三種驅(qū)動(dòng)方式中心點(diǎn)的軌跡跟蹤情況，不考慮窗口算法，取路徑函數(shù)如下，最大線速度vmax=0.8m/s，最大加速度amax=1.2m/s2，最大角速度ωmax=10°/s，最大角加速度βmax=10°/s2。

表1 算法參數(shù)設(shè)置

其結(jié)果如圖7所示。

圖7 第一次軌跡跟蹤仿真結(jié)果

從中可以得到：三種驅(qū)動(dòng)方式與預(yù)定軌跡的重合度都很好，但是在預(yù)定軌跡圓弧與直線的交點(diǎn)處會(huì)發(fā)生一定的突變；第三種與第二種驅(qū)動(dòng)方式的軌跡是一樣的。

引入動(dòng)態(tài)窗口算法，本研究中不存在障礙物，因此，之后的結(jié)果如圖8所示。

可以得到：經(jīng)過(guò)改進(jìn)的動(dòng)態(tài)窗口法之后，行進(jìn)路徑點(diǎn)與原預(yù)設(shè)路徑交錯(cuò)，可以看出，其與預(yù)設(shè)路徑更逼近。但是，可以得到圖8(a)圖更加逼近預(yù)設(shè)路徑，因?yàn)槠洳捎玫氖且苿?dòng)平臺(tái)的中心點(diǎn)作為移動(dòng)參考點(diǎn)，而圖8(b)、圖8(c)采用的是前后輪作為移動(dòng)參考點(diǎn)，這樣在實(shí)際情況下會(huì)出現(xiàn)較大的偏移。

圖8 第二次軌跡跟蹤仿真簡(jiǎn)圖

6 樣機(jī)實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證仿真結(jié)果，結(jié)合相關(guān)知識(shí)，將得到的控制方法應(yīng)用在搭建的樣機(jī)上，進(jìn)行進(jìn)一步的驗(yàn)證。樣機(jī)與底部舵輪布局模型如圖9所示。

圖9 實(shí)驗(yàn)樣機(jī)

圖10 采集數(shù)據(jù)結(jié)果

樣機(jī)行走路線依然采用上述的直線與標(biāo)準(zhǔn)圓弧的組合形式。主要是驗(yàn)證預(yù)定軌跡與通過(guò)數(shù)據(jù)采集的軌跡之間的偏差。分別進(jìn)行三種驅(qū)動(dòng)形式下的實(shí)驗(yàn)，通過(guò)里程計(jì)反饋舵輪的轉(zhuǎn)角與速度。

從中可以得出：第一種驅(qū)動(dòng)方式相較于后面兩種，更接近于預(yù)定軌跡；在直線與圓弧的交點(diǎn)處，都出現(xiàn)了突變；第二種與第三種起始階段與預(yù)定軌跡保持良好，但是隨著行進(jìn)時(shí)間的推移，其偏離程度越來(lái)越大。總的情況與趨勢(shì)與仿真結(jié)果相同，但是實(shí)際環(huán)境中干擾因素很多，因此與仿真結(jié)果表現(xiàn)的情況存在差異。

7 總結(jié)

7.1 創(chuàng)新點(diǎn)

多數(shù)研究主要是針對(duì)于單舵輪的情況，對(duì)于雙舵輪的運(yùn)動(dòng)模型相關(guān)研究較少。本研究通過(guò)相應(yīng)的簡(jiǎn)化方法，提出一種雙舵輪前后驅(qū)動(dòng)方式，為工業(yè)上的應(yīng)用提供一定的參照。

引入覆蓋面積法，評(píng)價(jià)實(shí)際軌跡與預(yù)定軌跡之間的逼近程度，將其作為全局路徑規(guī)劃的評(píng)價(jià)指標(biāo)。

考慮偏差，改進(jìn)動(dòng)態(tài)窗口法，將動(dòng)態(tài)窗口法中的傳統(tǒng)評(píng)價(jià)函數(shù)中的方位角偏差函數(shù)改為目前時(shí)刻與下一時(shí)刻的目標(biāo)位置之間的速度方向偏差。

基于傳統(tǒng)動(dòng)態(tài)窗口法的局限性，本研究考慮覆蓋面積區(qū)域?qū)φw規(guī)劃的影響，提出一種考慮全局與實(shí)際路徑情況的路徑規(guī)劃方法。

7.2 缺陷

該方法也存在一定的缺陷。在仿真過(guò)程中，做了一些假設(shè)條件；在樣機(jī)驗(yàn)證環(huán)節(jié)，由于測(cè)量設(shè)備的不精確和測(cè)量偏差等外部因素，導(dǎo)致實(shí)驗(yàn)數(shù)據(jù)只能逼近；在運(yùn)動(dòng)學(xué)建模中，由于雙舵輪驅(qū)動(dòng)形式的復(fù)雜性，本研究做了部分簡(jiǎn)化。

7.3 結(jié)語(yǔ)

本研究以雙舵輪驅(qū)動(dòng)為研究對(duì)象，綜合比較常使用的三種雙舵輪驅(qū)動(dòng)形式之間的優(yōu)缺點(diǎn)，經(jīng)過(guò)仿真驗(yàn)證和實(shí)物驗(yàn)證環(huán)節(jié)，推論出第一種驅(qū)動(dòng)形式更合適實(shí)際情況的使用，但是第一種驅(qū)動(dòng)形式存在復(fù)雜性，后續(xù)會(huì)繼續(xù)進(jìn)行相關(guān)的研究。在路徑跟蹤過(guò)程中借鑒動(dòng)態(tài)窗口法，并在傳統(tǒng)的基礎(chǔ)之上進(jìn)行改進(jìn)，最后提出一種適合本研究的方法，經(jīng)過(guò)仿真與樣機(jī)驗(yàn)證，存在可行性，后續(xù)還會(huì)繼續(xù)在算法的實(shí)時(shí)性和靈敏性上做進(jìn)一步的修正。