4WID-4WIS智能車阿克曼轉向軌跡規劃及位置估算

時培成，陳 旭，楊愛喜，章 亮

（1.安徽工程大學汽車新技術安徽省工程技術研究中心，安徽 蕪湖 241000；2.浙江大學工程師學院，浙江 杭州 310000）

軌跡規劃是智能車的基本功能之一，其目的是通過傳感器感知周圍環境并利用主動避障功能找到一條從起點到終點的最佳路徑［1］。根據環境條件的不同，軌跡規劃方法分為全局規劃方法和局部規劃方法。常用的全局規劃方法有A*算法［2］、Dijkstra（狄克斯特拉）算法［3］和蟻群算法［4］等；常用的局部規劃方法有多項式曲線法［5］、人工勢場法［6］、貝塞爾曲線法［7］和B樣條曲線法［8］等。其中，貝塞爾曲線具有曲率連續的優點，只需要確定控制點的坐標即可獲得最優軌跡，其計算量較小，從而得到了廣泛應用。張新鋒等［9］基于貝塞爾曲線對智能商用車的換道避障軌跡進行了規劃，并利用遺傳算法選取最優避障路徑，但是其在路徑尋優中并未考慮遺傳算法對參數尋優的隨機性，且沒有通過實車驗證。陳成等［10］基于四階貝塞爾曲線探討了無人車軌跡規劃的可行性，并推導了相應的曲率約束條件，同時利用序列二次規劃（sequential quadratic programming,SQP）算法選取最優軌跡，但其在軌跡規劃中對車輛的動力學約束不足。Jolly等［11］利用三階貝塞爾曲線對機器人的軌跡規劃問題進行了研究，但其提出的規劃方法僅考慮了加速度約束，并未考慮曲率約束。高嵩等［12］針對無人車局部避障問題，基于貝塞爾曲線對無人車的避障路徑進行了規劃，其通過最小化曲線的曲率差值來選取曲線的控制點，并利用SQP算法選取最優避障路徑，同時結合實際道路情況進行了避障仿真實驗，但是其在規劃避障路徑時并未考慮無人車的速度規劃問題，在仿真實驗時也沒有考慮整車實際行駛情況對避障路徑的影響。

基于此，筆者針對四輪獨立驅動與四輪獨立轉向（four-wheel independent drive and four-wheel independent steering,4WID-4WIS）智能車的轉向行駛工況，在已知起點位置的情況下，給定終點位置和行駛方向，利用三階貝塞爾曲線規劃一系列軌跡，并采用優化函數獲取滿足智能車初始狀態約束、目標狀態約束和曲率連續約束且曲率差值最小的最優軌跡，使智能車按最優軌跡從起點移動到終點并保持給定的航向角。同時，提出一種位置估算算法，將軌跡微元近似（即將軌跡無限細分），以直線軌跡代替曲線軌跡，并利用慣性導航系統測量的航向角和編碼器的脈沖數對智能車的位置進行估算。

1 基于三階貝塞爾曲線的智能車軌跡規劃

1.1 三階貝塞爾曲線

貝塞爾曲線是法國工程師Bézier提出的一種適用于二維圖形應用程序的數學曲線［13］。貝塞爾曲線由一組控制點確定：將給定的控制點按順序連接構成多邊形，通過調整控制點的坐標來改變貝塞爾曲線的形狀，使曲線不斷逼近該多邊形。對于由n+1個控制點P0、P1、P2、…、Pn（其位置矢量分別為P0、P1、P2、…、Pn）確定的貝塞爾曲線，定義為：

式中：B(i，n)(t)為伯恩斯坦基函數［14］；t為比例，其決定了貝塞爾曲線組成點的數量。

鑒于本文基于三階貝塞爾曲線對4WID-4WIS智能車的阿克曼轉向軌跡進行規劃，則僅對該曲線的相關特性進行介紹。圖1所示為由4個控制點P0、P1、P2和P3唯一確定的三階貝塞爾曲線，依次連接4個控制點可得到3條一階貝塞爾曲線，分別為P0P1、P1P2和P2P3。

圖1 三階貝塞爾曲線示意Fig.1 Schematic diagram of third-order Bézier curve

三階貝塞爾曲線的性質［15］如下：

