4WID-4WIS智能車(chē)阿克曼轉(zhuǎn)向軌跡規(guī)劃及位置估算

時(shí)培成，陳 旭，楊愛(ài)喜，章 亮

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

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

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

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

1.1 三階貝塞爾曲線

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

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

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

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

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

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

2）曲線經(jīng)過(guò)兩端的控制點(diǎn)：

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

3）起點(diǎn)和終點(diǎn)分別為曲線兩端的切點(diǎn)，切向量為：

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

1.2 軌跡規(guī)劃

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

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

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

由控制點(diǎn)P0的坐標(biāo)可得，第1條一階貝塞爾曲線P0P1的方程為：

由控制點(diǎn)P3的坐標(biāo)可得，第3條一階貝塞爾曲線P2P3的方程為：

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

由此可得，控制點(diǎn)P1的橫、縱坐標(biāo)X1、Y1的取值范圍為：

控制點(diǎn)P2的橫、縱坐標(biāo)X2、Y2的取值范圍為：

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

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

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

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

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

2 智能車(chē)軌跡規(guī)劃仿真驗(yàn)證

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

1）給定控制點(diǎn)P1坐標(biāo)，控制點(diǎn)P2坐標(biāo)尋優(yōu)。

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

圖3 改變控制點(diǎn)P2所得的4WID-4WIS智能車(chē)軌跡規(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)軌跡，此時(shí)控制點(diǎn)P2的縱坐標(biāo)為8 m。由此可得，4個(gè)控制點(diǎn)P0、P1、P2和P3的坐標(biāo)分別為(0，0)，(10，0)，(20，9)和(20，30)m。

2）給定控制點(diǎn)P2坐標(biāo)，控制點(diǎn)P1坐標(biāo)尋優(yōu)。

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

圖4 改變控制點(diǎn)P1所得的4WID-4WIS智能車(chē)軌跡規(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)軌跡，此時(shí)控制點(diǎn)P1的橫坐標(biāo)為13 m。由此可得，4個(gè)控制點(diǎn)P0、P1、P2和P3的坐標(biāo)分別為(0，0)，(13，0)，(20，15)和(20，30)m。

3）控制點(diǎn)P1、P2坐標(biāo)同時(shí)尋優(yōu)。

由上述結(jié)果可知，僅改變控制點(diǎn)P1或P2的坐標(biāo)，無(wú)法真正得到最優(yōu)軌跡，這是因?yàn)橹桓淖円粋€(gè)點(diǎn)的坐標(biāo)有可能會(huì)漏掉曲率差值真正最小的曲線。因此，應(yīng)按照上文所述方法，通過(guò)同時(shí)改變控制點(diǎn)P1的橫坐標(biāo)和控制點(diǎn)P2的縱坐標(biāo)來(lái)尋找真正的最優(yōu)軌跡。通過(guò)同時(shí)改變控制點(diǎn)P1、P2的坐標(biāo)規(guī)劃得到600條不同的軌跡，結(jié)果如圖5所示。其中：圖5（a）所示為規(guī)劃得到的600條軌跡；圖5（b）所示為每條規(guī)劃軌跡的最大曲率與最小曲率的差值變化情況；圖5（c）所示為最優(yōu)軌跡，即第406條軌跡。

圖5 同時(shí)改變控制點(diǎn)P1、P2所得的4WID-4WIS智能車(chē)軌跡規(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)軌跡，此時(shí)控制點(diǎn)P1的橫坐標(biāo)為13 m，控制點(diǎn)P2的縱坐標(biāo)為16 m。由此可得，4個(gè)控制點(diǎn)P0、P1、P2和P3的坐標(biāo)分別為(0，0)，(13，0)，(20，16)和(20，30)m。

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

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

3 智能車(chē)位置估算算法

3.1 算法簡(jiǎn)介

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

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

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

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

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

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

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

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

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

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

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

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

3.3 位置增量計(jì)算

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

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

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

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

將長(zhǎng)度為lA1A2的圓弧近似為長(zhǎng)度為L(zhǎng)的線段進(jìn)行計(jì)算，由圖7可得：

則可推算出終點(diǎn)A2處智能車(chē)的位姿為：

4 試驗(yàn)驗(yàn)證

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

圖8 4WID-4WIS智能車(chē)實(shí)物Fig.8 Physical object of 4WID-4WIS intelligent vehicle

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

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

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

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

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

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

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

由圖10（a）和圖10（b）可得，估算得到的終點(diǎn)坐標(biāo)為（19.962，29.941）m，與給定的終點(diǎn)坐標(biāo)（20，30）m相比，橫向位置的相對(duì)誤差為0.19%，縱向位置的相對(duì)誤差為0.20%。由圖10（c）可以看出，推算軌跡與規(guī)劃軌跡基本吻合，通過(guò)計(jì)算得到推算軌跡的長(zhǎng)度為39.698 m，與規(guī)劃軌跡長(zhǎng)度39.785 m的相對(duì)誤差為0.22%。

5 結(jié)語(yǔ)

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

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

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

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