本發(fā)明屬于無人駕駛汽車導(dǎo)航領(lǐng)域，尤其涉及一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法。

背景技術(shù)：

無人車導(dǎo)航模塊綜合了感知的結(jié)果，并考慮車輛動力學(xué)特性，輸出一條可以行駛的軌跡。在城市結(jié)構(gòu)化道路的完全自動駕駛汽車導(dǎo)航中，導(dǎo)航任務(wù)一般分為三層：任務(wù)規(guī)劃層、行為決策層和軌跡規(guī)劃層。軌跡規(guī)劃層一般分兩步進(jìn)行：不考慮移動障礙物的參考軌跡生成和考慮移動障礙物的局部軌跡規(guī)劃。對于軌跡規(guī)劃問題，在路口這樣復(fù)雜的場景中，參考軌跡尤其重要，它彌補(bǔ)了上層任務(wù)規(guī)劃的不完整性，同時(shí)利用地理信息降低感知難度，而且增加了最終行車軌跡的全局最優(yōu)性。

目前，路口轉(zhuǎn)向參考軌跡的生成方法尚不完善，一般用直線連接、圓弧連接或在地圖上手動畫出虛擬車道線的方式生成，對汽車動力學(xué)特性、人類駕駛員駕駛習(xí)慣以及乘坐舒適性等因素的考慮尚不深入。另外，手動畫線方式本身效率不高，不適于無人車大范圍的行駛導(dǎo)航。因此，現(xiàn)在亟需一種適用于解決多種路口內(nèi)部多種轉(zhuǎn)向情形下的參考軌跡自動規(guī)劃方法，同時(shí)考慮汽車動力學(xué)特性、人類駕駛員駕駛習(xí)慣和乘坐舒適性等因素，提高軌跡規(guī)劃的合理性。

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

針對上述問題，本發(fā)明的目的是提供一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，適用于多種路口內(nèi)部、多種轉(zhuǎn)向情形下的參考軌跡自動規(guī)劃，同時(shí)考慮汽車動力學(xué)特性、人類駕駛員駕駛習(xí)慣和乘坐舒適性等因素，提高軌跡規(guī)劃的合理性，為進(jìn)一步的無人車控制提供目標(biāo)軌跡。

為實(shí)現(xiàn)上述目的，本發(fā)明采取以下技術(shù)方案：一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，包括以下步驟：

1)建立車輛轉(zhuǎn)向模型，根據(jù)車輛基本參數(shù)，生成車輛轉(zhuǎn)向時(shí)的動力學(xué)約束；

2)根據(jù)車輛上層決策獲取道路的地圖信息，根據(jù)地圖信息得到參考軌跡的起終點(diǎn)約束信息；

3)根據(jù)車輛轉(zhuǎn)向時(shí)的動力學(xué)約束和參考軌跡的起終點(diǎn)約束信息，生成若干條基本滿足車輛轉(zhuǎn)向的幾何學(xué)和動力學(xué)約束且符合人的駕駛習(xí)慣和轉(zhuǎn)向規(guī)律的轉(zhuǎn)彎軌跡曲線，形成基本可行軌跡簇；

4)以橫向加速度對時(shí)間的積分為指標(biāo)，從基本可行軌跡簇中選擇最為舒適的一條作為最終生成的最佳路口內(nèi)參考行駛軌跡。

所述步驟1)中的車輛轉(zhuǎn)向模型為：

式中，π為轉(zhuǎn)彎軌跡曲線；s為轉(zhuǎn)彎軌跡曲線的任意弧長；l為轉(zhuǎn)彎軌跡曲線π的長度。

所述步驟1)中的車輛基本參數(shù)包括車輛軸距l(xiāng)、外側(cè)轉(zhuǎn)向輪最大轉(zhuǎn)角β和轉(zhuǎn)彎軌跡曲線曲率變化率最大值σmax；

車輛轉(zhuǎn)向時(shí)的動力學(xué)約束為：

κ(s)≤κmax，s∈[0,l]

