本發(fā)明屬于計算機和自動化技術(shù)，特別是智能車輛路徑規(guī)劃技術(shù)領(lǐng)域，具體涉及一種基于控制采樣的智能車輛路徑規(guī)劃系統(tǒng)及方法。

背景技術(shù)：

路徑規(guī)劃技術(shù)是智能車輛關(guān)鍵技術(shù)問題之一。智能車輛在動態(tài)的不確定環(huán)境中，根據(jù)環(huán)境感知系統(tǒng)和車輛狀態(tài)測量系統(tǒng)提供的信息，和所要到達的目的地，實時地規(guī)劃出車輛當前應(yīng)該行駛的路徑，使車輛能安全快速地到達終點。

中國專利申請：用于無人駕駛汽車局部路徑規(guī)劃的裝置及方法(申請?zhí)枺?01110007154.x)采用人工勢場法綜合環(huán)障礙物和目標點等因素的影響產(chǎn)生最終路徑。該方法簡單易行，實時性有保證，但是產(chǎn)生的路徑可能違背車輛運動學約束，運動控制系統(tǒng)執(zhí)行起來會有較大誤差。中國專利申請：一種隨機路線圖的快速路徑規(guī)劃方法及增強方法(申請?zhí)枺?01010519248.0)改進的prm(probabilisticroadmapsmethod，概率路徑圖法)方法解決了一般prm方法完全等概率均勻分布自由節(jié)點導(dǎo)致困難區(qū)域自由節(jié)點數(shù)目分布不夠的問題，具有較好的完備性，減少了遺漏某些通過困難區(qū)域的可行路徑的概率，但與傳統(tǒng)prm方法一樣未能解決基于隨機獲得的自由節(jié)點產(chǎn)生的路徑不能保證符合車輛運動學約束的問題。文獻：”杜明博等，復(fù)雜環(huán)境下基于rrt的智能車輛運動規(guī)劃算法，機器人,2015(04):443-450”在距離度量函數(shù)中加入了對路段間夾角的度量，一定程度上改善路徑的平順性，但此限制排除了大量符合車輛運動學的路徑形態(tài)，導(dǎo)致在較復(fù)雜的環(huán)境下路徑規(guī)劃成功率較低。文獻：”hundelshausenf.v.，etal.drivingwithtentacles:integralstructuresforsensingandmotion，journaloffieldrobotics，2009，25(9):640-673”通過對預(yù)先設(shè)定好的平滑路徑簇中路徑進行評價來選取一條最優(yōu)路徑執(zhí)行，但沒有考慮到車輛前輪轉(zhuǎn)角對路徑選擇的限制而導(dǎo)致選取的最優(yōu)路徑與車輛實際運動軌跡有較大差距。文獻：”leecheng-lung,etal.anewtrajectory-basedpathplanningapproachfordifferentialdrivevehicles，2013ieeeinternationalsymposiumonroboticandsensorsenvironments”依據(jù)差分運動方程生成其在不同控制輸入和運動狀態(tài)下的軌跡集，依據(jù)探測到的實時障礙位置和實時車速對這些軌跡進行篩選匹配出合適的軌跡執(zhí)行。該類探索型機器人的應(yīng)用場景中環(huán)境完全是未知的，因此機器人的運動過程并沒有清晰的目標，因此是一種反應(yīng)式運動規(guī)劃方法，與智能車輛的路徑規(guī)劃應(yīng)用場景不同。

在現(xiàn)有的路徑規(guī)劃方法中，基于采樣的路徑規(guī)劃方法避免了對路徑與環(huán)境精確建模，具有實現(xiàn)簡單，實時性高等特點，因此在智能車輛路徑規(guī)劃領(lǐng)域內(nèi)被廣泛使用。目前流行的prm等基于采樣的路徑規(guī)劃方法直接采樣于車輛所在的構(gòu)型空間，產(chǎn)生的路徑具有較強的不確定性，路徑過于抖動、曲折，不可避免地會違背車輛運動學約束，使得路徑的可執(zhí)行性差。

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

