判斷該最 小值是否小于所述第一閾值，若小于所述第一閾值，則確定該最小值對(duì)應(yīng)的柵格為此次搜 索的第二級(jí)父柵格，將此次搜索的第二級(jí)父柵格存入所述已搜索列表中，并將所述第一級(jí) 父柵格作為此次搜索的第二級(jí)父柵格的父柵格，將此次搜索的第二級(jí)父柵格作為所述第一 級(jí)父柵格的子?xùn)鸥?，建立所述第一?jí)父柵格和此次搜索的第二級(jí)父柵格的父子關(guān)系，否則， 若該最小值不小于所述第一閾值，或者該三個(gè)柵格均存在于所述已搜索列表中，則將所述 已搜索列表中的歷史父子關(guān)系鏈中第一次距離目的區(qū)域柵格最近的歷史父子關(guān)系鏈相應(yīng) 的路徑確定為從車輛車頭中心位置所處柵格至目的區(qū)域柵格所能達(dá)到的最遠(yuǎn)路徑，i為正 整數(shù)。
[0027] 本申請(qǐng)中SRA搜索算法建立于以下基礎(chǔ)：
[0028] 1、道路中所檢測(cè)的障礙物為靜態(tài)，或速度較低，在智能車決策的一個(gè)周期內(nèi)可以 看成靜態(tài)障礙物來處理。
[0029] 2、已經(jīng)利用環(huán)境感知方法準(zhǔn)確獲取以下全部或部分信息：
[0030] (1)障礙物大小、位置及分布；
[0031] (2)車道線位置及種類（如雙黃線，白虛線等）；
[0032] (3)本車速度及安全轉(zhuǎn)向角度；
[0033] (4)當(dāng)前行駛?cè)蝿?wù)；
[0034] (5)其他需要危險(xiǎn)評(píng)估的已知信息。
[0035] 3、智能車可以按照SRA搜索算法提供的路徑行駛。
[0036] 下面對(duì)本發(fā)明的一具體實(shí)施例進(jìn)行詳細(xì)說明。
[0037] 本發(fā)明一種基于道路危險(xiǎn)評(píng)估的智能車路徑搜索方法的具體步驟如下：
[0038] (1)構(gòu)造環(huán)境柵格圖。
[0039] 將道路環(huán)境抽象成邊長相同的二維矩形柵格，柵格的邊長根據(jù)環(huán)境識(shí)別精度和車 輛控制精度選?。ㄔ趯?shí)施例中選取的長度為20cm)。將柵格編號(hào)后，把本車自身位置，障礙 物位置、車道中心線位置等都描述為其在柵格中的坐標(biāo)位置。如圖2所示，是把障礙物位置 描述為其所在的柵格區(qū)域的示意圖。
[0040] (2)確定柵格的危險(xiǎn)估計(jì)值。
[0041] 若智能車行駛到任意柵格所在的區(qū)域，都會(huì)面臨來自環(huán)境和自身的危險(xiǎn)，其危險(xiǎn) 評(píng)估方法如下：
[0042]f(n) =g(n)+h(n)+i(n) +j(n)+k(n),
[0043] 其中，f(n)是當(dāng)前柵格危險(xiǎn)評(píng)估值，g(n)是障礙物危險(xiǎn)評(píng)估值，h(n)是車速危險(xiǎn) 評(píng)估值，i(n)是車道線危險(xiǎn)評(píng)估值，j(n)是任務(wù)危險(xiǎn)評(píng)估值，k(n)是其他危險(xiǎn)評(píng)估值。
[0044] S卩：當(dāng)前危險(xiǎn)評(píng)估值=障礙物危險(xiǎn)評(píng)估值+車速危險(xiǎn)評(píng)估值+車道線危險(xiǎn)評(píng)估值 +任務(wù)危險(xiǎn)評(píng)估值+其他危險(xiǎn)評(píng)估值。
[0045] 障礙物危險(xiǎn)評(píng)估g(n)是指，道路中出現(xiàn)的障礙物對(duì)柵格區(qū)域危險(xiǎn)程度的影響值。 這里的障礙物包括正障礙（如車輛、行人）和負(fù)障礙（如水坑，懸崖）。根據(jù)常理我們可 知，車輛距離障礙物越近其面臨的危險(xiǎn)程度越大，而離障礙物越遠(yuǎn)越安全。因此柵格障礙 物危險(xiǎn)評(píng)估值同該柵格與最近障礙物之間的距離成反比，于是可以構(gòu)造如下公式：g(n)= W-S?u，其中，w為該類方法中指定的最大危險(xiǎn)值。S為當(dāng)前柵格到最近障礙柵格的柵格距 離，可以規(guī)定，兩個(gè)相鄰柵格的距離值為1，對(duì)角線方向相鄰的柵格距離為，以此類推。u 為單位距離的第一危險(xiǎn)值。
[0046]如圖3所示，A，B為障礙物所在柵格，C，D為待求柵格。假設(shè)我們指定柵格的最大 危險(xiǎn)值為1〇〇〇,單位距離第一危險(xiǎn)值為1〇〇。那么對(duì)于柵格C，障礙A離它最近，且柵格距離 為2,因此柵格C的障礙物危險(xiǎn)評(píng)估值為g(C) = 1000-2 ? 100 = 800。
[0047] 對(duì)于柵格D，障礙A離它最近，且柵格距離為，
，因此柵格D的障礙物 危險(xiǎn)評(píng)估值為
，運(yùn)用此種方法可以計(jì)算任意柵格的障礙物危險(xiǎn) 估計(jì)值g(n)。
[0048] 車速危險(xiǎn)評(píng)估h(n)是指，當(dāng)本車處于任意柵格時(shí)，在當(dāng)前車速和行駛方向的 條件下，能夠到達(dá)另一柵格的危險(xiǎn)評(píng)估值。根據(jù)車體動(dòng)力學(xué)我們可知，當(dāng)車速較快時(shí)， 轉(zhuǎn)向角越大則離心力越大，車輛越面臨翻車的危險(xiǎn)；相反轉(zhuǎn)向角越小越安全。因此柵 格車速危險(xiǎn)評(píng)估值同車輛當(dāng)前位置、車速、轉(zhuǎn)向角一定關(guān)系。于是可以構(gòu)造如下公式：
[0049] 其中，a為車輛行駛到該柵格需要轉(zhuǎn)向的柵格角度，由于本文所述搜索算法只評(píng) 估車輛所處柵格之后的三個(gè)柵格，因此轉(zhuǎn)向的柵格角度只有可能為〇度或45度，V為當(dāng)前 車速，k為一常數(shù)系數(shù)。
[0050] 車道線危險(xiǎn)評(píng)估i(n)是指，車道虛擬中心線對(duì)柵格區(qū)域危險(xiǎn)程度的影響值。根據(jù) 交通法規(guī)我們可知，車輛應(yīng)該行駛于兩車道線中間的虛擬中心線上，而長時(shí)間行駛在車道 線上特別是雙黃線，白實(shí)線上都屬于違章行為。因此車道線危險(xiǎn)評(píng)估同該柵格與車道線虛 擬中心線之間的距離成正比，于是可以構(gòu)造如下公式：
[0051] i(n) =p?q，其中，p為待計(jì)算危險(xiǎn)評(píng)估值的柵格到最近車道虛擬中心線的柵格 距離，可以規(guī)定，兩個(gè)相鄰柵格的距離值為1，以此類推，q為預(yù)設(shè)的單位柵格距離的第二危 險(xiǎn)值。
[0052] 任務(wù)危險(xiǎn)評(píng)估j(n)是指，車輛當(dāng)前執(zhí)行的特殊任務(wù)對(duì)柵格區(qū)域危險(xiǎn)程度的影響 值。此類危險(xiǎn)評(píng)估沒有具體計(jì)算方式，當(dāng)車輛正常行駛時(shí)，任務(wù)危險(xiǎn)評(píng)估值為〇 ;當(dāng)車輛執(zhí) 行特殊任務(wù)時(shí)，如紅燈停車，跟車行駛等，此時(shí)柵格任務(wù)危險(xiǎn)評(píng)估值需要人為指定。
[0053] 其他危險(xiǎn)評(píng)估k(n)是指，其他可能的因素對(duì)柵格區(qū)域危險(xiǎn)程度的影響值。此類危 險(xiǎn)評(píng)估沒有具體計(jì)算方式，當(dāng)車輛正常行駛時(shí)，其他危險(xiǎn)評(píng)估值為〇 ;當(dāng)遇到惡劣天氣，車 輛自身問題，人工干預(yù)意向等，其影響因素都可以通過人為指定的方式構(gòu)造其他危險(xiǎn)評(píng)估 值。
[0054] 在車輛行駛過程中，當(dāng)車輛傳感器只能感知到一類或者幾類環(huán)境信息時(shí)，可以令 需要根據(jù)車輛傳感器未感知到的環(huán)境信息計(jì)算得到的各項(xiàng)危險(xiǎn)評(píng)估值為〇,僅用已知環(huán)境 信息來評(píng)價(jià)道路各區(qū)域的危險(xiǎn)評(píng)估值，從而本發(fā)明所述的搜索方法仍然能夠?qū)崿F(xiàn)較優(yōu)路徑 的搜索。
[0055] (3)搜索可行駛路徑。
[0056] SRA路徑搜索算法依據(jù)各柵格危險(xiǎn)評(píng)估值，運(yùn)用深度優(yōu)先遍歷的啟發(fā)式搜索方法 進(jìn)行路徑搜索。該算法依靠父子?xùn)鸥?、已搜索列表、搜索范圍三個(gè)屬性展開：如果從柵格A 搜索到下一柵格B，則稱A為B的父柵格，B為A的子?xùn)鸥?；搜索列表保存了所有已?jīng)搜索過 的父柵格坐標(biāo)；搜索范圍是指能夠產(chǎn)生子?xùn)鸥竦臇鸥穹秶?。算法以到達(dá)目的區(qū)域或找不到 子?xùn)鸥穸Y(jié)束。其具體搜索步驟為：
[0057] 1.算法開始時(shí)，把車輛自身位置作為父柵格，并把父柵格放入到已搜索列表。由于 智能車只能向前方直線行駛或轉(zhuǎn)向，而不能橫向移動(dòng)，本文也暫不考慮倒車情況，因此智能 車所有可到達(dá)的相鄰柵格只有正前方的3個(gè)柵格。如圖4所示，圖中A為車輛自身位置所 在父柵格，則其搜索范圍為B、C、D三個(gè)柵格。
[0058] 2.通過對(duì)危險(xiǎn)估計(jì)值的計(jì)算，找出這三個(gè)柵格中危險(xiǎn)估計(jì)值最小的柵格，判斷其 危險(xiǎn)估計(jì)值是否超過閾值。閾值是由車體大小和操作經(jīng)驗(yàn)人為設(shè)定的，估計(jì)值超過閾值的 柵格為不可行駛柵格，反之為可行駛柵格。若該柵格危險(xiǎn)估計(jì)值小于閾值，則將其作為當(dāng)前 父柵格的子?xùn)鸥?，并作為下一次搜索的父柵格，放入已搜索列表?br>[0059] 3.開始下一次搜索，通過與已搜列表比對(duì)，判斷本次父柵格搜索范圍內(nèi)所有柵格 是否有未搜柵格。
[0060] 4.若存在未搜柵格，并存在危險(xiǎn)估計(jì)值小于閾值的柵格，則找出這幾個(gè)未搜柵格 中危險(xiǎn)估計(jì)值最小的柵格，作為當(dāng)前父柵格的子?xùn)鸥?，并作為下一次搜索的父柵格，放入?搜索列表。然后回到步驟3。
[0061] 5.若不存在未搜柵格，或所有未搜柵格的危險(xiǎn)估計(jì)值均大于閾值，則找到本次父 柵格在上一次搜索時(shí)所對(duì)應(yīng)的父柵格，取消它們之間的父子關(guān)系，并把上一次搜索所對(duì)應(yīng) 的父柵格作為下一次搜索的父柵格。然后回到步驟3。
[0062] 6.在執(zhí)行（3)、（4)、（5)的循環(huán)搜索過程中，如果搜索到目的區(qū)域的柵格，則搜索 結(jié)束，生成從車輛自身柵格到目的區(qū)域柵格的父子關(guān)系鏈；如果在未到達(dá)目的區(qū)域柵格之 前，搜索已經(jīng)回退到車輛自身柵格，并且其搜索范圍內(nèi)不存在未搜柵格，則搜索結(jié)束，生成 歷史父子關(guān)系鏈中第一次到達(dá)距離目標(biāo)區(qū)域最近的一條父子關(guān)系鏈。
[0063] 本發(fā)明提出了一種智能車SRA路徑搜索算法，在智能車路徑規(guī)劃的過程中，由于 環(huán)境感知所得信息有限，往往不知道以什么標(biāo)準(zhǔn)來搜索最佳行駛路徑。SRA路徑搜索算法提 供一種依據(jù)道路危險(xiǎn)評(píng)估搜索最優(yōu)路徑的方法，在比較和實(shí)驗(yàn)多種方法的基礎(chǔ)上，自主提 出危險(xiǎn)值計(jì)算方法和搜索方法，并能夠依據(jù)所計(jì)算的道路危險(xiǎn)值實(shí)時(shí)搜索可行路徑。
[0064] 在智能車路徑規(guī)劃的過程中，需要從車輛當(dāng)前點(diǎn)規(guī)劃最佳路線到達(dá)目標(biāo)點(diǎn)。SRA搜 索算法是發(fā)展人工智能而產(chǎn)生的路徑規(guī)劃十分有效的啟發(fā)式搜索方法，算法采用深度優(yōu)先 遍歷的方法，到達(dá)目標(biāo)節(jié)點(diǎn)即停止搜索，若無法到達(dá)目標(biāo)結(jié)點(diǎn)，算法可以給出最接近于目標(biāo) 點(diǎn)的路徑。它首先利用二維柵格圖對(duì)車輛行駛環(huán)境進(jìn)行建模；然后采用危險(xiǎn)