基于改進的動態窗口法的雙舵輪軌跡跟蹤研究

任文強，張秋菊

（江南大學 機械學院，無錫 214000）

0 引言

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

針對于以上問題，本研究以雙舵輪驅動為研究對象，研究與分析常用的三種雙舵輪驅動形式的優劣，通過借助專用軟件進行仿真和搭建樣機進行驗證，得出第一種驅動形式更適合應用。并且借鑒動態窗口法，在傳統的動態窗口法基礎之上進行適應性改進，改進其速度域搜索范圍和最后的評價函數指標，通過軟件仿真與樣機搭建驗證該方法的可行性。

1 運動學建模

1.1 第一種驅動方式

圖1 第一種驅動模式示意簡圖

假設：α1與α2分別表示前后輪與中心方向線的夾角；R、R1和R2分別表示中心點、前輪和后輪的旋轉半徑；v、v1和v2分別表示中心點、前輪與后輪的驅動速度。因此可以得到如下關系：

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

線速度與角速度是整個移動平臺運動的參數指標，通過控制前后舵輪的運動速度與方向來控制整體的線速度與角速度。由示意圖可以得到如下的簡化關系式：

從而得到理論路徑：

后舵輪的轉動角度和速度的調整和前輪的運動模型一樣的，只是其中的個別參數會發生變化。

1.2 第二種驅動方式

圖2 第二種驅動模式示意簡圖

該種驅動方式是以后輪為中心，作為中心驅動輪，前輪作為轉向輪沿著預定軌跡進行移動。得到其在參考坐標系下的方程如下：

經過計算，得到如下運動學模型：

1.3 第三種驅動方式

第三種驅動方式和第一種在形式上具有類似性，主要是以后輪作為轉向輪，前輪作為驅動輪。將整體的中心定義在前輪處，假設前輪與預定軌跡的行走路線是完全重合的，后輪作為改變方向的輪子。如下所示：

圖3 第三種驅動模式示意簡圖

2 誤差

根據相關文獻中定義的理論姿態與實際姿態的相關概念[9]，設實際姿態為pr（xryrθr），理論姿態為pt（xtytθt）。因此可以得到誤差為：

在利用運動模型進行運動控制過程中，軌跡跟蹤的最終目的就是通過控制相關參數間接控制誤差[10]，即pe=0。得到：

3 求最優覆蓋面積

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

圖4 示意簡圖

針對于上述第一種驅動方式，以移動平臺的中心作為預定軌跡跟蹤點，前后輪中心的連線作為移動平臺簡化輪廓，則得到圖5示意圖。

圖5 第一種驅動模式行進示意簡圖

在已知輪廓中心點的情況下，求前后輪中心點的坐標如下：

對于第二種和第三種驅動方式采用類似上述第一種的方式計算前后輪中心的坐標。然后計算出陰影部分的面積S，該面積即為評價實際軌跡與預定軌跡的重合度的指標，也為評價上述三種驅動方式優劣的指標，最重要的是作為動態窗口法速度選擇的一個限制條件。

4 動態窗口法

4.1 速度搜索

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

1）移動平臺速度約束

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

2）移動平臺加速度約束該項約束是由于移動平臺的電機性能和外界環境影響決定的。

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

3）障礙物約束

考慮障礙物，主要是由于移動平臺在運動過程中會遇到障礙，當遇到障礙時，要求其必須緊急停止。

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

4）前后輪速度約束：

該項約束主要是考慮前后輪的速度。對于第一種驅動方式，其中心位置的速度才是速度域中所選定的速度，所以需要根據前后輪的速度對其進行相應的約束；對于第二，三種驅動方式，主要是考慮非中心位置輪的舵輪處的速度，該處以第一種驅動方式為例。

4.2 評價函數

設計評價函數的目的是作為速度空間選擇的指標或者標準。根據比較評價函數，選擇其中最合適的速度。其設計原則：根據方位角偏差、距離障礙物最短距離、速度評價函數和相應的權重，規劃路徑使其最快、最準確的到達目標位置。