本發(fā)明的目的是解決無人駕駛智能車輛的路徑規(guī)劃問題。當存在大量無規(guī)則分布障礙物時，現(xiàn)有智能車輛路徑規(guī)劃方法存在建模復(fù)雜，計算復(fù)雜度高，收斂速度慢，規(guī)劃出的路徑可執(zhí)行性差等問題，為此本發(fā)明提出了一種無須對環(huán)境、軌跡等進行復(fù)雜的建模、有較快的收斂速度、規(guī)劃出的路徑符合車輛運動學約束，可執(zhí)行性佳的基于控制采樣的智能車輛路徑規(guī)劃系統(tǒng)及方法。本發(fā)明的技術(shù)方案如下：

一種基于控制采樣的智能車輛路徑規(guī)劃系統(tǒng)，包括環(huán)境地圖模塊、車輛狀態(tài)計算模塊和路徑搜索模塊。其中，所述環(huán)境地圖模塊，用于將車載傳感器提供的環(huán)境信息以柵格地圖形式存儲下來，為路徑規(guī)劃提供任務(wù)場景信息，供車輛狀態(tài)計算模塊和路徑搜索模塊訪問；還用于環(huán)境地圖更新，實時接收車輛參數(shù)測量裝置提供的車輛狀態(tài)信息和車載傳感器提供的環(huán)境信息來更新柵格地圖。車輛狀態(tài)計算模塊，用于通過碰撞檢查算法，判斷某一車輛狀態(tài)是否與環(huán)境中障礙物發(fā)生碰撞；依據(jù)車輛狀態(tài)擴展模型描述的車輛狀態(tài)擴展方程和控制空間采樣策略，對任意車輛狀態(tài)擴展，得一簇新的狀態(tài)；并基于當前狀態(tài)、環(huán)境障礙物信息和本次任務(wù)的目標狀態(tài)，實現(xiàn)對車輛狀態(tài)的狀態(tài)評價。路徑搜索模塊，用于采用初步路徑搜索算法，得到以離散狀態(tài)序列形式的初步路徑；并通過路徑插值，對初步路徑的狀態(tài)序列，利用滿足切向約束的2次b樣條插值優(yōu)化處理，得到連續(xù)可執(zhí)行的最終路徑。

進一步的，所述車輛狀態(tài)計算模塊：在實現(xiàn)碰撞檢查功能時，將車輛抽象地看做一個矩形，稱為車輛區(qū)域，車輛區(qū)域?qū)④囕v整體包括在內(nèi)，一次碰撞檢查過程分為如下兩步：

(1)確定掃描柵格范圍，為減小一次碰撞檢查過程中掃描的柵格范圍，取車輛區(qū)域的四個頂點，勾勒出一個掃描范圍；

(2)判斷障礙柵格是否在車輛區(qū)域內(nèi)，對于掃描范圍內(nèi)的柵格，首先判斷其是否為障礙物柵格，若不是障礙物柵格，則繼續(xù)掃描下一個柵格；若為障礙物柵格，則判斷障礙柵格是否在車輛區(qū)域內(nèi)，若障礙物柵格在車輛區(qū)域內(nèi)，則判定為碰撞。

進一步的，所述車輛狀態(tài)計算模塊：依據(jù)車輛狀態(tài)擴展方程，在控制輸入下，預(yù)測一個步長后的車輛狀態(tài)。

進一步的，所述路徑搜索模塊：用于采用初步路徑搜索算法，得到以離散狀態(tài)序列形式的初步路徑。路徑搜索算法具體分為路徑選擇，節(jié)點擴展，節(jié)點更新，初步路徑生成這四個步驟：

(1)路徑選擇：從根狀態(tài)節(jié)點起，依據(jù)節(jié)點選擇公式從子狀態(tài)節(jié)點中選擇一個狀態(tài)節(jié)點，重復(fù)直至到達當前狀態(tài)樹的葉節(jié)點；

(2)節(jié)點擴展：采樣控制輸入，并代入狀態(tài)擴展方程，得到一批新的節(jié)點，對這些新節(jié)點對應(yīng)的車輛空間位姿作碰撞檢查，保留無碰撞節(jié)點為新的葉節(jié)點，計算新葉節(jié)點的評價值；

(3)節(jié)點更新：當狀態(tài)擴展產(chǎn)生的新狀態(tài)中沒有到達目標狀態(tài)的節(jié)點時，將該次狀態(tài)擴展的信息沿所選擇的路徑反向傳播，形式為更新路徑途徑節(jié)點的評價值和節(jié)點訪問次數(shù)；

(4)初步路徑生成，當狀態(tài)擴展產(chǎn)生的某新狀態(tài)到達目標狀態(tài)時，由根狀態(tài)節(jié)點到該新狀態(tài)節(jié)點的狀態(tài)序列即為初步路徑。

