宋國浩，黃晉英，蘭艷亭

（中北大學(xué)機械與動力工程學(xué)院，太原030051）

基于Dubins路徑的智能車輛路徑規(guī)劃算法*

宋國浩，黃晉英，蘭艷亭

（中北大學(xué)機械與動力工程學(xué)院，太原030051）

路徑規(guī)劃是車輛智能化的核心問題之一，而所有路徑均可分解為簡單的Dubins路徑。在Dubins路徑的思想下對智能車輛的行駛路徑進行分段研究，并利用經(jīng)典PID控制對該算法的執(zhí)行性能進行檢驗。研究表明:算法能計算出車輛行駛的最短路徑，減少了車輛行駛的路徑長度，縮短了行駛時間，減少了控制系統(tǒng)的計算量，提高了車輛執(zhí)行系統(tǒng)的執(zhí)行力度，降低了執(zhí)行誤差，對最優(yōu)路徑具有較好的選擇性。

智能車，路徑規(guī)劃，Dubins路徑，最短路徑

0　引言

路徑規(guī)劃應(yīng)用在很多領(lǐng)域，例如:軍事無人機、航天探測機器人、智能車輛以及監(jiān)視和偵察等工作［1-3］。路徑規(guī)劃在現(xiàn)代汽車領(lǐng)域中是一個研究熱門領(lǐng)域，需要考慮多方面的因素，如:汽車自身約束條件，車輛行駛環(huán)境的約束以及其他的行駛問題。在路徑規(guī)劃中，首先應(yīng)考慮車輛的可行駛性，在對車輛行駛路線進行規(guī)劃時，應(yīng)保證其安全行駛的前提下，盡可能大地規(guī)劃出車輛行使范圍。在保證車輛安全行駛的問題中，需要使車輛自主地繞開其他影響車輛行駛的物體，使車輛避免與障礙物相撞。路徑規(guī)劃算法應(yīng)具有精確性，占有較小的內(nèi)存，并滿足實時性的要求，在執(zhí)行過程中沒有明顯的延時問題［4-5］。此外，為了使行駛路徑達到最優(yōu)，提高行駛效率，還應(yīng)縮短車輛行駛長度。

目前，在有關(guān)路徑規(guī)劃的研究中，如張明環(huán)等［6］提出的觸須算法，此算法是在車輛行駛前，首先對車輛將要行駛的路線進行規(guī)劃，讓車輛按照規(guī)劃好的16*81條可使用的路徑行駛，這樣可以使車輛節(jié)省大量的反應(yīng)時間，但卻不能夠處理突變情況，研究背景過于理想化；王凱等［7］提出了改進的人工勢場法，將此算法應(yīng)用在智能車路徑規(guī)劃中的避障環(huán)節(jié)，解決了傳統(tǒng)人工勢場法在路徑規(guī)劃中易陷入局部極小值的問題，具有一定的實時性，但其受限于所用傳感器性能的影響，其作用范圍較小，且易受外界環(huán)境影響Li J J等［8］提出了協(xié)調(diào)避障算法，在車輛行駛過程中，應(yīng)用一階運動學(xué)、二階動力學(xué)，對障礙物進行無速度監(jiān)測并繞過障礙物，但其將所有障礙物作為靜態(tài)處理，不具有靈活性，可能存在兩車同時避讓卻無處可避的局面。

本文根據(jù)Dubins路徑規(guī)劃算法對車輛行駛路線進行規(guī)劃控制，此種算法能很好地決策出車輛在行駛過程中的最優(yōu)路徑，并可以解決多障礙物間的避障問題，具有良好的實時性，延時性較小。

1　路徑的選擇

路徑規(guī)劃的主要目的是尋求一條安全、快捷的行駛路線，使車輛完成行駛目的。一般情況下，車輛行駛在已知或者部分已知的區(qū)域內(nèi)，即已知一些靜態(tài)障礙物?，F(xiàn)用P（x，y，θ，v，a）來表示車輛行駛狀態(tài)，（x，y）表示車輛行駛位置，（θ，v，a）中參數(shù)分別表示車輛行駛偏向角、行駛速度和行駛加速度。若車輛從起點P0行駛到終點P2的路徑為R（q），則可以近似表示為:

式中R（q）為產(chǎn)生的路徑，q為路徑參數(shù)，表示路徑的長度變量（0≤q≤s）或路徑中的角度變量（0≤q≤θ），q的具體值取決于行駛中的路況。

對上式進行詳細描述如下:

當(dāng)車輛行駛過程中，遇到障礙物時，可通過控制系統(tǒng)及時改變參數(shù)（θ，v，a）來使車輛繞開障礙物。

當(dāng)然，車輛在行駛過程中，不只有已知的障礙物，還有其他的約束條件，如:在正常行駛條件下，最小時間、最小路徑等作為其約束條件。用ψ表示約束條件，則路徑可表示為

其控制原理示意圖如圖1所示:

圖1　路徑規(guī)劃示意圖

路徑規(guī)劃中的運動學(xué)模型，在兩自由度下的運動學(xué)特性和當(dāng)前狀態(tài)可表示為:式中，v是車輛行駛速度，θ為水平角，ω為車輛轉(zhuǎn)動角速度。

我們期望車輛在行駛過程中能繞過障礙物且最終回到原行駛軌道上。則車輛行駛路徑可以簡化為Dubins路徑:一個圓?。–路徑）或兩個相切圓?。–C路徑）或兩個圓弧通過一個共同的相切直線連接（CLC路徑）。這是Dubins證明的兩個位置點的最短路徑［9］，C表示圓弧段，L表示與圓弧段相切的直線段?？梢钥闯鲎詈笠环N路徑包含前兩種路徑，本文對CLC路徑進行研究，即車輛行駛路徑為起始轉(zhuǎn)彎、直線行駛、終止轉(zhuǎn)彎。分別對CLC路徑中的三段Dubins路徑建立對應(yīng)的坐標(biāo)系，坐標(biāo)系分別為O0、O1、O2。每個基本坐標(biāo)定義為O=［t n］，t是與速度矢量平行的單位切矢量，n是與t垂直的單位法矢量。圖2為CLC路徑幾何圖:

圖2　Dubins路徑

矢量r0和r2的模表示為初始與終止的轉(zhuǎn)彎半徑，其正負代表車輛是向左轉(zhuǎn)彎還是向右轉(zhuǎn)彎，同時也確定了運動曲率Q的正負，每個矢量定義如下:

式中:k0是初始轉(zhuǎn)彎曲率，k2是終止轉(zhuǎn)彎曲率，l是直線行駛的長度。

則車輛行駛時的速度矢量在各坐標(biāo)系下的變換關(guān)系為:

式中:R（θ）是從起始坐標(biāo)系變換到終止坐標(biāo)系的旋轉(zhuǎn)矩陣。所以總的旋轉(zhuǎn)角θ可表示為:

又因為連接矢量a1與a0、a2垂直。

t1是a1基矢量；θ0是起始圓弧對應(yīng)的角度。

系統(tǒng)在不同坐標(biāo)下對車輛行駛的位置進行定義時，在起始坐標(biāo)系O0下定義起點P0與終點P2的相對位置，如下式表示:

在起始坐標(biāo)系內(nèi)用矢量的和表示這個位置矢量:

等式左邊表示從起始圓弧中心到終止圓弧中心之間的矢量，所以:

等式中d表示兩圓弧中心之間的距離。

為了減少系統(tǒng)所處理數(shù)據(jù)的工作量，使系統(tǒng)更加高效地完成車輛在行駛時路徑選擇減少計算的誤差率，希望整個過程能夠在同一坐標(biāo)系下進行運算，其基礎(chǔ)就是將連接各坐標(biāo)系的連接矢量a0、a1、a2表示在同一坐標(biāo)系下，則三者表示在起始坐標(biāo)系O0中如下:

則，式（14）在轉(zhuǎn)換坐標(biāo)下，可以表示為:

由于R（θ）為旋轉(zhuǎn)矩陣，所以:

即:

顯然，若路徑可行，則

由式（17）和式（18）可得:

又因轉(zhuǎn)換矩陣為:

可以得到關(guān)于角度θ0的等式如下:

其中:

由已知式（10）求得終止角度如下:

所以，車輛經(jīng)過一個CLC路徑行駛的總長度為:

車輛從所求得的可行性的行駛路徑中，比較選擇最短路徑來作為車輛的行駛路徑，即min（L）。

而車輛從坐標(biāo)系O0行駛到坐標(biāo)系O2后，其在兩個坐標(biāo)系中存在的關(guān)系為:

根據(jù)式（26）可知，車輛在行駛過程中，通過坐標(biāo)轉(zhuǎn)換可以使用同一坐標(biāo)進行表示，等式中矩陣B為齊次變換坐標(biāo)矩陣。

2　避障系統(tǒng)控制