1）曲線的參數化表達式為：

2）曲線經過兩端的控制點：

p(0)=P0，p(1)=P3

3）起點和終點分別為曲線兩端的切點，切向量為：

p′(0)=3(P1-P0)，p′(1)=3(P3-P2)

1.2 軌跡規劃

針對4WID-4WIS智能車在阿克曼轉向工況下進行曲線行駛時，已知起點A，即第1個控制點P0的坐標為(X0，Y0，β0)，其中X0、Y0分別為起點A的橫、縱坐標，β0為起點A處的航向角；終點B，即第4個控制點P3的坐標為(X3，Y3，β3)，各變量的含義同上?；谌A貝塞爾曲線規劃軌跡，使智能車從起點A移動到指定的終點B，規劃的軌跡如圖2所示。

圖2 基于三階貝塞爾曲線的智能車軌跡規劃示意Fig.2 Schematic diagram of intelligent vehicle trajectory planning based on third-order Bézier curve

根據三階貝塞爾曲線的定義可知，該規劃軌跡由3條一階貝塞爾曲線和4個控制點確定，第1條一階貝塞爾曲線為控制點P0、P1的連線P0P1，第2條一階貝塞爾曲線為控制點P1、P2的連線P1P2，第3條一階貝塞爾曲線為控制點P2、P3的連線P2P3。延長P0P1和P2P3相交于O點，則控制點P1可以取直線AO上的任意一點，但不與A點和O點重合；控制點P2可以取直線BO上的任意一點，但不與B點和O點重合。

由控制點P0的坐標可得，第1條一階貝塞爾曲線P0P1的方程為：

由控制點P3的坐標可得，第3條一階貝塞爾曲線P2P3的方程為：

聯立式（2）和式（3）可得，一階貝塞爾曲線P0P1和P2P3的交點O的橫、縱坐標XO、YO分別為：

由此可得，控制點P1的橫、縱坐標X1、Y1的取值范圍為：

控制點P2的橫、縱坐標X2、Y2的取值范圍為：

通過以上方法，固定三階貝塞爾曲線的控制點P0、P3，在直線AO和BO上分別選擇2個點作為控制點P1和P2，即可獲得滿足智能車初始狀態約束、目標狀態約束以及曲率連續約束的一系列軌跡。

1.3 軌跡優化求解

本文采用最優函數來求解滿足上述約束條件的最優參數，從而獲得最優軌跡。所構建的優化函數為J(P1,P2)：

式中：t1、t2分別為三階貝塞爾曲線達到最大曲率與最小曲率時t的取值。

函數J（P1，P2）的含義為：通過優化控制點P1和P2來使所規劃軌跡的最大曲率與最小曲率的差值最小，即其曲率變化最小，從而使軌跡更加平滑，以便車輛平順、穩定地行駛［16］。

2 智能車軌跡規劃仿真驗證

在4WID-4WIS智能車阿克曼轉向軌跡規劃中，首先要確定其初始位姿和目標位姿，然后根據上文方法進行軌跡規劃，最后在所規劃的一系列軌跡中選擇1條最為平滑的軌跡，即最優軌跡。本文以4WID-4WIS智能車的初始位置為坐標系原點，此時航向角為0°，即車身與x軸平行，則其初始位姿為（0 m，0 m，0°）；目標位置的坐標為（20，30）m，航向角為90°，即車身與y軸平行，則目標位姿為（20 m，30 m，90°）；所規劃軌跡由200個點組成。通過在MATLAB軟件中編程規劃得到從起點（0，0）m到終點（20，30）m的一系列軌跡，并在這些軌跡中選擇最大曲率與最小曲率差值最小的一條軌跡作為最優軌跡。

1）給定控制點P1坐標，控制點P2坐標尋優。

固定控制點P1的坐標，由智能車在起點處的航向角為0°可確定P1的坐標為(10，0)m；由智能車在終點處的航向角為90°可確定控制點P2的橫坐標為20 m。通過改變P2的縱坐標規劃得到30條不同的軌跡，結果如圖3所示。其中：圖3（a）所示為規劃得到的30條軌跡；圖3（b）所示為每條規劃軌跡的最大曲率與最小曲率的差值變化情況；圖3（c）所示為最優軌跡，即第9條規劃軌跡。

