本發(fā)明涉及自動(dòng)駕駛技術(shù)領(lǐng)域，尤其涉及一種基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法。

背景技術(shù)：

路徑規(guī)劃是自動(dòng)駕駛車(chē)輛的核心技術(shù)之一，其作用是保證車(chē)輛安全的完成對(duì)全局路徑的跟蹤。在城市道路環(huán)境或高速公路環(huán)境中，自動(dòng)駕駛車(chē)輛不僅要平順?lè)€(wěn)定的在車(chē)輛所在車(chē)道內(nèi)行駛，還需要結(jié)合環(huán)境感知信息以及智能決策指令進(jìn)行換道、并線等較為復(fù)雜的動(dòng)作。在車(chē)道保持的工況下，為了保證車(chē)輛精確的跟隨車(chē)道的中心線及保持橫向穩(wěn)定性，需要將車(chē)輛約束在其所在車(chē)道內(nèi)；在換道工況下，需要保證車(chē)輛能夠精確的換入到目標(biāo)車(chē)道內(nèi)，避免出現(xiàn)騎線、壓線行駛的情況。為了達(dá)到上述目的，自動(dòng)駕駛車(chē)輛的路徑規(guī)劃需要依賴車(chē)輛與道路相對(duì)橫向位置的精確定位。有研究通過(guò)差分gps和高精度地圖相結(jié)合的方式來(lái)實(shí)現(xiàn)車(chē)輛與道路橫向位置關(guān)系的精確定位，但是這種方式受限于差分gps信號(hào)的作用范圍，以及高精度地圖的測(cè)繪。

現(xiàn)階段，能夠?yàn)樽詣?dòng)駕駛車(chē)輛提供道路邊沿、車(chē)道線等豐富的環(huán)境感知信息的三維激光雷達(dá)以及相機(jī)仍然是自動(dòng)駕駛車(chē)輛上最重要的傳感器。有必要利用現(xiàn)有傳感器，設(shè)計(jì)一種融合環(huán)境感知信息與一般gps定位信息的路徑規(guī)劃方法。

技術(shù)實(shí)現(xiàn)要素：

鑒于上述的分析，本發(fā)明旨在提供一種基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法，用以解決現(xiàn)有技術(shù)受限于差分gps信號(hào)的作用范圍以及高精度地圖的問(wèn)題。

本發(fā)明的目的主要是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的：

一種基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法，包括以下步驟：

步驟s1：獲取車(chē)體坐標(biāo)系下的車(chē)道中心線航向；

步驟s2：利用步驟s1中獲取的車(chē)道中心線航向?qū)?chē)輛航向進(jìn)行修正；

步驟s3：利用步驟s2修正后的車(chē)輛航向?qū)υ悸窂街械穆窂近c(diǎn)進(jìn)行校正；

步驟s4：基于三維激光雷達(dá)柵格地圖，獲取自動(dòng)駕駛車(chē)輛與道路的相對(duì)橫向位置關(guān)系；

步驟s5：根據(jù)步驟s4獲取的自動(dòng)駕駛車(chē)輛與道路的相對(duì)橫向位置關(guān)系生成虛擬約束，所述虛擬約束包括所在車(chē)道的邊界約束和鄰車(chē)道邊界約束；

步驟s6：根據(jù)步驟s5生成的虛擬約束和自動(dòng)駕駛系統(tǒng)的決策指令，生成期望路徑。

所述步驟s1還包括以下步驟：

步驟s101：判斷車(chē)道線檢測(cè)結(jié)果是否有效，若有效，執(zhí)行步驟s102；若無(wú)效，沿用上一周期車(chē)道線檢測(cè)結(jié)果；

步驟s102：獲取車(chē)體坐標(biāo)系下的車(chē)道中心線航向。

所述步驟s102還包括以下步驟：

步驟s1021：計(jì)算車(chē)道中心線的位置

其中，車(chē)道線檢測(cè)結(jié)果(x1,y1)、(x2,y2)、(x3,y3)與(x4,y4)為左右車(chē)道線在車(chē)體坐標(biāo)系下的坐標(biāo)點(diǎn)，(x1,y1)為車(chē)輛左前方近車(chē)點(diǎn)，(x2,y2)為左前方遠(yuǎn)車(chē)點(diǎn)，(x3,y3)為右前方近車(chē)點(diǎn)，(x4,y4)為右前方遠(yuǎn)車(chē)點(diǎn)；