進一步的，所述路徑選擇，采用uct(upperconfidenceboundappliedtotreesearch，應(yīng)用于樹搜索的上限置信區(qū)間方法)選擇路徑。

進一步的，所述路徑搜索模塊，用于路徑插值，對初步路徑的狀態(tài)序列，利用滿足切向約束的2次b樣條插值優(yōu)化處理，得到連續(xù)可執(zhí)行的最終路徑。具體包括：對初步路徑的狀態(tài)序列，利用滿足切向約束的2次b樣條插值優(yōu)化處理，得到最終的連續(xù)曲率且符合車輛非完整約束的易于跟蹤的最終路徑。

一種基于所述系統(tǒng)的基于控制采樣的智能車輛路徑規(guī)劃方法，其包括以下步驟：

(1)更新當前環(huán)境地圖：從車載傳感器讀入實時環(huán)境障礙信息，將它們以柵格地圖形式存儲于環(huán)境地圖模塊；每個柵格有占據(jù)和非占據(jù)兩種狀態(tài)。占據(jù)狀態(tài)表示該柵格所處位置存在障礙物；

(2)設(shè)定車輛當前狀態(tài)：從車輛參數(shù)測量裝置得到的實時車輛位姿和速度信息，這些參數(shù)組成了本次路徑規(guī)劃任務(wù)的車輛起始狀態(tài)；

(3)設(shè)定車輛目標狀態(tài)：設(shè)定車輛在任務(wù)結(jié)束時的狀態(tài)，在駕駛輔助系統(tǒng)中，目標狀態(tài)由駕駛員設(shè)定；在無人駕駛系統(tǒng)中，目標狀態(tài)由上層決策系統(tǒng)給出；

(4)初步路徑搜索：針對任務(wù)起始狀態(tài)和目標狀態(tài)運行初步路徑搜索算法，路徑搜索算法具體分為四步：

(4.1)路徑選擇：從根狀態(tài)節(jié)點起，依據(jù)節(jié)點選擇公式從子狀態(tài)節(jié)點中選擇一個狀態(tài)節(jié)點，重復(fù)直至到達當前狀態(tài)樹的葉節(jié)點。

(4.2)節(jié)點擴展：采樣控制輸入，并代入狀態(tài)擴展方程，得到一批新的節(jié)點。對這些新節(jié)點對應(yīng)的車輛空間位姿作碰撞檢查，保留無碰撞節(jié)點為新的葉節(jié)點。計算新葉節(jié)點的評價值；

(4.3)節(jié)點更新：當狀態(tài)擴展產(chǎn)生的新狀態(tài)中沒有到達目標狀態(tài)的節(jié)點時，將該次狀態(tài)擴展的信息沿所選擇的路徑反向傳播，形式為更新路徑途徑節(jié)點的評價值和節(jié)點訪問次數(shù)；

(4.4)初步路徑生成：重復(fù)(4.1)-(4.3)，當狀態(tài)擴展產(chǎn)生的某新狀態(tài)到達目標狀態(tài)時，由根狀態(tài)節(jié)點到該新狀態(tài)節(jié)點的狀態(tài)序列即為初步路徑；

(5)路徑插值：對初步路徑的狀態(tài)序列，利用滿足切向約束的2次b樣條插值優(yōu)化處理，得到最終的連續(xù)曲率且符合車輛非完整約束的易于跟蹤的最終路徑。

本發(fā)明的優(yōu)點及有益效果如下：

本專利通過建立車輛狀態(tài)擴展方程，得到控制輸入與未來狀態(tài)的對應(yīng)關(guān)系。依據(jù)車輛實際轉(zhuǎn)向能力和控制機構(gòu)實時狀態(tài)，實時構(gòu)建控制輸入采樣區(qū)間。通過在采樣區(qū)間采樣控制輸入來不斷擴展車輛狀態(tài)，進而實現(xiàn)增量式路徑搜索。這種方式使得路徑樹上相鄰狀態(tài)點間的過渡滿足車輛運動學約束。因此，通過插值得到的最終路徑平滑、穩(wěn)定，曲率半徑滿足車輛最小轉(zhuǎn)向半徑的約束，可大大減少車輛路徑跟蹤過程的誤差。