圖3 改變控制點P2所得的4WID-4WIS智能車軌跡規劃仿真結果Fig.3 Trajectory planning simulation results of 4WID-4WIS intelligent vehicle obtained by changing control pointP2

由圖3可以得出，第9條規劃軌跡的最大曲率與最小曲率的差值最小，為0.046 3，則該軌跡即為最優軌跡，此時控制點P2的縱坐標為8 m。由此可得，4個控制點P0、P1、P2和P3的坐標分別為(0，0)，(10，0)，(20，9)和(20，30)m。

2）給定控制點P2坐標，控制點P1坐標尋優。

固定控制點P2的坐標，由智能車在終點處的航向角為90°可確定控制點P2的坐標為(20，15)m；由智能車在起點處的航向角為0°可知，控制點P1的縱坐標為0 m。通過改變P1的橫坐標規劃得到20條不同的軌跡，結果如圖4所示。其中：圖4（a）所示為規劃得到的20條軌跡；圖4（b）所示為每條軌跡的最大曲率與最小曲率的差值變化情況；圖4（c）所示為最優軌跡，即第13條軌跡。

圖4 改變控制點P1所得的4WID-4WIS智能車軌跡規劃仿真結果Fig.4 Trajectory planning simulation results of 4WID-4WIS intelligent vehicle obtained by changing control pointP1

由圖4可以得出，第13條規劃軌跡的最大曲率與最小曲率的差值最小，為0.045 6，則該軌跡即為最優軌跡，此時控制點P1的橫坐標為13 m。由此可得，4個控制點P0、P1、P2和P3的坐標分別為(0，0)，(13，0)，(20，15)和(20，30)m。

3）控制點P1、P2坐標同時尋優。

由上述結果可知，僅改變控制點P1或P2的坐標，無法真正得到最優軌跡，這是因為只改變一個點的坐標有可能會漏掉曲率差值真正最小的曲線。因此，應按照上文所述方法，通過同時改變控制點P1的橫坐標和控制點P2的縱坐標來尋找真正的最優軌跡。通過同時改變控制點P1、P2的坐標規劃得到600條不同的軌跡，結果如圖5所示。其中：圖5（a）所示為規劃得到的600條軌跡；圖5（b）所示為每條規劃軌跡的最大曲率與最小曲率的差值變化情況；圖5（c）所示為最優軌跡，即第406條軌跡。

圖5 同時改變控制點P1、P2所得的4WID-4WIS智能車軌跡規劃仿真結果Fig.5 Simulation results of trajectory planning of 4WID-4WIS intelligent vehicle obtained by changing control pointP1andP2at the same time

由圖5可以得出，第406條規劃軌跡的最大曲率與最小曲率的差值最小，為0.045 2，則該軌跡即為最優軌跡，此時控制點P1的橫坐標為13 m，控制點P2的縱坐標為16 m。由此可得，4個控制點P0、P1、P2和P3的坐標分別為(0，0)，(13，0)，(20，16)和(20，30)m。

基于不同尋優方式獲得的智能車最優軌跡控制點的仿真結果如表1所示。由表中數據可知，僅對控制點P1的橫坐標或控制點P2的縱坐標進行尋優，無法獲得曲率差值真正最小的最優軌跡，須同時對控制點P1的橫坐標和控制點P2的縱坐標進行尋優，才能得到滿足約束條件的最優軌跡。

表1 4WID-4WIS智能車最優軌跡控制點仿真結果對比Table 1 Comparison of simulation results of optimal trajectory control points of 4WID-4WIS intelligent vehicle

3 智能車位置估算算法

3.1 算法簡介