由圖3可知車輛在行駛過程中，可能存在同時與多個障礙物相遇的問題，在解決此類問題中，首先根據(jù)Dubins最短路徑算法確定車輛與障礙物間的最短距離以及相對速度。相對速度的表示則可由下式給出:

v0表示車輛行駛的速度，vi表示第i個障礙物的行駛速度，v0i表示第i個障礙物相對于車輛的行駛速度。則第i個障礙物相對于車輛的最短相遇距離矢量如下式所示:

H0i表示車輛與第i個障礙物之間的直線距離，θ0i表示車輛與第i個障礙物行駛的速度方向之間的夾角。S0i表示車輛與第i個障礙物相遇時的最短距離。

圖3　躲障礙物相遇示意圖

因此，若要保證車輛在行駛中的安全性與可行駛，需要根據(jù)監(jiān)測的情況對車輛行駛速度的方向θ0進行調(diào)整，使得S0i大于車輛的安全距離L1、L2。

在求解與多個障礙物相遇的問題中，通常需要構(gòu)造Lyapunov函數(shù)來確定使用函數(shù)的穩(wěn)定性［10］:

其中，Γ0是車輛行駛速度方向與系統(tǒng)期望車輛行駛速度方向之間的角度差，即車輛行駛速度方向角度誤差；Γ1、Γ2分別為車輛由行駛速度方向向左或向右旋轉(zhuǎn)到避開障礙物時的安全行駛速度方向的角度。則車輛與多個障礙物相遇的狀況下，車輛的行駛速度方向Γ的角速度可表示為:

上式也可表述為控制車輛避障系統(tǒng)的算法。由此可知，

其中:

車輛在行駛過程中，控制系統(tǒng)根據(jù)監(jiān)測裝置傳遞的信息進行控制，監(jiān)測值用Ω表示，其輸出為高電平1時，表示前方有障礙物；輸出為低電平0時，表示前方無障礙物。而Ω0、Ω1、Ω2分別表示車輛左、中、右3個方向的監(jiān)測值。則車輛行駛決策如表1。

表1　車輛行駛決策表

3　實驗仿真

①仿真系統(tǒng)搭建。車輛控制系統(tǒng)根據(jù)Dubins路徑進行路徑規(guī)劃，并將此算法應(yīng)用Matlab軟件進行仿真。首先對道路進行設(shè)計，為了使道路狀況盡可能接近現(xiàn)實路況，設(shè)計道路為15 m*15 m的矩形場地，并與改進的人工勢場法［7］進行對比；然后利用經(jīng)典PID控制對系統(tǒng)的跟隨情況進行檢驗。

將車輛的舵機作為被控對象，圖4為車輛對輸入信號的跟隨響應(yīng)控制的方框圖:

圖4　車輛對輸入信號的跟隨響應(yīng)控制方框圖

根據(jù)車輛的控制方案圖可得方塊圖如下頁圖5所示。

②仿真結(jié)果及分析。車輛行駛過程中，首先確定目標(biāo)方向，通過轉(zhuǎn)向、直行等過程繞過監(jiān)測到的障礙物，并在整個過程中實時更新車輛所在位置的坐標(biāo)。仿真結(jié)果如下，其中:黑色為Dubins算法行駛路徑，藍色為對比算法行駛路徑。

圖5　車輛跟隨響應(yīng)控制方塊圖

設(shè)計如圖6所示典型路況，場地內(nèi)設(shè)有各種典型矩形障礙物，并包括“一”字形障礙物和“U”形障礙物，檢驗車輛在典型路況下的行駛路徑，驗證Dubins路徑規(guī)劃算法的可行性。圖7中設(shè)置有隨機障礙物，檢驗車輛在復(fù)雜路況下的行駛路徑。從兩圖中可以看出車輛在最短路徑下對車輛行駛方位進行調(diào)整，在保證車輛行駛安全的條件下，能對路徑進行最優(yōu)選擇。

圖6　典型路況下車輛行駛路線仿真圖

圖7　復(fù)雜路況下車輛行駛路線仿真圖

表2　典型路況仿真結(jié)果

表3　復(fù)雜路況仿真結(jié)果

通過典型路況和復(fù)雜路況仿真結(jié)果表明，Dubins路徑規(guī)劃算法能選擇最短路徑，縮短了車輛行駛時間，能為車輛提供更為合適的路徑，并避免了車輛在行駛時陷入局部最優(yōu)的傳統(tǒng)缺陷，車輛在繞過障礙物后可以回到與初始位置相對應(yīng)的路徑上，與其他算法對比可得，Dubins路徑規(guī)劃算法能更好地完成實驗?zāi)康摹?/p>