(xc1,yc1)與(xc2,yc2)分別代表車(chē)道中心線的近車(chē)端與遠(yuǎn)車(chē)端；

步驟s1022：計(jì)算車(chē)道中心線的航向：

其中θlane代表車(chē)道中心線在車(chē)體坐標(biāo)系下的航向。

所述步驟s2還包括以下步驟：

步驟s201：初始化卡爾曼濾波器；

步驟s202：根據(jù)車(chē)輛當(dāng)前的轉(zhuǎn)彎半徑和速度預(yù)測(cè)本周期內(nèi)的大地坐標(biāo)系下車(chē)輛航向預(yù)測(cè)值：

其中，θveh-pred為大地坐標(biāo)系下當(dāng)前時(shí)刻車(chē)輛航向的預(yù)測(cè)值，θveh-correct-old為大地坐標(biāo)系下上一周期車(chē)輛航向的修正值，v為車(chē)輛當(dāng)前的速度，r為車(chē)輛當(dāng)前的轉(zhuǎn)彎半徑，t為規(guī)劃周期；

步驟s203：計(jì)算車(chē)體坐標(biāo)系下原始路徑航向與車(chē)道中心線航向的差值：

θerror＝θroad-θlane

其中，θerror為車(chē)體坐標(biāo)系下原始路徑航向與車(chē)道中心線航向的差值，θroad為車(chē)體坐標(biāo)系下原始路徑的航向，θlane為車(chē)道中心線航向；

步驟s204：計(jì)算大地坐標(biāo)系下車(chē)輛的航向測(cè)量值：

θveh-measure＝θveh-gps+θerror，

其中θveh-measure為大地坐標(biāo)系下車(chē)輛的航向測(cè)量值，θveh-gps為大地坐標(biāo)系下車(chē)輛的gps航向值；

步驟s205：將步驟s202中θveh-pred與步驟s204中θveh-measure作為卡爾曼濾波器的輸入得到車(chē)輛在大地坐標(biāo)系下的航向修正值θveh-correct；

步驟s206：將θveh-correct賦值給θveh-correct-old并進(jìn)入到下一周期。

所述步驟s3還包括以下步驟：

步驟s301：計(jì)算大地坐標(biāo)系下的車(chē)輛航向修正值與車(chē)輛gps航向值的偏差：

δθ＝θveh-correct-θveh-gps；

其中，θveh-correct為車(chē)輛在大地坐標(biāo)系下的航向修正值，θveh-gps為大地坐標(biāo)系下車(chē)輛的gps航向值；

步驟s302：利用步驟s301中結(jié)果計(jì)算得到校正后的路徑點(diǎn)：

其中，[xcorrectycorrectθcorrect]^t是校正后的路徑在車(chē)體坐標(biāo)系下的坐標(biāo)，[xveh-roadyveh-roadθveh-road]^t是校正前原始路徑在車(chē)體坐標(biāo)系下的坐標(biāo)。

所述步驟s4還包括以下步驟：

步驟s401：統(tǒng)計(jì)激光雷達(dá)柵格地圖中，每一列被占據(jù)的柵格的個(gè)數(shù)ogni；

步驟s402：根據(jù)每一列被占據(jù)的柵格的個(gè)數(shù)ogni，計(jì)算每一列柵格的統(tǒng)計(jì)標(biāo)志量ogsi：

其中i是柵格地圖中的列號(hào)，ogsi表示柵格地圖中第i列柵格的統(tǒng)計(jì)標(biāo)志量，ot表示柵格地圖的占據(jù)標(biāo)志閾值；

步驟s403：根據(jù)道路模型寬度rw以及道路模型寬度偏差rwt生成卷積核：

其中kj為第j列柵格對(duì)應(yīng)的卷積核，rw為道路模型寬度，rwt為道路模型寬度偏差，gs為被選中作為脈沖的柵格的列號(hào)，gs的取值范圍為[0,rwt]；

步驟s404：將步驟s402中結(jié)果ogsi與步驟s403中結(jié)果kj進(jìn)行卷積

其中，mw為柵格地圖中柵格的列數(shù)，卷積過(guò)程中如果re＝1，那么對(duì)應(yīng)柵格列的匹配值hit就加1；如果re＝-1，那么對(duì)應(yīng)柵格列的未匹配值miss就加1；如果re＝0，那么對(duì)應(yīng)柵格列的未知值uknow就加1；

