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

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

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

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

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

1.1 三階貝塞爾曲線

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

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

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

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

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

1）曲線的參數(shù)化表達式為：

2）曲線經(jīng)過兩端的控制點：

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

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

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

1.2 軌跡規(guī)劃

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

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

根據(jù)三階貝塞爾曲線的定義可知，該規(guī)劃軌跡由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的方程為：

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

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

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

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

1.3 軌跡優(yōu)化求解

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

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

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

2 智能車軌跡規(guī)劃仿真驗證

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

1）給定控制點P1坐標，控制點P2坐標尋優(yōu)。

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

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

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

2）給定控制點P2坐標，控制點P1坐標尋優(yōu)。

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

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

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

3）控制點P1、P2坐標同時尋優(yōu)。

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

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

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

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

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

3 智能車位置估算算法

3.1 算法簡介

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

3.2 阿克曼轉(zhuǎn)向模型

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

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

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

由左、右側(cè)前后2個驅(qū)動輪的轉(zhuǎn)彎半徑相同可知：

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

由此可得，左、右側(cè)驅(qū)動輪的行駛距離Sl、Sr分別為：

式中：ηl、ηr為左、右側(cè)驅(qū)動輪的滾動角度；Rwheel為驅(qū)動輪半徑。

則左、右側(cè)驅(qū)動輪的轉(zhuǎn)彎半徑Rl、Rr分別為：

基于左、右側(cè)驅(qū)動輪轉(zhuǎn)彎半徑計算得到的整車中心點轉(zhuǎn)彎半徑R1、R2，分別為：

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

3.3 位置增量計算

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

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

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

根據(jù)智能車中心點轉(zhuǎn)彎半徑和該段軌跡內(nèi)航向角的變化量，計算得到該段軌跡的長度lA1A2為：

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

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

4 試驗驗證

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

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

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

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

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

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

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

根據(jù)4WID-4WIS智能車運行時慣性導(dǎo)航系統(tǒng)測量的航向角以及編碼器的脈沖數(shù)，利用所提出的位置估算算法對智能車軌跡進行推算，并與規(guī)劃規(guī)跡進行對比，結(jié)果如圖10所示。

圖10 4WID-4WIS智能車軌跡推算結(jié)果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）可以看出，推算軌跡與規(guī)劃軌跡基本吻合，通過計算得到推算軌跡的長度為39.698 m，與規(guī)劃軌跡長度39.785 m的相對誤差為0.22%。

5 結(jié)語

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

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

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

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