式中，κ(s)為轉(zhuǎn)彎軌跡曲線任意弧長s處的曲率；κmax為轉(zhuǎn)彎軌跡曲線曲率的最大值；為轉(zhuǎn)彎軌跡曲線曲率的導(dǎo)數(shù)。

所述步驟2)中的車輛上層決策是指給出從“入車道”進(jìn)入路口，后經(jīng)“出車道”駛出路口的決策；道路的地圖信息包括入車道結(jié)束中心點(diǎn)pi的位置和方向信息pi＝(xi，yi，θi)，以及出車道起始中心點(diǎn)pe的位置和方向信息pe＝(xe，ye，θe)；其中，xi、yi為入車道結(jié)束中心點(diǎn)的經(jīng)緯度坐標(biāo)，θi為入車道結(jié)束中心點(diǎn)處的理想行車方向；xe、ye為出車道起始中心點(diǎn)的經(jīng)緯度坐標(biāo)，θe為出車道起始中心點(diǎn)處的理想行車方向；得到的參考軌跡的起終點(diǎn)約束信息為：π(0)＝pi且π(l)＝pe。

所述步驟3)中基本可行軌跡簇的生成，具體包括以下步驟：

①根據(jù)車輛轉(zhuǎn)向時(shí)的動力學(xué)約束和參考軌跡的起終點(diǎn)約束信息，確定轉(zhuǎn)向軌跡曲線參數(shù)l、l1、l2、κmax的求解空間為：

l1＝λ1l,l2＝λ2l且0≤λ1≤λ2≤1

0≤κmax≤σmax·max(l1,l-l2)

式中，l1和l2分別為轉(zhuǎn)彎軌跡曲線中曲率增加段和曲率減小段的長度；λ1和λ2分別為轉(zhuǎn)彎軌跡曲線中曲率增加段和曲率減小段占轉(zhuǎn)彎軌跡曲線總長度的比例；

②建立轉(zhuǎn)向軌跡曲線終點(diǎn)處的車輛車身方向角以及橫坐標(biāo)和縱坐標(biāo)的計(jì)算公式；

③根據(jù)轉(zhuǎn)向軌跡曲線終點(diǎn)處的車輛車身方向角以及橫坐標(biāo)和縱坐標(biāo)的計(jì)算公式，計(jì)算求解空間中的點(diǎn)(li,l1i，l2i，κmaxi)的終點(diǎn)處的車輛車身方向角及橫縱坐標(biāo)θei，xei，yei；如果θei，xei，yei在出車道中心點(diǎn)信息θe，xe，ye的可行領(lǐng)域內(nèi)，則將該曲線留在曲線簇中，最終得到一定數(shù)量曲線的基本可行軌跡簇。

所述步驟②中的轉(zhuǎn)向軌跡曲線終點(diǎn)處的車輛車身方向角計(jì)算公式為：

轉(zhuǎn)向軌跡曲線終點(diǎn)的橫坐標(biāo)和縱坐標(biāo)的計(jì)算公式分別為：

式中，θ(s)為轉(zhuǎn)彎軌跡曲線任意弧長s處的方向角。

所述步驟③中的可行領(lǐng)域是指轉(zhuǎn)向軌跡曲線的終點(diǎn)位置加上半個(gè)車身的寬度不能超出車道實(shí)線，且終點(diǎn)處的車身方向角不能超過限定值。

所述步驟4)中橫向加速度對時(shí)間的積分為：

式中，k為基本可行軌跡簇中轉(zhuǎn)向軌跡曲線的曲率；v為車速；s為轉(zhuǎn)彎軌跡曲線的任意弧長；l為轉(zhuǎn)彎軌跡曲線的長度；最為舒適即橫向加速度對時(shí)間的積分a值最小。