步驟s405：根據(jù)步驟s404中結(jié)果得到某一列柵格為道路中心的概率gci：

其中g(shù)ci代表第i列柵格為道路中心的概率；

步驟s406：找出gci的最大值，其對(duì)應(yīng)的i就是道路中心所在的柵格的列號(hào)；

cr＝i

其中cr為道路中心所在的柵格列號(hào)；

步驟s407：計(jì)算左右路沿所在的柵格列號(hào)；

其中，lr和rr分別為左右路沿所在的柵格的列號(hào)；

步驟s408：計(jì)算車(chē)輛與道路的相對(duì)橫向位置關(guān)系

其中dl和dr分別為車(chē)輛到左右路沿的距離。

所述步驟s401中，從柵格地圖的正中間一列，即車(chē)輛所在的那一列，沿著與修正后的路徑相垂直的方向開(kāi)始向柵格地圖的兩邊開(kāi)始統(tǒng)計(jì)。

所述步驟s5還包括以下步驟：

步驟s501：計(jì)算車(chē)道總數(shù)cz；

步驟s502：定位車(chē)輛所在的車(chē)道c；

車(chē)輛所在的車(chē)道序號(hào)：

其中c表示車(chē)輛所在車(chē)道的左數(shù)序號(hào)，lw為車(chē)道寬度；

步驟s503：根據(jù)所在車(chē)道中心線位置(xc1,yc1)與(xc2,yc2)，計(jì)算車(chē)輛到所在車(chē)道中心線的距離dc；

步驟s504：根據(jù)車(chē)輛到所在車(chē)道中心線的距離dc和車(chē)道寬度lw分別計(jì)算車(chē)輛到左車(chē)道中心線的距離dcl和車(chē)輛到右車(chē)道中心線的距離dcr：

dcl＝lw-dc

dcr＝lw+dc

步驟s505：根據(jù)車(chē)輛距離各個(gè)車(chē)道中心線的距離生成虛擬約束，所述虛擬約束包括所在車(chē)道邊界約束和鄰車(chē)道邊界約束。

當(dāng)所在車(chē)道左右兩邊均有車(chē)道時(shí)，所述虛擬約束為左車(chē)道的左邊約束vll、所在車(chē)道的左邊約束vcl、所在車(chē)道的右邊約束vcr及右車(chē)道的右邊約束vrr；當(dāng)只有左側(cè)有車(chē)道時(shí)，所述虛偽約束為左車(chē)道的左邊約束vll、所在車(chē)道的左邊約束vcl及所在車(chē)道的右邊約束vcr；當(dāng)只有右側(cè)有車(chē)道時(shí)，所述虛擬約束為所在車(chē)道的左邊約束vcl、所在車(chē)道的右邊約束vcr及右車(chē)道的右邊約束vrr。

所述步驟s6還包括以下步驟：

步驟s601：判斷決策指令是否換道，若是，則轉(zhuǎn)入步驟s603；若否，則轉(zhuǎn)入步驟s602；

步驟s602：將校正后的路徑平移到所在車(chē)道的中心線處作為期望路徑進(jìn)行跟蹤，轉(zhuǎn)入步驟s604；

步驟s603：將校正后的路徑平移到目標(biāo)車(chē)道的中心線處作為期望路徑進(jìn)行跟蹤，轉(zhuǎn)入步驟s604；

步驟s604：根據(jù)期望路徑進(jìn)行路徑跟蹤。

本發(fā)明有益效果如下：

本發(fā)明提出的基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法，融合gps、視覺(jué)與激光雷達(dá)信息完成車(chē)輛與道路相對(duì)橫向位置的精確定位，而不需要高精度定位設(shè)備和高精度地圖。在車(chē)道保持行駛時(shí)，能夠有效保證車(chē)輛沿著所在車(chē)道的中心線行駛，保證了車(chē)輛自動(dòng)行駛時(shí)的橫向穩(wěn)定性；在換道時(shí)，能夠保證車(chē)輛準(zhǔn)確的換到目標(biāo)車(chē)道，避免自動(dòng)駕駛過(guò)程中車(chē)輛騎線或壓線行駛。本發(fā)明有助于提高自動(dòng)駕駛車(chē)輛在結(jié)構(gòu)化的、有清晰車(chē)道線的城市或高速公路環(huán)境下的車(chē)道保持與換道能力，對(duì)于自動(dòng)駕駛車(chē)輛的安全、穩(wěn)定、平順行駛具有重要意義。