在智能車運行時，需要對其位置進行估算，以便實時掌握其運行姿態。常用定位算法通常采用慣性導航系統和編碼器來測量數據。如：文獻［17］針對輪式機器人，提出了一種改進的軌跡推算方法，即通過編碼器測定2個同軸驅動輪的速度，從而建立一種新的軌跡推算理論模型，以盡可能去除外部干擾，提高定位精度，但該方法只是改變了推算模型，并未解決使用單一編碼器誤差較大的問題。文獻［18］通過測量差動驅動機器人的2個同軸驅動輪的角速度，并利用其差動驅動系統的運動規律推算驅動輪在下一個采樣時刻的位置，但該方法僅僅在理論上進行了推導，并未通過機器人的實際行駛情況來驗證理論模型的正確性。為此，筆者提出了一種基于慣性導航系統和編碼器的4WID-4WIS智能車位置估算算法。首先，在根據比例t劃分的每小段軌跡內，通過慣性導航系統測量獲得智能車航向角的變化量，并利用編碼器記錄4個驅動電機的脈沖數，根據航向角的變化量和編碼器的脈沖數來計算智能車中心點的轉彎半徑；然后，根據中心點轉彎半徑和航向角變化量，計算該段軌跡內智能車的轉向弧長；最后，將轉向弧線分段近似為線段，并計算各段軌跡的長度，同時根據各段軌跡的長度和航向角的變化量，計算智能車的位置增量，實現其位置估算。

3.2 阿克曼轉向模型

4WID-4WIS智能車的阿克曼轉向模型（左轉）如圖6所示。圖中：H為整車轉向中心，D為左、右側驅動輪輪距，W為前后驅動輪軸距，R為整車中心點轉彎半徑，R1為左側驅動輪轉彎半徑，Rr為右側驅動輪轉彎半徑，δl為左側驅動輪轉角，δr為右側驅動輪轉角。

當4WID-4WIS智能車運行時，利用其驅動輪上編碼器的脈沖數和慣性導航系統測量的航向角來估算其位姿。設編碼器旋轉一周的脈沖數為N，在每小段軌跡內，慣性導航系統測量得到的航向角變化量為dθ，編碼器的脈沖數為M，則有：

式中：ηl1、ηl2、ηr1和ηr1為智能車運行時左、右側前后驅動輪的滾動角度；Ml1、Ml2、Mr1和Mr1為左、右側前后驅動輪上編碼器的脈沖數。

由左、右側前后2個驅動輪的轉彎半徑相同可知：

在實際運行過程中，因驅動輪的加工材質不完全相同導致不同驅動輪與地面的摩擦不同［19］，以及硬件上的干擾信號導致編碼器返回值受影響等因素，同側驅動輪上編碼器的脈沖數也可能不同。為了防止因驅動輪打滑而造成的編碼器脈沖數的增加，本文取同側編碼器中較小的脈沖數，同時在運行過程中觀察脈沖數的變化，若發現脈沖數增加明顯，則說明側滑現象嚴重，應重新進行分析。

由此可得，左、右側驅動輪的行駛距離Sl、Sr分別為：

式中：ηl、ηr為左、右側驅動輪的滾動角度；Rwheel為驅動輪半徑。

則左、右側驅動輪的轉彎半徑Rl、Rr分別為：

基于左、右側驅動輪轉彎半徑計算得到的整車中心點轉彎半徑R1、R2，分別為：

由于存在不可抗力的影響因素，基于左、右側驅動輪轉彎半徑求得的整車中心點轉彎半徑可能不同，本文取二者的平均值，即：

3.3 位置增量計算

對于輪式智能車來說，傳統的軌跡推算過程是將曲線路徑進行微元近似，即把曲線無限細分，并將每一小段路徑近似成線段進行計算，所有線段距離加和值即為擬合的曲線長度［20］，其原理如圖7所示。

圖7 4WID-4WIS智能車軌跡推算原理Fig.7 Calculation principle of trajectory of 4WID-4WIS intelligent vehicle

如圖7所示，假設4WID-4WIS智能車沿著圓弧形軌跡從起點A1(x1，y1，θ1)運行到終點A2(x2，y2，θ2)，其中θ1、θ2分別為智能車在點A1和A2處的航向角，dθ為點A2相對于點A1的航向角變化量，即：

根據智能車中心點轉彎半徑和該段軌跡內航向角的變化量，計算得到該段軌跡的長度lA1A2為：

將長度為lA1A2的圓弧近似為長度為L的線段進行計算，由圖7可得：

則可推算出終點A2處智能車的位姿為：

4 試驗驗證