本發(fā)明由于采取以上技術(shù)方案，其具有以下優(yōu)點(diǎn)：1、本發(fā)明的一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，生成的參考軌跡與上下游路段連接處曲率連續(xù)，從而保證軌跡追蹤的可行性，降低控制的難度。2、本發(fā)明的一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，考慮不同車輛的轉(zhuǎn)向特性，保證軌跡適用于不同車型車輛的追蹤控制。3、本發(fā)明的一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，考慮車輛動力學(xué)特性，尤其考慮乘坐感受，提高了軌跡追蹤時(shí)的舒適性。4、本發(fā)明的一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，考慮人的駕駛特性，模擬人的駕駛規(guī)律，生成的軌跡符合人的駕駛習(xí)慣。

附圖說明

圖1是本發(fā)明方法的流程示意圖；

圖2是四輪汽車轉(zhuǎn)向時(shí)車輛姿態(tài)及轉(zhuǎn)彎軌跡示意圖；

圖3是交叉路口地圖道路信息示意圖；

圖4是入車道結(jié)束中心點(diǎn)信息示意圖；

圖5是理想的行駛軌跡示意圖；

圖6是轉(zhuǎn)向曲率模型示意圖；

圖7是轉(zhuǎn)向過程示意圖；

圖8是基本可行軌跡簇示意圖。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對本發(fā)明進(jìn)行詳細(xì)的描述。

如圖1所示，本發(fā)明提供的一種面向無人駕駛的擬人化參考軌跡規(guī)劃方法，適用于無人車在路口場景下的路口內(nèi)部參考行駛軌跡的規(guī)劃，具體包括以下步驟：

1)車輛轉(zhuǎn)向建模：建立車輛轉(zhuǎn)向模型，根據(jù)車輛基本參數(shù)，生成車輛轉(zhuǎn)向時(shí)的動力學(xué)約束。

如圖2所示，轉(zhuǎn)彎軌跡曲線π為車輛后軸中點(diǎn)p掃過的軌跡，在點(diǎn)p處，轉(zhuǎn)彎軌跡曲線π的切線與車身方向平行，瞬時(shí)半徑ρ即為此刻的車輛轉(zhuǎn)彎半徑。采用曲率-弧長參數(shù)來表示轉(zhuǎn)彎軌跡曲線π，路口內(nèi)參考行駛軌跡的規(guī)劃問題可歸結(jié)為計(jì)算轉(zhuǎn)彎軌跡曲線其中，s為轉(zhuǎn)彎軌跡曲線的任意弧長，l為轉(zhuǎn)彎軌跡曲線π的長度。

轉(zhuǎn)彎軌跡曲線π應(yīng)滿足車輛轉(zhuǎn)向時(shí)的動力學(xué)約束。其中，車輛轉(zhuǎn)彎過程中，由于轉(zhuǎn)向機(jī)構(gòu)的限制，車輛轉(zhuǎn)彎半徑不能小于最小轉(zhuǎn)彎半徑rmin，即轉(zhuǎn)彎軌跡曲線的曲率不應(yīng)超過最大值κmax，因此有κ(s)≤κmax，s∈[0,l]，其中，κ(s)為轉(zhuǎn)彎軌跡曲線任意弧長s處的曲率。

設(shè)車輛后軸中點(diǎn)p的坐標(biāo)為(x，y)，車身方向與x軸夾角為θ；獲取車輛軸距l(xiāng)和外側(cè)轉(zhuǎn)向輪最大轉(zhuǎn)角β，可以生成轉(zhuǎn)向約束中的最大曲率為：

考慮到實(shí)際轉(zhuǎn)向過程中轉(zhuǎn)向角不能發(fā)生突變，這要求轉(zhuǎn)彎軌跡曲線曲率的導(dǎo)數(shù)存在且連續(xù)。事實(shí)上，轉(zhuǎn)向輪轉(zhuǎn)動的速度是受限的，轉(zhuǎn)彎軌跡曲線曲率變化率存在最大值σmax(由經(jīng)驗(yàn)得到)，因此有s∈[0,l]，其中，為轉(zhuǎn)彎軌跡曲線曲率的導(dǎo)數(shù)。