本發(fā)明的其他特征和優(yōu)點(diǎn)將在隨后的說(shuō)明書(shū)中闡述，并且，部分的從說(shuō)明書(shū)中變得顯而易見(jiàn)，或者通過(guò)實(shí)施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點(diǎn)可通過(guò)在所寫(xiě)的說(shuō)明書(shū)、權(quán)利要求書(shū)、以及附圖中所特別指出的結(jié)構(gòu)來(lái)實(shí)現(xiàn)和獲得。

附圖說(shuō)明

附圖僅用于示出具體實(shí)施例的目的，而并不認(rèn)為是對(duì)本發(fā)明的限制，在整個(gè)附圖中，相同的參考符號(hào)表示相同的部件。

圖1為本發(fā)明中基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法整體流程；

圖2為本發(fā)明利用卡爾曼濾波原理對(duì)車(chē)輛的航向進(jìn)行修正的流程圖；

圖3為本發(fā)明中基于激光雷達(dá)柵格地圖獲取車(chē)輛與道路相對(duì)橫向位置關(guān)系的流程圖；

圖4為本發(fā)明中在激光雷達(dá)柵格地圖上統(tǒng)計(jì)每一列被占據(jù)的柵格數(shù)目的示意圖；

圖5為本發(fā)明中在獲取車(chē)輛與道路相對(duì)橫向位置關(guān)系時(shí)卷積核的生成方式示意圖；

圖6為本發(fā)明中虛擬約束生成流程圖；

圖7為本發(fā)明中虛擬約束生成示意圖；

圖8為本發(fā)明中期望路徑生成示意圖。

附圖標(biāo)記：

401-車(chē)體坐標(biāo)系、402-車(chē)輛、403-柵格地圖正中間的一列柵格、404-校正后的路徑、405-柵格地圖的左右邊界、406-統(tǒng)計(jì)方向；

501-道路模型寬度偏差rwt、502-被選中作為脈沖的柵格、503-道路模型寬度的邊界、504-道路邊界、507-生成的卷積核kj；

702-虛擬約束、703-車(chē)道線；

805-車(chē)道中心線、807-左換道時(shí)的期望路徑、808-車(chē)道保持時(shí)的期望路徑、809-右換道時(shí)的期望路徑。

具體實(shí)施方式

下面結(jié)合附圖來(lái)具體描述本發(fā)明的優(yōu)選實(shí)施例，其中，附圖構(gòu)成本申請(qǐng)一部分，并與本發(fā)明的實(shí)施例一起用于闡釋本發(fā)明的原理。

根據(jù)本發(fā)明的一個(gè)優(yōu)選實(shí)施例，提供了一種基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法，如圖1所示，包括以下步驟：

步驟s1：根據(jù)車(chē)道線檢測(cè)結(jié)果獲取車(chē)體坐標(biāo)系下的車(chē)道中心線航向；

具體地，所述步驟s1還包括以下子步驟：

步驟s101：判斷車(chē)道線檢測(cè)結(jié)果是否有效，若有效，執(zhí)行步驟s102；若無(wú)效，沿用上一周期車(chē)道線檢測(cè)結(jié)果；

在本實(shí)施例中，自動(dòng)駕駛車(chē)輛在結(jié)構(gòu)化的有清晰車(chē)道線和道路邊沿的環(huán)境下進(jìn)行車(chē)道保持和換道，自動(dòng)駕駛車(chē)輛的視覺(jué)傳感器可以檢測(cè)出車(chē)道線信息，但是考慮到十字路口處沒(méi)有車(chē)道線的情況，所以要對(duì)車(chē)道線的檢測(cè)結(jié)果是否有效進(jìn)行判斷。

步驟s102：獲取車(chē)體坐標(biāo)系下的車(chē)道中心線航向；

車(chē)道線檢測(cè)結(jié)果(x1,y1)、(x2,y2)、(x3,y3)與(x4,y4)為左右車(chē)道線在車(chē)體坐標(biāo)系下的坐標(biāo)點(diǎn)，其中，(x1,y1)為車(chē)輛左前方近車(chē)點(diǎn)，(x2,y2)為左前方遠(yuǎn)車(chē)點(diǎn)，(x3,y3)為右前方近車(chē)點(diǎn)，(x4,y4)為右前方遠(yuǎn)車(chē)點(diǎn)，根據(jù)這四個(gè)點(diǎn)，求出車(chē)體坐標(biāo)系下車(chē)道中心線的位置以及航向：