圖8、圖9反映了車輛實際行駛路線對控制器設(shè)計路線的跟隨情況，圖8可以看出車輛對系統(tǒng)設(shè)計的路線具有很好的執(zhí)行性，其執(zhí)行偏差在0.5 m以內(nèi)；圖9可看出，車輛的執(zhí)行誤差在4%以內(nèi)，而隨著車輛行駛狀況的逐漸穩(wěn)定，其執(zhí)行誤差會繼續(xù)降低，具有較高的可靠性。

圖8　車輛跟蹤曲線與參考曲線對比

圖9　車輛跟蹤曲線誤差

4　結(jié)論

在如今車輛智能化的趨勢下，路徑規(guī)劃已經(jīng)被越來越多的人們研究。本文在Dubins路徑思想下對車輛行駛路徑進行規(guī)劃，完成了該算法下的仿真實驗，并與其他算法進行了對比，算法簡潔可行。在此算法理論下，對車輛行駛路徑設(shè)計了兩種不同復(fù)雜程度的路況，滿足復(fù)雜路況下的行駛要求；對比結(jié)果表明，Dubins路徑規(guī)劃算法能計算出最短路徑，對最優(yōu)路徑具有良好的選擇性；車輛對此算法具有較高的執(zhí)行性，并具有較低的誤差率。

［1］吳友謙，裴海龍.基于Dubins曲線的無人直升機軌跡規(guī)劃［J］.計算機工程與設(shè)計，2011，32（4）:1426-1429.

［2］朱大奇，顏明重.移動機器人路徑規(guī)劃技術(shù)綜述［J］.控制與決策，2010，25（7）:961-967.

［3］高強，宋雨，呂東澔，等.多移動機器人路徑規(guī)劃仿真研究［J］.計算機仿真，2014，31（7）:325-329.

［4］GIORDANO P R，VENDITTELLI M.Shortest paths to obstacles for a polygonal Dubins car［J］.Robotics，IEEE Transactions on，2009，25（5）:1184-1191.

［5］SAVLA K，F(xiàn)RAZZOLI E，BULLO F.Traveling salesperson problems for the Dubins vehicle［J］.Automatic Control，IEEE Transactions on，2008，53（6）:1378-1391.

［6］張明環(huán)，張科.智能車避障觸須算法中的障礙物探測研究［J］.西北工業(yè)大學(xué)學(xué)報，2012，30（5）:763-767.

［7］王凱，宋星秀，張一聞.利用改進人工勢場法的智能車避障路徑規(guī)劃［J］.遼寧工程技術(shù)大學(xué)學(xué)報:自然科學(xué)版，2014，33（9）:1236-1239.

［8］LI J，ZHANG W，SU H，et al.Coordinated obstacle avoidance with reduced interaction［J］.Neurocomputing，2014，139: 233-245.

［9］ANDERSON R P，MILUTINOVIC D.A Stochastic approach to dubins vehicle tracking problems［J］.Automatic Control，IEEE Transactions on，2014，10:2081-2086.

［10］［英］楚拉多斯，［英］懷特，［英］尚穆加韋爾.無人機協(xié)同路徑規(guī)劃［M］.北京:國防工業(yè)出版社，2013.

Intelligent Vehicles Path Planning Algorithm Based on Dubins Path

SONG Guo-hao，HUANG Jin-ying，LAN Yan-ting
（School of Mechanical and Power Engineering，North University of China，Taiyuan 030051，Chian）

The path planning is one of the core issues of intelligent vehicles.All paths can be decomposed into Dubins path.This paper sectionally researches into the intelligent vehicles’travel path under the idea of Dubins path and carries out tests on the execution performance of the algorithm using PID control strategy.Researches showed that this algorithm can calculate the vehicles’shortest path，reduce the vehicles’path length，shorten the time of driving，reduce the computation amount of the control system，improve the enforcement of the vehicle execution system，reduce the execution error，and have a good selectivity of the optimal path.

intelligent vehicles，path planning，dubins path，the shortest path

TP273+.1

A

1002-0640（2016）06-0041-05

2015-05-10

2015-06-09

山西省科技攻關(guān)基金資助項目（20130321005-04）

宋國浩（1990-），男，山東曲阜人，在讀碩士研究生。研究方向：機械工程、智能車輛。