2)地圖信息獲?。焊鶕?jù)車輛上層決策獲取道路的地圖信息，根據(jù)地圖信息得到參考軌跡的起終點(diǎn)約束信息。

如圖3所示，當(dāng)車輛上層決策給出從“入車道”進(jìn)入路口，后經(jīng)“出車道”駛出路口時(shí)，從地圖中獲取的道路信息應(yīng)包括入車道結(jié)束中心點(diǎn)pi的位置和方向信息pi＝(xi，yi，θi)，以及出車道起始中心點(diǎn)pe的位置和方向信息pe＝(xe，ye，θe)；其中，xi、yi為入車道結(jié)束中心點(diǎn)的經(jīng)緯度坐標(biāo)，θi為入車道結(jié)束中心點(diǎn)處的理想行車方向；xe、ye為出車道起始中心點(diǎn)的經(jīng)緯度坐標(biāo)，θe為出車道起始中心點(diǎn)處的理想行車方向。如圖4所示，入車道結(jié)束中心點(diǎn)處的理想行車方向θi定義為垂直于車道結(jié)束線指向交叉口內(nèi)部的向量與正北方向所呈逆時(shí)針夾角，出車道起始中心點(diǎn)處的理想行車方向θe定義與入車道結(jié)束中心點(diǎn)處的理想行車方向θi定義類似。

如圖5所示，理想的行駛軌跡假定汽車沿入車道結(jié)束中心點(diǎn)pi沿方向θi進(jìn)入交叉口，經(jīng)歷轉(zhuǎn)向過程后，沿出車道起始中心點(diǎn)pe沿方向θe駛出交叉口，因此有起終點(diǎn)限制條件為：π(0)＝pi且π(l)＝pe。

3)基本可行軌跡簇生成：根據(jù)車輛轉(zhuǎn)向時(shí)的動力學(xué)約束和起終點(diǎn)限制條件，生成若干條基本滿足車輛轉(zhuǎn)向的幾何學(xué)和動力學(xué)約束且符合人的駕駛習(xí)慣和轉(zhuǎn)向規(guī)律的轉(zhuǎn)彎軌跡曲線，形成基本可行軌跡簇。

首先，以入車道結(jié)束中心點(diǎn)為坐標(biāo)原點(diǎn)，入車道中心點(diǎn)的入方向?yàn)閥軸，將出車道起始中心點(diǎn)的經(jīng)緯度坐標(biāo)轉(zhuǎn)化為笛卡爾坐標(biāo)，相關(guān)方法已很成熟，這里不贅述。

然后，生成考慮人的駕駛習(xí)慣和轉(zhuǎn)向規(guī)律的連續(xù)曲率轉(zhuǎn)彎軌跡曲線。人的駕駛習(xí)慣可以描述為：在車輛進(jìn)入交叉口時(shí)，開始轉(zhuǎn)動方向盤；方向盤到達(dá)一定角度后，保持方向盤轉(zhuǎn)角一段時(shí)間，如交叉口較小，該段時(shí)間會縮短或不存在；在車輛即將駛出交叉口時(shí)，回正方向盤至方向盤轉(zhuǎn)角為0。如圖6所示，為了模擬這一轉(zhuǎn)向過程，同時(shí)不增加轉(zhuǎn)彎軌跡曲線的復(fù)雜程度，本發(fā)明的曲率模型將轉(zhuǎn)向過程分為三段：第一段中，轉(zhuǎn)彎軌跡曲線曲率從0開始隨弧長增加均勻增加，對應(yīng)車輛進(jìn)入交叉口的逐漸轉(zhuǎn)動方向盤過程；第二段中曲率到達(dá)最大值，并保持一段弧長，對應(yīng)車輛經(jīng)過交叉口的保持方向盤轉(zhuǎn)角過程；第三段中曲率均勻減小到0，對應(yīng)轉(zhuǎn)向過程最后階段方向盤逐漸回正的過程。各段對應(yīng)的轉(zhuǎn)向軌跡如圖7所示。