步驟s1021：計(jì)算車(chē)道中心線的位置

其中(xc1,yc1)與(xc2,yc2)分別代表車(chē)道中心線的近車(chē)端與遠(yuǎn)車(chē)端；

步驟s1022：計(jì)算車(chē)道中心線的航向

其中θlane代表車(chē)道中心線在車(chē)體坐標(biāo)系下的航向。

步驟s2：利用卡爾曼濾波器和步驟s1中獲取的車(chē)道中心線航向?qū)?chē)輛航向進(jìn)行修正；

由于從車(chē)載gps得到的車(chē)輛的航向具有時(shí)間上的滯后和空間上的偏差，所以直接將其用于把大地坐標(biāo)系下的全局路徑轉(zhuǎn)換為車(chē)體坐標(biāo)系下的局部路徑時(shí)會(huì)導(dǎo)致較大的誤差，從而使得車(chē)輛不能準(zhǔn)確的跟蹤實(shí)際路徑；

車(chē)道中心線的航向具有相對(duì)穩(wěn)定性，因此可以利用車(chē)道中心線的航向?qū)?chē)輛的航向進(jìn)行修正，考慮到測(cè)量噪聲的影響，修正后的車(chē)輛航向需要利用卡爾曼濾波器進(jìn)行維護(hù)；

具體地，如圖2所示，所述步驟s2包括如下步驟：

步驟s201：初始化卡爾曼濾波器；

步驟s202：根據(jù)車(chē)輛當(dāng)前的轉(zhuǎn)彎半徑和速度預(yù)測(cè)本周期內(nèi)的大地坐標(biāo)系下車(chē)輛航向預(yù)測(cè)值θveh-pred；

其中θveh-pred為大地坐標(biāo)系下當(dāng)前時(shí)刻車(chē)輛航向的預(yù)測(cè)值，θveh-correct-old為大地坐標(biāo)系下上一周期車(chē)輛航向的修正值，v為車(chē)輛當(dāng)前的速度，r為車(chē)輛當(dāng)前的轉(zhuǎn)彎半徑，t為規(guī)劃周期。

步驟s203：計(jì)算車(chē)體坐標(biāo)系下原始路徑航向與車(chē)道中心線航向的差值：

路徑航向和車(chē)道中心線航向都是車(chē)體坐標(biāo)系下的航向，且車(chē)道中心線航向平行于路徑的切線方向。由于車(chē)道中心線航向是根據(jù)車(chē)道線檢測(cè)結(jié)果直接在車(chē)體坐標(biāo)系下計(jì)算得到，而路徑航向是以車(chē)輛的gps坐標(biāo)為基點(diǎn)經(jīng)坐標(biāo)轉(zhuǎn)換得到，因此，只要求得車(chē)體坐標(biāo)系下的路徑航向與車(chē)道中心線航向就相當(dāng)于求出了車(chē)輛的gps航向與其真實(shí)航向的差值θerror：

θerror＝θroad-θlane

其中，θerror為車(chē)體坐標(biāo)系下原始路徑航向與車(chē)道中心線航向的差值，θroad為車(chē)體坐標(biāo)系下原始路徑的航向，θlane為車(chē)道中心線航向；

步驟s204：計(jì)算大地坐標(biāo)系下車(chē)輛的航向測(cè)量值：

θveh-measure＝θveh-gps+θerror

其中θveh-measure為大地坐標(biāo)系下車(chē)輛的航向測(cè)量值，θveh-gps為大地坐標(biāo)系下車(chē)輛的gps航向值；

步驟s205：將步驟s202中θveh-pred與步驟s204中θveh-measure作為卡爾曼濾波器的輸入得到車(chē)輛在大地坐標(biāo)系下的航向修正值θveh-correct；

步驟s206：將θveh-correct賦值給θveh-correct-old并進(jìn)入到下一周期。

步驟s3：利用步驟s2修正后的車(chē)輛航向?qū)υ悸窂街械穆窂近c(diǎn)進(jìn)行校正；

所述原始路徑為車(chē)體坐標(biāo)下的路徑，原始路徑的起點(diǎn)為車(chē)體坐標(biāo)系原點(diǎn)；