引入面積評價函數：

引入路徑偏差評價函數：

綜上得到評價函數如下：

式中：head(v，ω)，用于評價方位角偏差的評價函數，該方位角偏差不是與目標點的方位角偏差，而是與下一時刻的預定軌跡之間的偏差；vel(v，ω)，用于評價當前速度的評價函數；σ，表示平滑函數；α，β，γ和μ，分別表示對應的權重。其整體算法流程如圖6所示。

圖6 算法流程圖

5 軌跡跟蹤仿真

在實際應用中，多采用直線和標準圓弧組合的方式，在直線與圓弧的交點處會發生曲率突變的情況，本研究假設不發生突變。首先借助MATLAB軟件進行仿真，比較上述三種驅動方式中心點的軌跡跟蹤情況，不考慮窗口算法，取路徑函數如下，最大線速度vmax=0.8m/s，最大加速度amax=1.2m/s2，最大角速度ωmax=10°/s，最大角加速度βmax=10°/s2。

表1 算法參數設置

其結果如圖7所示。

圖7 第一次軌跡跟蹤仿真結果

從中可以得到：三種驅動方式與預定軌跡的重合度都很好，但是在預定軌跡圓弧與直線的交點處會發生一定的突變；第三種與第二種驅動方式的軌跡是一樣的。

引入動態窗口算法，本研究中不存在障礙物，因此，之后的結果如圖8所示。

可以得到：經過改進的動態窗口法之后，行進路徑點與原預設路徑交錯，可以看出，其與預設路徑更逼近。但是，可以得到圖8(a)圖更加逼近預設路徑，因為其采用的是移動平臺的中心點作為移動參考點，而圖8(b)、圖8(c)采用的是前后輪作為移動參考點，這樣在實際情況下會出現較大的偏移。

圖8 第二次軌跡跟蹤仿真簡圖

6 樣機實驗

為進一步驗證仿真結果，結合相關知識，將得到的控制方法應用在搭建的樣機上，進行進一步的驗證。樣機與底部舵輪布局模型如圖9所示。

圖9 實驗樣機

圖10 采集數據結果

樣機行走路線依然采用上述的直線與標準圓弧的組合形式。主要是驗證預定軌跡與通過數據采集的軌跡之間的偏差。分別進行三種驅動形式下的實驗，通過里程計反饋舵輪的轉角與速度。

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

7 總結

7.1 創新點

多數研究主要是針對于單舵輪的情況，對于雙舵輪的運動模型相關研究較少。本研究通過相應的簡化方法，提出一種雙舵輪前后驅動方式，為工業上的應用提供一定的參照。

引入覆蓋面積法，評價實際軌跡與預定軌跡之間的逼近程度，將其作為全局路徑規劃的評價指標。

考慮偏差，改進動態窗口法，將動態窗口法中的傳統評價函數中的方位角偏差函數改為目前時刻與下一時刻的目標位置之間的速度方向偏差。

基于傳統動態窗口法的局限性，本研究考慮覆蓋面積區域對整體規劃的影響，提出一種考慮全局與實際路徑情況的路徑規劃方法。

7.2 缺陷

該方法也存在一定的缺陷。在仿真過程中，做了一些假設條件；在樣機驗證環節，由于測量設備的不精確和測量偏差等外部因素，導致實驗數據只能逼近；在運動學建模中，由于雙舵輪驅動形式的復雜性，本研究做了部分簡化。

7.3 結語

本研究以雙舵輪驅動為研究對象，綜合比較常使用的三種雙舵輪驅動形式之間的優缺點，經過仿真驗證和實物驗證環節，推論出第一種驅動形式更合適實際情況的使用，但是第一種驅動形式存在復雜性，后續會繼續進行相關的研究。在路徑跟蹤過程中借鑒動態窗口法，并在傳統的基礎之上進行改進，最后提出一種適合本研究的方法，經過仿真與樣機驗證，存在可行性，后續還會繼續在算法的實時性和靈敏性上做進一步的修正。