同時，通過車輛狀態(tài)擴展方程，本發(fā)明將增量搜索過程建模為蒙特卡洛搜索樹。將路徑搜索問題轉(zhuǎn)化成博弈問題，通過uct方法實現(xiàn)最優(yōu)路徑搜索。uct搜索作為一種并行搜索算法，能突破其他路徑規(guī)劃算法遇到的性能瓶頸，在得到最優(yōu)路徑的同時實時性更好。

附圖說明

圖1是本發(fā)明提供優(yōu)選實施例一種基于控制采樣的智能車輛路徑規(guī)劃系統(tǒng)總體框架；

圖2本發(fā)明基于控制采樣的智能車輛路徑規(guī)劃方法流程；

圖3本發(fā)明車輛狀態(tài)及車輛區(qū)域定義；

圖4本發(fā)明碰撞檢查算法原理；

圖5本發(fā)明初步路徑搜索過程流程圖；

圖6本發(fā)明執(zhí)行一次路徑規(guī)劃的實例表示；

具體實施方式

下面將結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進行清楚、詳細地描述。所描述的實施例僅僅是本發(fā)明的一部分實施例。

本發(fā)明解決上述技術(shù)問題的技術(shù)方案是：

首先依據(jù)車輛參數(shù)、運動學方程和推導(dǎo)車輛狀態(tài)擴展方程以及控制輸入(前輪轉(zhuǎn)角)的采樣范圍。然后，從起始狀態(tài)(當前狀態(tài))開始，利用采樣得到的控制輸入不斷擴展狀態(tài)樹，直到狀態(tài)樹葉節(jié)點到達目標點。此時得到由狀態(tài)點序列組成的初始路徑。最后，對初始路徑進行處理，以使其便于被跟蹤執(zhí)行。

以下結(jié)合附圖和具體實例對本發(fā)明所述系統(tǒng)的具體工作方式進行描述：

如圖1所示為本發(fā)明一種基于控制采樣的智能車輛路徑規(guī)劃系統(tǒng)總體框架,系統(tǒng)包括環(huán)境地圖模塊、車輛狀態(tài)計算模塊和路徑搜索模塊。

其中，所述環(huán)境地圖模塊，用于將車載傳感器提供的環(huán)境信息以柵格地圖形式存儲下來，為路徑規(guī)劃提供任務(wù)場景信息，供車輛狀態(tài)計算模塊和路徑搜索模塊訪問；還用于環(huán)境地圖更新，實時接收車輛參數(shù)測量裝置提供的車輛狀態(tài)信息和車載傳感器提供的環(huán)境信息來更新柵格地圖；

車輛狀態(tài)計算模塊，用于通過碰撞檢查算法，判斷某一車輛狀態(tài)是否與環(huán)境中障礙物發(fā)生碰撞；依據(jù)車輛狀態(tài)擴展模型描述的車輛狀態(tài)擴展方程和控制空間采樣策略，對任意車輛狀態(tài)擴展，得一簇新的狀態(tài)；并基于當前狀態(tài)、環(huán)境障礙物信息和本次任務(wù)的目標狀態(tài)，實現(xiàn)對車輛狀態(tài)的狀態(tài)評價；

路徑搜索模塊，用于采用初步路徑搜索算法，得到以離散狀態(tài)序列形式的初步路徑；并通過路徑插值，對初步路徑的狀態(tài)序列，利用滿足切向約束的2次b樣條插值優(yōu)化處理，得到連續(xù)可執(zhí)行的最終路徑。

如圖2所示為本發(fā)明一種基于控制采樣的智能車輛路徑規(guī)劃方法流程，該方法包括以下步驟：

(1)更新當前環(huán)境地圖：從車載傳感器讀入實時環(huán)境障礙信息，將它們以柵格地圖形式存儲于環(huán)境地圖模塊；每個柵格有占據(jù)和非占據(jù)兩種狀態(tài)。占據(jù)狀態(tài)表示該柵格所處位置存在障礙物；

(2)設(shè)定車輛當前狀態(tài)：從車輛參數(shù)測量裝置得到的實時車輛位姿和速度信息，這些參數(shù)組成了本次路徑規(guī)劃任務(wù)的車輛起始狀態(tài)；

(3)設(shè)定車輛目標狀態(tài)：設(shè)定車輛在任務(wù)結(jié)束時的狀態(tài)，駕駛輔助系統(tǒng)中目標狀態(tài)由駕駛員設(shè)定；在無人駕駛系統(tǒng)中目標狀態(tài)由上層決策系統(tǒng)給出；