由于車(chē)輛航向的不準(zhǔn)確，車(chē)體坐標(biāo)系下的原始路徑也是存在誤差的，因此需要根據(jù)修正后的車(chē)輛航向，對(duì)原始路徑進(jìn)行校正，校正后路徑的航向與車(chē)輛所在車(chē)道的航向一致。

具體地，包括以下步驟：

步驟s301：計(jì)算大地坐標(biāo)系下的車(chē)輛航向修正值與車(chē)輛gps航向值的偏差：

δθ＝θveh-correct-θveh-gps

步驟s302：利用步驟s301中結(jié)果計(jì)算得到校正后的路徑點(diǎn)：

其中[xcorrectycorrectθcorrec]t^t是校正后的路徑在車(chē)體坐標(biāo)系下的坐標(biāo)，[xveh-roadyveh-roadθveh-road]^t是校正前原始路徑在車(chē)體坐標(biāo)系下的坐標(biāo)。

步驟s4：基于三維激光雷達(dá)柵格地圖，獲取自動(dòng)駕駛車(chē)輛與道路的相對(duì)橫向位置關(guān)系；

所述三維激光雷達(dá)柵格地圖為車(chē)體坐標(biāo)系下的柵格地圖，每個(gè)柵格對(duì)應(yīng)一個(gè)表示該柵格是否被障礙物占據(jù)的值；

由于道路gps點(diǎn)坐標(biāo)誤差的存在，僅僅依靠車(chē)輛的gps定位、路點(diǎn)的gps定位以及道路模型的寬度，是無(wú)法確定車(chē)輛在道路上的橫向定位的，因此也無(wú)法確定車(chē)輛在哪個(gè)車(chē)道上。本發(fā)明中提出了基于激光雷達(dá)柵格地圖，利用統(tǒng)計(jì)學(xué)方法求得道路中心在柵格地圖中的位置，然后在利用車(chē)輛與柵格地圖的相對(duì)關(guān)系以及道路模型寬度，可以得到車(chē)輛相對(duì)于道路中心、道路左邊沿、道路右邊沿的位置。

具體地，如圖3所示，基于三維激光雷達(dá)柵格地圖，獲取自動(dòng)駕駛車(chē)輛與道路的相對(duì)橫向位置關(guān)系的過(guò)程包括以下步驟：

步驟s401：統(tǒng)計(jì)激光雷達(dá)柵格地圖中，每一列被占據(jù)的柵格的個(gè)數(shù)ogni；

優(yōu)選地，如圖4所示，從柵格地圖的正中間一列(亦即車(chē)輛所在的那一列)沿著與修正后的路徑相垂直的方向開(kāi)始向柵格地圖的兩邊開(kāi)始統(tǒng)計(jì)。

步驟s402：根據(jù)每一列被占據(jù)的柵格的個(gè)數(shù)ogni，計(jì)算每一列柵格的統(tǒng)計(jì)標(biāo)志量ogsi：

其中i是柵格地圖中的列號(hào)，ogsi表示柵格地圖中第i列柵格的統(tǒng)計(jì)標(biāo)志量，ot表示柵格地圖的占據(jù)標(biāo)志閾值；

賦值統(tǒng)計(jì)結(jié)果標(biāo)志量的原則為：如果第i列被占據(jù)的柵格個(gè)數(shù)大于ot，那么這一列柵格的統(tǒng)計(jì)結(jié)果標(biāo)志量賦值為1；如果第i列被占據(jù)的柵格個(gè)數(shù)不大于ot，那么再考察第i-1列被占據(jù)的柵格的個(gè)數(shù)是否大于ot，如果是，那么第i列柵格的統(tǒng)計(jì)結(jié)果標(biāo)志量賦值為0，如果否，那么第i列柵格的統(tǒng)計(jì)結(jié)果標(biāo)志量賦值為-1。

步驟s403：根據(jù)道路模型寬度rw以及道路模型寬度偏差rwt生成卷積核：

其中kj為第j列柵格對(duì)應(yīng)的卷積核，rw為道路模型寬度，rwt為道路模型寬度偏差，gs為被選中作為脈沖的柵格的列號(hào)，gs的取值范圍為[0,rwt]；

