本發(fā)明涉及路徑預測裝置，該路徑預測裝置使用由雷達、GPS等傳感器構成的觀測裝置來觀測飛機、船舶、車輛等移動的對象物體的位置，根據其觀測值預測用于防止對象物體與位于其周圍的多個物體的碰撞的路徑。
背景技術：
：近年來，在車輛的駕駛輔助系統(tǒng)、航空管制等各種領域中要求預測避免移動體之間的碰撞的安全路徑的技術。例如開發(fā)了如下的技術：在車輛的駕駛輔助系統(tǒng)中，通過裝載于本車輛的毫米波雷達、激光雷達等的傳感器獲得位于本車輛周圍的車輛、停止物等障礙物的位置，在根據本車輛與障礙物的相對距離、相對速度判斷碰撞危險度之后，控制本車輛，防止碰撞。此外，作為更高端的技術，也開發(fā)了如下的自動駕駛技術：利用所述傳感器識別周圍環(huán)境，在沒有駕駛員的操作的情況下自動進行方向盤操作、制動器等的操作，到達目的地。作為這種關于路徑預測的現有技術，例如在專利文獻1所示的裝置中，預先生成多個車輛預測軌道，基于生成的預測軌道計算時間空間內的預測路徑的存在概率。此外，例如在專利文獻2所示的駕駛輔助裝置中，計算本車輛相對于其他車輛的潛在風險映射圖，使得基于風險的油門、制動器等的控制介入。另一方面，在航空管制中，相對于以往的基于三維位置導航方法，研究了在導航方法中采納包括三維位置和時間的四維軌道(4DT：4DimensionalTrajectory)。該4DT相當于預測路徑，能夠通過管理4DT來預測航空交通量、空域容量，從而預計會提高航運的安全性。作為這樣的路徑預測的技術，例如，在專利文獻3中，基于目標的當前速度和機頭方向，假定為勻速直線前進而計算將來位置。此外，例如，在專利文獻4所示的系統(tǒng)中，使用基于A*算法的最佳路徑探索方法作為預測將來位置的方法。在該算法中，確定包括禁止進入區(qū)域(障礙物)的、將候選路徑細分為網狀的移動空間中的從出發(fā)至目標(或者通過點)止的節(jié)點?，F有技術文獻專利文獻專利文獻1：日本特開2007-233646號公報專利文獻2：日本特開2012-148747號公報專利文獻3：日本特開平11-120500號公報專利文獻4：日本特開2009-251729號公報技術實現要素：發(fā)明要解決的課題但是，在上述專利文獻1所述的現有裝置中，為了計算存在概率，必須生成多個預測軌道，存在運算負荷增大的問題。此外，在專利文獻2所示的裝置中，存在這樣的問題：風險的計算方法是不明確、且依賴參數的計算方法，不能準確地評估風險。并且，在專利文獻3所述的現有技術中，存在這樣的問題：在目標為了回避雷云等障礙物而變更路線的情況下，將來位置的估計精度惡化。此外，在專利文獻4所述的使用A*算法的系統(tǒng)中，存在這樣的問題：由于路徑由陣點決定，未考慮移動體的運動。為了得到自然的路徑，必須細分陣點的間隔，存在犧牲處理時間的問題。本發(fā)明是為了解決該問題而完成的，目的在于，提供能夠減輕計算碰撞危險度低的預測路徑時的運算負荷的路徑預測裝置。用于解決課題的手段本發(fā)明的路徑預測裝置具備：傳感器部，其觀測對象物體與位于該對象物體的周圍的周圍物體的位置；跟蹤處理部，其根據所述對象物體與所述周圍物體的位置進行跟蹤處理，計算對象物體與周圍物體的估計位置和估計速度；碰撞物體檢測部，其根據估計位置和估計速度，檢測可能與對象物體碰撞的周圍物體作為目標物體；路徑預測部，其基于碰撞回避模型，預測對象物體相對于目標物體的路徑；碰撞危險度評估部，其與碰撞回避模型對應地計算對象物體與目標物體的碰撞危險度；碰撞判定部，其根據碰撞危險度判定有無碰撞，在判定為碰撞的情況下，向路徑預測部反饋碰撞回避模型修正值；以及回避路徑選擇部，其從碰撞判定部判定為無碰撞的多個碰撞回避模型中選擇任意碰撞回避模型，選擇該碰撞回避模型的路徑作為避免物體之間的碰撞的路徑，路徑預測部使用碰撞回避模型修正值進行新的路徑預測。發(fā)明的效果本發(fā)明的路徑預測裝置基于碰撞回避模型，預測對象物體相對目標物體的路徑，與碰撞回避模型對應地計算對象物體與目標物體的碰撞危險度，根據該碰撞危險度判定有無碰撞，選擇從判定為無碰撞的多個碰撞回避模型中選擇的任意一個碰撞回避模型的路徑，作為避免物體之間碰撞的路徑。由此，能夠減輕計算碰撞危險度低的預測路徑時的運算負荷。附圖說明圖1是示出本發(fā)明的實施方式1的路徑預測裝置的結構圖。圖2是本發(fā)明的實施方式1的路徑預測裝置的碰撞危險度的說明圖。圖3是本發(fā)明的實施方式1的路徑預測裝置的碰撞危險度高的情況下的說明圖。圖4是本發(fā)明的實施方式1的路徑預測裝置的碰撞危險度低的情況下的說明圖。圖5是示出本發(fā)明的實施方式1的路徑預測裝置中的轉向回避時的碰撞危險度計算對象的說明圖。圖6是示出本發(fā)明的實施方式1的路徑預測裝置中的從路徑預測部至碰撞判定部的處理部的動作的流程圖。具體實施方式以下，為了更詳細地說明本發(fā)明，基于附圖對用于實施本發(fā)明的方式進行說明。實施方式1.圖1是示出本實施方式的路徑預測裝置的結構圖。如圖所示，本實施方式的路徑預測裝置具有傳感器部1、跟蹤處理部2、碰撞物體檢測部3、路徑預測部4、碰撞危險度評估部5、碰撞判定部6以及碰撞回避路徑選擇部7。傳感器部1是觀測對象物體與位于該對象物體的周圍的周圍物體的相對位置的處理部，使用毫米波雷達、激光雷達、光學相機、紅外相機等的傳感器、或者接收周圍車輛或歩行者的GPS位置的通信裝置等而構成。跟蹤處理部2是如下的處理部：根據由傳感器部1觀測到的相對位置進行跟蹤處理，計算對象物體與周圍物體的估計位置、估計速度、估計位置的估計誤差、估計速度的估計誤差。碰撞物體檢測部3是如下的處理部：根據估計位置和所述估計速度，檢測可能與對象物體碰撞的周圍物體作為目標物體。路徑預測部4是如下的處理部：分別在M個碰撞回避模型中計算對象物體相對于目標物體的提前N步的預測位置(此處，M和N是任意的整數)。碰撞危險度評估部5是如下的處理部：根據由跟蹤處理部2計算出的估計位置和估計誤差，按照每個碰撞回避模型計算碰撞危險度。碰撞判定部6是如下的處理部：根據由碰撞危險度評估部5計算出的碰撞危險度判定有無碰撞，在判定為碰撞的情況下向路徑預測部4反饋碰撞回避模型修正值，在判定為無碰撞的情況下，將該碰撞回避模型輸出至碰撞回避路徑選擇部7。碰撞回避路徑選擇部7是如下的處理部：基于規(guī)定的判定基準，針對從碰撞判定部6輸出的碰撞回避模型，選擇任意的碰撞回避模型，確定碰撞回避用的預測路徑。另外，使用計算機構成路徑預測裝置，通過用CPU執(zhí)行與各個處理部的功能對應的軟件來實現跟蹤處理部2至碰撞回避路徑選擇部7?；蛘?，也可以用專用的硬件構成上述傳感器部1至碰撞回避路徑選擇部7中的至少一個。接著，對實施方式1的路徑預測裝置的動作進行說明。傳感器部1測定周圍車輛或歩行者的位置和速度。跟蹤處理部2根據位置和速度，借助跟蹤處理來計算位置估計值、速度估計值、位置及速度的估計誤差協方差矩陣。碰撞物體檢測部3檢測可能與本車輛碰撞的周圍車輛。例如可以基于TTC(TimeToCollision)的思想進行檢測。TTC由式(1)定義，如果TTC為閾值以下，則檢測為可能碰撞的車輛。并且，將檢測出的周圍車輛i定義為目標車輛。TTC=(y^s,k(i)-yk(ego))(y·^s,k(i)-y·k(ego))---(1)]]>采樣時刻k的周圍車輛i相對于縱方向的估計位置采樣時刻k的周圍車輛i相對于縱方向的估計速度來樣時刻k的本車輛相對于縱方向的位置采樣時刻k的本車輛相對于縱方向的速度此外，作為碰撞物體檢測部3的其他方法，在本車輛周圍先設定規(guī)定區(qū)域，檢測1至N步后的預測位置進入該規(guī)定區(qū)域的車輛，可以視作目標車輛。此處，如式(2)那樣計算提前N步的N個預測位置。x^p,k+N(i)=ΦNx^s,k(i)---(2)]]>x^s,k(i)=x^s,k(i)y^s,k(i)x·^s,k(i)y·^s,k(i)T---(3)]]>x^p,k+N(i)=x^p,k+N(i)y^p,k+N(i)x·^p,k+N(i)y·^p,k+N(i)T---(4)]]>ΦN=I2×2N·ΔT·I2×20·I2×2I2×2---(5)]]>采樣時刻k的周圍車輛i的估計狀態(tài)矢量采樣時刻k的周圍車輛i的提前N步預測狀態(tài)矢量采樣時刻k的周圍車輛i相對于橫向的估計位置采樣時刻k的周圍車輛i相對于橫向的估計速度采樣時刻k的周圍車輛i相對于橫向的提前N步預測位置采樣時刻k的周圍車輛i相對于橫向的提前N步預測速使采樣時刻k的周圍車輛i相對于縱向的提前N步預測位置采樣時刻k的周圍車輛i相對于縱向的提前N步預測速度ΔT:步寬IL×L：L行L列的單位矩陣路徑預測部4針對由碰撞物體檢測部3檢測出的目標車輛tgti，分別在M個碰撞回避模型中計算提前N步的預測位置。此處，作為碰撞回避模型，例如可以定義制動回避模型、左轉向回避模型、右避免轉向模型。假設制動回避模型為在維持車道的狀態(tài)下通過制動來避免碰撞的模型，左/右轉向回避模型為通過輸入轉向量而向左/右變更車道來避免碰撞的模型。此外，對于這些模型，假設制動量或者轉向量設定成不超過規(guī)定的界限值。特別是在后述的碰撞判定部6中，在判斷為不可能避免碰撞的情況下，制動量或者轉向量的修正值被反饋到路徑預測部4，實施不超過規(guī)定的界限值的動作。此外，在路徑預測部4中需要設定碰撞回避模型的制動量或者轉向量的初始值?？梢园凑战涷炘O定制動及轉向回避時輸入的值作為初始值。此外，也可以利用學習算法，按照每個駕駛員設定不會感到不快的程度的制動量、轉向量。并且，在路徑預測部4中，不限于上述的模型，除此以外也可以增加與各種場景對應的碰撞回避模型。此外，也可以根據地圖數據、GPS位置等，在車道數和本車輛行駛的車道為已知的情況下，丟棄不需要的碰撞回避模型，從而削減碰撞回避模型數。例如，在行車道數為2且本車輛行駛在左車道的情況下，無法進行左轉向回避，因此，丟棄左轉向回避模型，對剩下的碰撞回避模型進行計算。另外，例如，在途中車道數從2車道增加至3車道的地點，能夠容易增加用于將車道變更為增加車道的碰撞回避模型，容易進行利用地圖數據的碰撞回避模型的增加、削減。如果在地圖數據以外還利用激光雷達、相機等，則能夠識別外部環(huán)境，可以用來代替地圖。對基于碰撞回避模型的預測位置計算方法進行說明?；谥苿踊乇苣Ｐ偷闹苿佑眉铀俣萢b如式(6)所示那樣計算預測路徑(提前N步的預測位置)。x^p,k+N(ego)=F(ab)=ΦNxk(ego)+0-12(N·ΔT)2·ab0-N·ΔT·ab---(6)]]>xk(ego)=xk(ego)yk(ego)x·k(ego)y·k(ego)T---(7)]]>x^p,k+N(ego)=x^p,k+N(ego)y^p,k+N(ego)x·^p,k+N(ego)y·^p,k+N(ego)T---(8)]]>ab:制動用加速度對左/右轉向回避模型也能同樣地計算。此處，車輛相對于轉向的預測位置根據車輛重量、車體重心位置、偏航慣性力矩等車輛參數而不同，因此，在車輛參數為已知的情況下，預先設定，計算預測位置。此外，在車輛參數為未知的情況下，可以使用通過公知的學習算法等估計出的參數。碰撞危險度評估部5根據由跟蹤處理部2輸出的位置的估計誤差協方差矩陣和位置及速度估計值來計算碰撞危險度。如式(9)所示，計算利用估計誤差協方差矩陣對采樣時刻k的本車輛的N步后的預測位置與目標車輛tgti的n(n＝1、…、N)步驟后的預測位置之差進行歸一化而得到的值即馬氏距離的平方值εk+n。ϵk+n=Δx^k+nTPp,k+n(tgti)-1Δx^k+n---(9)]]>Δx^k+n=x^p,k+n(tgti)-x^p,k+n(ego)y^p,k+n(tgti)-y^p,k+n(ego)T---(10)]]>Pp,k+n(tgti)=ΦnPs,k(tgti)ΦnT---(11)]]>采樣時刻k的周圍車輛tgti的平滑誤差協方差矩陣采樣時刻k的周圍車輛tgti的提前n步預測誤差協方差矩陣此處，可知橫位置x、縱位置y這兩個變量遵循正態(tài)分布的情況下，馬氏距離的平方值εk+n的概率分布為自由度2的卡方分布。利用該性質，碰撞危險度評估部5如圖2所示那樣將碰撞危險度定義為卡方分布的上側概率(圖2的斜線部分100)。為了直觀地理解碰撞危險度，對本車輛(目標2)與目標車輛(目標1)的相對位置和碰撞危險度的對應關系進行說明。例如，如圖3所示，在目標1與目標2碰撞的場景(目標1與目標2的位置相同)中，圖3的斜線部分101接近1。即，計算出碰撞危險度為1(或者100％)。另一方面，如圖4所示，在目標1與目標2的距離無限遠的場景中，圖4的斜線部分接近0。即，計算出碰撞危險度為0(0％)。因此，可知卡方分布的上側概率直觀地為與碰撞危險度相當的值。并且，能夠預先計算馬氏距離的平方值εk+n與卡方分布的上側概率的對應表，因此如果先保持對應表，則無需計算就能夠讀取與馬氏距離的平方值相應的碰撞危險度。此外，以上記述了計算本車輛與周圍車輛的相對位置的碰撞危險度的方法，在此基礎上說明使用目標1與目標2的絕對位置的情況下的碰撞危險度計算方法。例如，面向車輛駕駛輔助系統(tǒng)，可考慮在車車間通信等中獲得本車輛和周圍車輛的GPS位置這樣的絕對值的情況。此外，在航空管制的領域中，對于多架飛機，可考慮獲得基于雷達的觀測位置、GPS位置并用于飛機管制的情況。在該情況下，由于各目標位置中含有位置誤差，因此，使用下式(12)、(13)計算碰撞危險度的評估值，讀取與評估值對應的碰撞危險度。ϵk+n=Δx^k+nT(Pp,k+n(1)+Pp,k+n(2))-1Δx^k+n---(12)]]>Δx^k+n=x^p,k+n(1)-x^p,k+n(2)y^p,k+n(1)-y^p,k+n(2)T---(13)]]>采樣時刻k的目標tgti的平滑誤差協方差矩陣采樣時刻k的目標tgti的提前n步預測誤差協方差矩陣此處，為了根據目標的誤差分布的重疊計算碰撞危險度，必須進行基于誤差分布的復雜的數值計算，但在本發(fā)明中不需要復雜的數值計算就能夠計算碰撞危險度。此外，可以用其他的概率分布(例如正態(tài)分布)對馬氏距離的平方值εk+n的概率分布進行近似。碰撞判定部6根據由碰撞危險度評估部5計算出的碰撞危險度判定碰撞，在碰撞的情況下向路徑預測部4輸出預測路徑修正值，對預測路徑進行再修正。在不碰撞的情況下向碰撞回避路徑選擇部7輸出預測路徑和碰撞危險度。在碰撞判定中，在相對于概率變量εk+n(n＝1，…，N)的最小值為閾值εth以下的情況下，視作碰撞。所述的閾值εth使用自由度m的卡方分布表，如在碰撞危險度評估部5中敘述的那樣，如果預先設定與碰撞危險度對應的碰撞閾值εth，則能容易地判定是否碰撞。此外，在圖5所示的轉向回避的的情況下，可考慮其他的周圍車輛正在在本車輛200通過轉向回避而變更車道后位置行駛的情形，因此，對于變更后的車道，計算最近的前方車輛201與最近的后方車輛202的碰撞危險度。并且，從目標車輛203、最近的前方車輛201以及最近的后方車輛202的碰撞危險度中選擇最大值，實施碰撞判定。另外，由圖中的虛線包圍的區(qū)域表示預測誤差。并且，碰撞判定部6將預測路徑的修正值反饋至路徑預測部4，由此，路徑預測部4和碰撞危險度評估部5再次計算預測路徑和碰撞危險度。重復這些步驟，直至超過閾值εth為止。圖6示出這些路徑預測部4～碰撞判定部6的處理流程。即，按照每個目標車輛，對所有的模型進行N步的路徑預測(步驟ST1)、碰撞危險度的評估(步驟ST2)以及碰撞判定(步驟ST3、ST4)。此外，在步驟ST4中為碰撞閾值以下的情況下，執(zhí)行模型循環(huán)直至超過碰撞閾值為止。另外，在模型循環(huán)達到預先設定的規(guī)定次數的情況下，可以停止該碰撞回避模型的計算。碰撞回避路徑選擇部7根據基于路徑預測部4～碰撞判定部6己計算的各碰撞回避模型的預測路徑，確定碰撞回避用的預測路徑。對于基于各碰撞回避模型的N個預測位置，比較碰撞危險度的最大值，將具有最小值的碰撞回避模型看作最安全的回避路徑，作為碰撞回避用預測路徑輸出。此外，也可以選擇包含最小值的設定值以下的碰撞回避模型。此外，可以比較對所述N個預測位置賦予的N個碰撞危險度的總值，選擇最小的路徑。另外，在此也可以選擇包含最小值的設定值以下的路徑。此外，在制動量或者轉向量超過規(guī)定的界限值的情況下，可以丟棄。此外，也可以根據駕駛員的需要，選擇制動量的總值最小的路徑、或者轉向回避量的總值最小的路徑。由此，在實施方式1中，通過限定為在現實中假定的碰撞回避模型，不必計算以往那樣的無數個路徑，能夠減輕運算負荷。如以上說明的那樣，根據實施方式1的路徑預測裝置，具備：傳感器部，其觀測對象物體與位于該對象物體的周圍的周圍物體的位置；跟蹤處理部，其根據對象物體與周圍物體的位置進行跟蹤處理，計算對象物體與周圍物體的估計位置和估計速度；碰撞物體檢測部，其根據估計位置和估計速度，檢測可能與對象物體碰撞的周圍物體作為目標物體；路徑預測部，其基于碰撞回避模型，預測對象物體相對于目標物體的路徑；碰撞危險度評估部，其與碰撞回避模型對應地計算對象物體與目標物體的碰撞危險度；碰撞判定部，其根據碰撞危險度判定有無碰撞，在判定為碰撞的情況下，向路徑預測部反饋碰撞回避模型修正值；以及回避路徑選擇部，其從由碰撞判定部判定為無碰撞的多個碰撞回避模型中，選擇任意碰撞回避模型，選擇碰撞回避模型的路徑作為避免物體之間的碰撞的路徑，路徑預測部使用碰撞回避模型修正值，進行新的路徑預測，因此，能夠減輕計算碰撞危險度低的預測路徑時的運算負荷。此外，根據實施方式1的路徑預測裝置，跟蹤處理部計算估計位置的估計誤差和估計速度的估計誤差，碰撞危險度評估部根據用估計誤差對估計位置歸一化而得到的值來計算碰撞危險度，因此，不必進行復雜的數值計算就能計算碰撞危險度。此外，根據實施方式1的路徑預測裝置，碰撞危險度評估部從示出用估計誤差對估計位置進行歸一化而得到的值與碰撞危險度的對應關系的對應表，獲得碰撞危險度，因此不必進行數值計算就能容易得到碰撞危險度。此外，根據實施方式1的路徑預測裝置，回避路徑選擇部對于碰撞回避模型的碰撞危險度的時間方向累計值，選擇累計值在設定值以下的碰撞回避模型，因此，不必計算無數個路徑，能減輕運算負荷。此外，根據實施方式1的路徑預測裝置，回避路徑選擇部以碰撞回避模型的碰撞危險度在時間方向上的最大值為代表值，選擇代表值在設定值以下的碰撞回避模型，因此，不必計算無數個路徑，能減輕運算負荷。此外，根據實施方式1的路徑預測裝置，碰撞判定部通過將碰撞危險度與設定的閾值進行比較來進行碰撞判定，因此，能夠容易判定是否碰撞。另外，本發(fā)明能夠在其發(fā)明范圍內進行實施方式的任意構成要素的變形，或者可以省略實施方式的任意構成要素。產業(yè)上的可利用性如上所述，本發(fā)明的路徑預測裝置涉及這樣的路徑預測裝置：該路徑預測裝置使用由雷達、GPS等傳感器構成的觀測裝置來觀測飛機、船舶、車輛等移動體的位置，根據其觀測值預測用于防止移動體與位于其周圍的多個移動體的碰撞的路徑，適于用在車輛駕駛輔助系統(tǒng)、航空管制等中。標號說明1：傳感器部；2：跟蹤處理部；3：碰撞物體檢測部；4：路徑預測部；5：碰撞危險度評估部；6：碰撞判定部；7：碰撞回避路徑選擇部。當前第1頁1 2 3