(4)初步路徑搜索：針對任務(wù)起始狀態(tài)和目標狀態(tài)運行初步路徑搜索算法，路徑搜索算法具體分為四步：

(4.1)路徑選擇：從根狀態(tài)節(jié)點起，依據(jù)節(jié)點選擇公式從子狀態(tài)節(jié)點中選擇一個狀態(tài)節(jié)點，重復(fù)直至到達當前狀態(tài)樹的葉節(jié)點。

(4.2)節(jié)點擴展：采樣控制輸入，并代入狀態(tài)擴展方程，得到一批新的節(jié)點。對這些新節(jié)點對應(yīng)的車輛空間位姿作碰撞檢查，保留無碰撞節(jié)點為新的葉節(jié)點。計算新葉節(jié)點的評價值；

(4.3)節(jié)點更新：當狀態(tài)擴展產(chǎn)生的新狀態(tài)中沒有到達目標狀態(tài)的節(jié)點時，將該次狀態(tài)擴展的信息沿所選擇的路徑反向傳播，形式為更新路徑途徑節(jié)點的評價值和節(jié)點訪問次數(shù)；

(4.4)初步路徑生成：重復(fù)(4.1)-(4.3)，當狀態(tài)擴展產(chǎn)生的某新狀態(tài)到達目標狀態(tài)時，由根狀態(tài)節(jié)點到該新狀態(tài)節(jié)點的狀態(tài)序列即為初步路徑。

(5)路徑插值：初步路徑是由空間中的車輛狀態(tài)點序列組成的，屬分段線性路徑。為使得路徑的表示更直觀明確，同時提高控制器在路徑跟蹤時候的跟蹤品質(zhì)，減小跟蹤誤差，需要對初步路徑進行一些優(yōu)化處理，得到曲率連續(xù)且滿足非完整約束的最終路徑。

由于車輛狀態(tài)點的狀態(tài)包括車輛坐標位置和姿態(tài)角，相連的兩狀態(tài)點由狀態(tài)采樣擴展而來，其姿態(tài)角分別代表該段路徑的起點的切線方向和終點的切線方向，因此采用二次b樣條曲線插值方法連接各個車輛狀態(tài)點，得到曲率連續(xù)且滿足非完整約束的最終路徑。

假設(shè)組成初步路徑的n個狀態(tài)點的序列為q1,q2,…,qn,提取各狀態(tài)中的坐標信息得到對應(yīng)的插值點序列p1,p2,…,pn。提取各狀態(tài)中的姿態(tài)角信息，得到各個插值點處曲線單位切矢v1,v2,…,vn。令表示節(jié)點向量，控制點為d1,d2,…,dn，令所求曲線為端點插值，即d1＝p1,dn＝pn,t0＝t1＝t2,tn+1＝tn+2＝tn+3，插值曲線c(t)滿足以下約束

c(ti+1)＝pi,i＝1,2,…,n

采用基于切向約束構(gòu)造復(fù)合二次b樣條插值方法，可求出

其中，bi,3(t)是規(guī)范二次b樣條的基函數(shù)。

如圖3所示為本發(fā)明車輛狀態(tài)及車輛區(qū)域定義，用于車輛狀態(tài)計算，包括碰撞檢查與車輛狀態(tài)擴展。

將車輛抽象地看做一個矩形范圍，稱為車輛區(qū)域。其中心與車輛中心重合，將車輛整體包括在矩形內(nèi)。車輛區(qū)域的長為length，寬為width。lengthmin和widthmin對應(yīng)于車長與車寬。參數(shù)length,width可依據(jù)環(huán)境因素和車速設(shè)置，以滿足不同條件下所規(guī)劃路徑的安全性和穩(wěn)定性需求。例如，在高速公路等較為開闊平直且車速高的環(huán)境中，可取較長的length和width提高路徑搜索效率；而在障礙物情況復(fù)雜且車速較低的十字路口或車庫等環(huán)境中，可取較小的length和width以求規(guī)劃出更精細復(fù)雜的路徑。

如圖4所示為本發(fā)明碰撞檢查算法原理。碰撞檢查依據(jù)環(huán)境模型中定義的車輛區(qū)域和碰撞檢查算法，判斷某一車輛狀態(tài)是否與環(huán)境中障礙物發(fā)生碰撞。若有障礙物柵格出現(xiàn)在車輛區(qū)域內(nèi)，則判定為碰撞。一次碰撞檢查過程分為如下兩步：