如圖5所示，生成卷積核的規(guī)則如下所述：選定在[0,rwt]范圍內(nèi)的某一列柵格(假設(shè)列號(hào)為gs)作為脈沖柵格，如果列號(hào)j小于gs，那么對(duì)應(yīng)的kj賦值為0；如果列號(hào)j等于gs，那么對(duì)應(yīng)的kj賦值為1；如果列號(hào)j大于gs且小于rw-gs，那么對(duì)應(yīng)的kj賦值為-1；如果列號(hào)等于rw-gs，那么對(duì)應(yīng)的kj賦值為1；如果列號(hào)j大于rw-gs且小于rw，那么對(duì)應(yīng)的kj賦值為0。其中，gs要從0依次取到rwt。

步驟s404：將步驟s402中結(jié)果ogsi與步驟s403中結(jié)果kj進(jìn)行卷積

其中，mw為柵格地圖中柵格的列數(shù)，卷積過(guò)程中如果re＝1，那么對(duì)應(yīng)柵格列的匹配值hit就加1；如果re＝-1，那么對(duì)應(yīng)柵格列的未匹配值miss就加1；如果re＝0，那么對(duì)應(yīng)柵格列的未知值uknow就加1。

步驟s405：根據(jù)步驟s404中結(jié)果得到某一列柵格為道路中心的概率gci：

其中g(shù)ci代表第i列柵格為道路中心的概率；

由于ogsi和kj的取值范圍都是{-1,0,1}，且j的卷積取值范圍為[-rw/2,gs]，即kj在卷積過(guò)程中不取-1，卷積過(guò)程中如果卷積結(jié)果re＝1，那么對(duì)應(yīng)柵格列的匹配值hit就加1；如果re＝-1，那么對(duì)應(yīng)柵格列的未匹配值miss就加1；如果re＝0，那么對(duì)應(yīng)柵格列的未知值uknow就加1。最后如果hit＝2，那么對(duì)應(yīng)的第i列柵格為道路中心的概率賦值為0.9；如果hit＝1,miss＝1或者h(yuǎn)it＝1,uknow＝1，那么對(duì)應(yīng)的第i列柵格為道路中心的概率賦值為0.5；如果miss＝2或uknow＝2或miss＝1,uknow＝1，那么對(duì)應(yīng)的第i列柵格為道路中心的概率賦值為0.3。

步驟s406：找出gci的最大值，其對(duì)應(yīng)的i就是道路中心所在的柵格的列號(hào)；

cr＝i

其中cr為道路中心所在的柵格列號(hào)。

步驟s407：計(jì)算左右路沿所在的柵格列號(hào)；

其中，lr和rr分別為左右路沿所在的柵格的列號(hào)。

步驟s408：計(jì)算車(chē)輛與道路的相對(duì)橫向位置關(guān)系

其中dl和dr分別為車(chē)輛到左右路沿的距離。

步驟s5：根據(jù)步驟s4獲取的自動(dòng)駕駛車(chē)輛與道路的相對(duì)橫向位置關(guān)系生成虛擬約束，所述虛擬約束包括所在車(chē)道的邊界約束和鄰車(chē)道邊界約束；

由于本發(fā)明中將車(chē)輛的行為分為了車(chē)道保持與換道兩種，在車(chē)道保持過(guò)程中，僅需考慮車(chē)輛所在車(chē)道的邊界約束、避撞等問(wèn)題；在換道過(guò)程中，只考慮所在車(chē)道與目標(biāo)車(chē)道內(nèi)的障礙物情況即可。因此，根據(jù)車(chē)輛與道路的相對(duì)定位關(guān)系、車(chē)道寬度以及道路寬度，虛擬出車(chē)輛當(dāng)前所在車(chē)道的邊界約束以及鄰車(chē)道邊界約束，從而四條虛擬約束可以確定三條車(chē)道，因此能夠在車(chē)道保持時(shí)保證車(chē)輛橫向運(yùn)動(dòng)的穩(wěn)定性，在換道時(shí)保證準(zhǔn)確的換到目標(biāo)車(chē)道的中心線位置，避免騎線或壓線行駛；

具體地，如圖6所示，虛擬出所在車(chē)道的邊界約束和鄰車(chē)道邊界約束的過(guò)程包括以下步驟：

步驟s501：計(jì)算車(chē)道總數(shù)cz；

步驟s502：定位車(chē)輛所在的車(chē)道c；

車(chē)輛所在的車(chē)道序號(hào)：

其中c表示車(chē)輛所在車(chē)道的左數(shù)序號(hào)，lw為車(chē)道寬度。

步驟s503：根據(jù)所在車(chē)道中心線位置(xc1,yc1)與(xc2,yc2)，計(jì)算車(chē)輛到所在車(chē)道中心線的距離dc；