給定轉(zhuǎn)彎過程中的參數(shù)，即唯一確定轉(zhuǎn)彎軌跡曲線。適當(dāng)放開入車道結(jié)束中心點(diǎn)的約束，即終點(diǎn)限制條件π(l)＝pe轉(zhuǎn)化為π(l)在pe的一定矩形范圍中，這樣即會得到一個(gè)參考軌跡曲線的基本可行軌跡簇。

基于上述原理，基本可行軌跡簇的生成，具體包括以下步驟：

①根據(jù)車輛轉(zhuǎn)向時(shí)的動力學(xué)約束和起終點(diǎn)限制條件，確定轉(zhuǎn)向軌跡曲線參數(shù)l、l1、l2、κmax的求解空間：

l1＝λ1l,l2＝λ2l且0≤λ1≤λ2≤1

0≤kmax≤σmax·max(l1,-l2)

式中，l1和l2分別為轉(zhuǎn)彎軌跡曲線中曲率增加段和曲率減小段的長度；λ1和λ2分別為轉(zhuǎn)彎軌跡曲線中曲率增加段和曲率減小段占轉(zhuǎn)彎軌跡曲線總長度的比例。

②建立轉(zhuǎn)向軌跡曲線終點(diǎn)處的車輛車身方向角以及橫坐標(biāo)和縱坐標(biāo)的計(jì)算公式：

由曲率的定義可知，轉(zhuǎn)向軌跡曲線任意弧長s處的切線方向?yàn)椋?/p>

因此在轉(zhuǎn)向軌跡曲線的終點(diǎn)l處的車輛車身方向角為：

則轉(zhuǎn)向軌跡曲線終點(diǎn)的橫坐標(biāo)和縱坐標(biāo)分別為：

式中，θ(s)為轉(zhuǎn)彎軌跡曲線任意弧長s處的方向角。

③根據(jù)轉(zhuǎn)向軌跡曲線終點(diǎn)處的車輛車身方向角以及橫坐標(biāo)和縱坐標(biāo)的計(jì)算公式，計(jì)算求解空間中的點(diǎn)(li,l1i，l2i，κmaxi)在終點(diǎn)處的車身方向角及橫縱坐標(biāo)θei，xei，yei；如果θei，xei，yei在出車道中心點(diǎn)信息θe，xe，ye的可行領(lǐng)域內(nèi)，則將該曲線留在曲線簇中；最終得到一定數(shù)量曲線的基本可行軌跡簇，如圖8所示。

其中，可行領(lǐng)域是指轉(zhuǎn)向軌跡曲線的終點(diǎn)位置加上半個(gè)車身的寬度不能超出車道實(shí)線，且終點(diǎn)處的車身方向角不能超過限定值；限定值根據(jù)經(jīng)驗(yàn)一般取±30°。

4)最優(yōu)軌跡選?。阂詸M向加速度對時(shí)間的積分為指標(biāo)，從基本可行軌跡簇中選擇最為舒適的一條作為本發(fā)明最終生成的參考行駛軌跡。

轉(zhuǎn)向軌跡曲線上任意一點(diǎn)的橫向加速度為：

at＝kv²

則橫向加速度對時(shí)間的積分為：

式中，k為基本可行軌跡簇中轉(zhuǎn)向軌跡曲線的曲率；v為車速，一般取平均車速，比如30km/h。

遍歷基本可行軌跡簇中所有的轉(zhuǎn)向軌跡曲線，選擇a值最小的轉(zhuǎn)向軌跡曲線，即得到最終的最佳路口內(nèi)參考行駛軌跡。

上述各實(shí)施例僅用于說明本發(fā)明，其中各部件的結(jié)構(gòu)、設(shè)置位置及其連接方式等都是可以有所變化的，凡是在本發(fā)明技術(shù)方案的基礎(chǔ)上進(jìn)行的等同變換和改進(jìn)，均不應(yīng)排除在本發(fā)明的保護(hù)范圍之外。