(1)確定掃描柵格范圍：為減小一次碰撞檢查過程中掃描的柵格范圍，取車輛區(qū)域的四個頂點abcd，利用他們勾勒出一個較小的掃描范圍，掃描范圍為點a和點d確定的矩形范圍abcd內(nèi)的所有柵格，即中心坐標在集合

{(x,y)|xa≤x≤xd,ya≤y≤yd}

中的所有柵格。其中有

(2)判斷障礙柵格是否在車輛區(qū)域內(nèi)：對于掃描范圍內(nèi)的柵格，首先判斷其是否為障礙物柵格，若不是障礙物柵格，則繼續(xù)掃描下一個柵格；若為障礙物柵格，則判斷障礙柵格是否在車輛區(qū)域內(nèi)。以圖中障礙物柵格o為例，該柵格坐標為(xo,yo)。若以下不等式組

成立，則代表柵格在車輛區(qū)域內(nèi)，判定該車輛區(qū)域與障礙物發(fā)生碰撞。

如圖5所示為本發(fā)明初步路徑搜索流程圖。初步路徑搜索過程是路徑樹的擴展過程，也是車輛狀態(tài)擴展過程，依據(jù)車輛狀態(tài)擴展模型描述的車輛狀態(tài)擴展方程和控制空間采樣策略，對任意車輛狀態(tài)擴展，得一簇新的狀態(tài)，并對車輛狀態(tài)進行評價，作為路徑搜索的依據(jù)。

車輛狀態(tài)擴展：已知車輛參數(shù)ωmax為車輛前輪最大轉(zhuǎn)向角速度，車輛前后軸距離為l，δmax代表前輪最大轉(zhuǎn)向角的絕對值，車輛當前線速度為u。定義車輛狀態(tài)為x為車輛中心橫坐標，y為車輛中心縱坐標，為車輛航向角，目標點狀態(tài)記為qgoal。依據(jù)車輛運動學模型、阿克曼轉(zhuǎn)向模型得到車輛狀態(tài)擴展方程：車輛狀態(tài)擴展方程為

其中表示此刻車輛狀態(tài)；表示擴展后車輛狀態(tài)，δs為車輛狀態(tài)擴展的單步弧長。為保證在車輛狀態(tài)擴展過程中，具有父子關(guān)系的兩個狀態(tài)所對應(yīng)的車輛區(qū)域保持聯(lián)通，避免狹小障礙物出現(xiàn)在兩個非聯(lián)通車輛區(qū)域之間，但該次狀態(tài)擴展確被判定為無碰撞的錯誤情形，始終取δs＝length。

在確定控制輸入采樣策略時，考慮到車輛轉(zhuǎn)向機構(gòu)存在前輪最大轉(zhuǎn)向角速度ωmax限制和前輪最大轉(zhuǎn)向角δmax的限制，得到當前前輪轉(zhuǎn)角為δnow時，控制輸入的采樣區(qū)間為

δi＝[min(-δmax,δnow-ωmaxt),max(δmax,δnow+ωmaxt)]

δi為擴展到下一狀態(tài)的前輪轉(zhuǎn)角輸入，為該狀態(tài)擴展方程唯一輸入。其中t為狀態(tài)擴展的時間步長。假設(shè)此時車速為u,當前狀態(tài)下車輛的前輪轉(zhuǎn)角為δnow，則可得狀態(tài)擴展的時間步長t＝δs/u。在該區(qū)間內(nèi)均勻采樣得到離散的控制輸入以擴展當前狀態(tài)。

車輛狀態(tài)評價：基于當前狀態(tài)、環(huán)境障礙物信息和本次任務(wù)的目標狀態(tài)對車輛狀態(tài)的狀態(tài)評價，狀態(tài)節(jié)點評價值vq的定義如下：

vq＝r(wdistvdist+wheurvheur)

其中vdist表示從起始節(jié)點到節(jié)點q所經(jīng)過的路程，wdist為權(quán)值。有：

vdist(q)＝vdist(p)+δs

vheur表示狀態(tài)節(jié)q的啟發(fā)值，為從q節(jié)點到qgoal的二維歐式距離,wheur為權(quán)值。有：