步驟s504：根據(jù)車(chē)輛到所在車(chē)道中心線的距離dc和車(chē)道寬度lw分別計(jì)算車(chē)輛到左車(chē)道中心線的距離dcl和車(chē)輛到右車(chē)道中心線的距離dcr：

dcl＝lw-dc

dcr＝lw+dc

步驟s505：根據(jù)車(chē)輛距離各個(gè)車(chē)道中心線的距離生成虛擬約束，所述虛擬約束包括所在車(chē)道邊界約束和鄰車(chē)道邊界約束；

具體地，當(dāng)所在車(chē)道左右兩邊均有車(chē)道時(shí)，如圖7(a)所示，所述虛擬約束為左車(chē)道的左邊約束vll、所在車(chē)道的左邊約束vcl、所在車(chē)道的右邊約束vcr及右車(chē)道的右邊約束vrr；當(dāng)只有左側(cè)有車(chē)道時(shí)，如圖7(b)所示，所述虛偽約束為左車(chē)道的左邊約束vll、所在車(chē)道的左邊約束vcl及所在車(chē)道的右邊約束vcr；當(dāng)只有右側(cè)有車(chē)道時(shí)，如圖7(b)所示，所述虛擬約束為所在車(chē)道的左邊約束vcl、所在車(chē)道的右邊約束vcr及右車(chē)道的右邊約束vrr；

靠近道路邊界的虛擬約束與道路邊界之間有一定的安全距離。

步驟s6：根據(jù)步驟s5生成的虛擬約束和自動(dòng)駕駛系統(tǒng)的決策指令，生成期望路徑。

校正后的路徑可以保證航向與車(chē)道線方向一致，但并不能保證該路徑位于當(dāng)前所在車(chē)道的中心位置，因此經(jīng)校正后的路徑首先需要平移到當(dāng)前所在車(chē)道的中心線位置，得到在進(jìn)行車(chē)道保持時(shí)的期望路徑，而在換道時(shí)，則將期望路徑平移到目標(biāo)車(chē)道的中心線位置；

圖8為期望路徑生成示意圖；

具體地，包括以下步驟：

步驟s601：判斷決策指令是否換道，若是，則轉(zhuǎn)入步驟s603；若否，則轉(zhuǎn)入步驟s602；

步驟s602：將校正后的路徑平移到所在車(chē)道的中心線處作為期望路徑進(jìn)行跟蹤，轉(zhuǎn)入步驟s604；

步驟s603：將校正后的路徑平移到目標(biāo)車(chē)道的中心線處作為期望路徑進(jìn)行跟蹤，轉(zhuǎn)入步驟s604；

步驟s604：根據(jù)期望路徑進(jìn)行路徑跟蹤。

綜上所述，本發(fā)明實(shí)施例提供了一種基于虛擬約束的自動(dòng)駕駛車(chē)輛路徑規(guī)劃方法，融合gps、視覺(jué)與激光雷達(dá)信息完成車(chē)輛與道路相對(duì)橫向位置的精確定位，而不需要高精度定位設(shè)備和高精度地圖。在車(chē)道保持行駛時(shí)，能夠有效保證車(chē)輛沿著所在車(chē)道的中心線行駛，保證了車(chē)輛自動(dòng)行駛時(shí)的橫向穩(wěn)定性；在換道時(shí)，能夠保證車(chē)輛準(zhǔn)確的換到目標(biāo)車(chē)道，避免自動(dòng)駕駛過(guò)程中車(chē)輛騎線或壓線行駛。本發(fā)明有助于提高自動(dòng)駕駛車(chē)輛在結(jié)構(gòu)化的、有清晰車(chē)道線的城市或高速公路環(huán)境下的車(chē)道保持與換道能力，對(duì)于自動(dòng)駕駛車(chē)輛的安全、穩(wěn)定、平順行駛具有重要意義。

本領(lǐng)域技術(shù)人員可以理解，實(shí)現(xiàn)上述實(shí)施例方法的全部或部分流程，可以通過(guò)計(jì)算機(jī)程序來(lái)指令相關(guān)的硬件來(lái)完成，所述的程序可存儲(chǔ)于計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中。其中，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)為磁盤(pán)、光盤(pán)、只讀存儲(chǔ)記憶體或隨機(jī)存儲(chǔ)記憶體等。

以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。