為了驗證所提出的軌跡規劃方法以及位置估算算法的可行性，在實車試驗平臺開展相關試驗。試驗中4WID-4WIS智能車的初始位姿和目標位姿與仿真時一致，即分別為（0 m，0 m，0°）和（20 m，30 m，90°）。同時，為了避免智能車運行時四輪驅動和四輪轉向相互干涉影響，在硬件設計上，設置主控制板通過控制器局域網（controller area network，CAN）與電機驅動器進行通信：通過CANA與驅動電機驅動器通信，通過CANB與轉向電機驅動器通信，以保證驅動和轉向在控制上相互獨立。在軟件設計上，驅動電機在速度環、電流環模式下工作，主控制板通過發送目標速度、電流來控制驅動電機；轉向電機在位置環模式下工作，主控制板通過發送目標位置來控制轉向電機，以避免驅動和轉向在工作時相互影響。試驗用4WID-4WIS智能車實物如圖8所示。

圖8 4WID-4WIS智能車實物Fig.8 Physical object of 4WID-4WIS intelligent vehicle

試驗用4WID-4WIS智能車采用DSP28335芯片作為主控芯片，慣性導航系統為格納微公司生產的GNW-SurPass-A100航姿參考系統，驅動電機（實車上為輪轂電機）編碼器為增量式編碼器。該智能車的具體參數如表2所示。

表2 4WID-4WIS智能車的參數Table 2 Parameters of 4WID-4WIS intelligent vehicle

在起點處對4WID-4WIS智能車發送目標位姿指令，其主控制器根據軌跡規劃算法選取最優軌跡并控制智能車根據規劃軌跡從起點運行到終點。在運行過程中，利用慣性導航系統測量智能車的航向角變化量，并讀取增量式編碼器的脈沖數，試驗數據如圖9所示。

圖9 4WID-4WIS智能車運行過程中的航向角和脈沖數Fig.9 Navigation angle and pulse number of 4WID-4WIS intelligent vehicle during driving

由圖9（a）可知，4WID-4WIS智能車在初始位置處保持設定的航向角0°，在按照規劃軌跡運行時，其航向角不斷變化，到達目標位置時，由于地面摩擦力以及輪胎打滑等因素的影響，其最終的航向角為89.876°，與給定航向角90°的誤差為0.13%。由圖9（b）可知，在劃分的每段軌跡內，脈沖數越多，說明智能車的運行軌跡長度越長；脈沖數越少，說明智能車的運行軌跡長度越短。根據編碼器的總脈沖數（本實驗中為110 050），即可推算智能車的總運行軌跡長度。

根據4WID-4WIS智能車運行時慣性導航系統測量的航向角以及編碼器的脈沖數，利用所提出的位置估算算法對智能車軌跡進行推算，并與規劃規跡進行對比，結果如圖10所示。

圖10 4WID-4WIS智能車軌跡推算結果Fig.10 Calculation results of trajectory of4WID-4WIS intelligent vehicle

由圖10（a）和圖10（b）可得，估算得到的終點坐標為（19.962，29.941）m，與給定的終點坐標（20，30）m相比，橫向位置的相對誤差為0.19%，縱向位置的相對誤差為0.20%。由圖10（c）可以看出，推算軌跡與規劃軌跡基本吻合，通過計算得到推算軌跡的長度為39.698 m，與規劃軌跡長度39.785 m的相對誤差為0.22%。

5 結語

本文針對4WID-4WIS智能車的轉向行駛工況，做了以下研究。

1）基于阿克曼轉向原理，利用三階貝塞爾曲線對智能車進行軌跡規劃，并利用最優函數選取最優軌跡，使得規劃軌跡的最大曲率和最小曲率差值最小，滿足初始狀態約束、目標狀態約束以及曲率連續約束，實現智能車按規劃軌跡平滑行駛。

2）提出一種新的位置估算算法，綜合利用慣性導航系統和編碼器來估算智能車行駛時的位置，并對其軌跡進行推算。試驗結果表明，智能車可以按照規劃軌跡行駛到指定終點，且橫向位置估算誤差為0.19%，縱向位置估算誤差為0.20%，運行軌跡長度推算誤差為0.22%。

結果表明，所提出的軌跡規劃方法和位置估算算法可為其他移動機器人等的軌跡規劃和位置估算提供一定的參考。