r為目標點威脅懲罰因子，其計算公式如下

其中n為當前節(jié)點q的所有兄弟結(jié)點數(shù)目，ncollison為當前節(jié)點q的所有兄弟節(jié)點中，與障礙物發(fā)生了碰撞的節(jié)點的數(shù)目。

計算本次節(jié)點擴展過程所產(chǎn)生的新節(jié)點的評價值均值

基于上述車輛狀態(tài)擴展及評價方法，初步路徑搜索的步驟包括路徑選擇，節(jié)點擴展，節(jié)點更新三個子過程，三個子過程循環(huán)執(zhí)行，直到狀態(tài)樹擴展到目標點，得到以離散狀態(tài)序列形式的初步路徑。

(1)路徑選擇：

路徑選擇過程中，從根節(jié)點出發(fā)，逐層地選擇節(jié)點組成一條新的搜索路徑。因此，路徑選擇過程的關(guān)鍵在于節(jié)點選擇方法。采用uct方法選擇路徑，標準uct方法的節(jié)點選擇公式為：

其中，k為選擇的狀態(tài)節(jié)點，q代表為候選狀態(tài)節(jié)點，q為候選節(jié)點集合，vq為節(jié)點q的評價值，c為常數(shù)，通常依據(jù)經(jīng)驗選擇為0.8。np為節(jié)點q的父節(jié)點p的訪問次數(shù)，nq為節(jié)點q的訪問次數(shù)。

(2)節(jié)點擴展：

當路徑搜索到當前狀態(tài)樹的某一葉節(jié)點時，需要進行節(jié)點擴展。在該葉節(jié)點的狀態(tài)基礎(chǔ)上，通過對當前控制輸入空間的均勻采樣，得n個控制輸入δi(i＝1,2…n)，以此擴展出n個新狀態(tài)節(jié)點qi(i＝1,2…n)。若新狀態(tài)節(jié)點中存在到達目標點的節(jié)點，代表已經(jīng)搜索到從起點到目標點的初步路徑，則退出整個路徑搜索循環(huán)。否則，對每個新節(jié)點作碰撞檢查，從狀態(tài)樹上刪除其中有碰撞的狀態(tài)節(jié)點ncollison個，保留無碰撞節(jié)點n-ncollison個。計算節(jié)點的評價值vi(i＝1,2…n)，其中碰撞節(jié)點的評價值記為0。

(3)節(jié)點更新：

節(jié)點更新過程是一個信息的反向傳播過程，利用節(jié)點擴展過程對環(huán)境的探索結(jié)果，對狀態(tài)樹相關(guān)節(jié)點的信息進行更新，保證后續(xù)的路徑搜索過程能利用到之前搜索過程的信息。

狀態(tài)節(jié)點q的信息包括節(jié)點評價值vq和節(jié)點訪問次數(shù)nq兩部分。對于搜索路徑上經(jīng)過的所有節(jié)點，更新其評價值為v′q:

更新其節(jié)點訪問次數(shù)為：

n′q＝nq+1

圖6所示，即為本專利所描述的方法進行路徑規(guī)劃的一次實例。其中空心點和黑色實心點均表示當前車輛狀態(tài)樹上的狀態(tài)。黑色實心點序列即為初步路徑，連接初步路徑的黑色粗曲線即為得到的最終路徑，其符合車輛運動學約束，易于被執(zhí)行機構(gòu)控制車輛跟蹤。

上文給出的實例描述是為了便于該領(lǐng)域內(nèi)專家學者和技術(shù)人員理解和應(yīng)用本發(fā)明。該領(lǐng)域內(nèi)的專家學者與技術(shù)人員顯然可以容易地基于本專利的實例描述，加以簡單修改后將本專利說明的一般原理應(yīng)用到相關(guān)實例中，而不必經(jīng)過創(chuàng)造性的勞動。因此，本領(lǐng)域技術(shù)人員依據(jù)本發(fā)明所述一般原理，對本發(fā)明做出的修改和改進都在本專利保護范圍之內(nèi)。

以上這些實施例應(yīng)理解為僅用于說明本發(fā)明而不用于限制本發(fā)明的保護范圍。在閱讀了本發(fā)明的記載的內(nèi)容之后，技術(shù)人員可以對本發(fā)明作各種改動或修改，這些等效變化和修飾同樣落入本發(fā)明權(quán)利要求所限定的范圍。