TWI781829B

TWI781829B - 融合式車輛定位方法與系統

Info

Publication number: TWI781829B
Application number: TW110143390A
Authority: TW
Inventors: 張統凱; 徐錦衍; 林祐賢; 楊濟帆; 余建宏; 陳奕兆
Original assignee: 財團法人車輛研究測試中心
Priority date: 2021-11-22
Filing date: 2021-11-22
Publication date: 2022-10-21
Also published as: TW202321081A

Abstract

一種融合式車輛定位方法與系統，該方法於一處理器執行，該處理器電性連接一自駕車控制器與複數電子系統，該融合式車輛定位方法將從該複數電子系統接收到的一第一即時座標、一第二即時座標、一車道辨識即時資料、複數車輛動態參數與複數車輛資訊配合權重值進行延遲修正、進行融合處理以產生一融合定位座標，以及進行信心度資訊的判斷，再輸出延遲修正後的該第一即時座標、該第二即時座標與該車道辨識即時資料、該融合定位座標與該信心度資訊至該自駕車控制器，供該自駕車控制器據以實施自駕車的運作。

Description

融合式車輛定位方法與系統

本發明是有關一種車輛定位方法與系統，特別是指融合式車輛定位方法與系統。

自駕車(self-driving vehicle)是未來交通運輸的主要工具之一，且自駕車定位技術亦為熱門的研發目標，舉例而言，為實施國際汽車工程學會(Society of Automotive Engineers, SAE)所定義的第三級(Level 3)以上自動駕駛控制，現行大多採用同步定位與地圖構建(Simultaneous Localization And Mapping, SLAM)系統，然而，面對複雜多變的道路環境，SLAM系統恐有不足，理由在於：

1、透過SLAM系統提供車輛定位資訊，其定位更新頻率小於10Hz，導致定位更新速度過慢問題與定位延遲的問題。

2、道路環境可能存在路樹、建築物等物件一致性過高或偵測遮蔽導致特徵不足或環境特徵突然變動等議題，造成SLAM比對特徵資訊落差之現象，導致SLAM系統的定位信號發生短暫失效與飄動問題，無法即時定位。

3、如前所述，SLAM系統的定位信號可能發生短暫失效與飄動問題，也就是說，其定位信號可能是錯誤的定位訊號，將會造成自駕車行駛於錯誤路徑上，進而發生車禍或緊急煞車狀況。

有鑒於此，本發明的主要目的是提供一種融合式車輛定位方法與系統，以期克服先前技術所述定位更新速度過慢、定位延遲以及無法即時定位的問題。

本發明融合式車輛定位方法於一處理器執行，該處理器電性連接一自駕車控制器與複數電子系統，該融合式車輛定位方法包含：分別從該複數電子系統接收一第一即時座標、一第二即時座標、一車道辨識即時資料、複數車輛動態參數與複數車輛資訊；隨著時間進行，根據先後接收到的該複數車輛動態參數與該複數車輛資訊計算並儲存複數位移量；根據該複數位移量對該第一即時座標、該第二即時座標與該車道辨識即時資料進行延遲修正；判斷該第一即時座標與該第二即時座標的等級；根據該複數位移量與具有較佳等級的該第一即時座標或該第二即時座標計算出一動態預估座標；根據具有較佳等級的該第一即時座標或該第二即時座標搭配一地圖資訊產生一橫向位置資料，並判斷該橫向位置資料與該車道辨識即時資料的誤差是否在容許值內，若是，根據該車道辨識即時資料或該橫向位置資料與具有較佳等級的該第一即時座標或該第二即時座標計算出一橫向輔助座標；根據該第一即時座標與其權重值、該第二即時座標與其權重值、該動態預估座標與其權重值以及該橫向輔助座標與其權重值產生一融合定位座標；分別判斷該第一即時座標、該第二即時座標、該動態預估座標與該橫向輔助座標的信心度資訊；以及輸出延遲修正後的該第一即時座標、該第二即時座標與該車道辨識即時資料、該融合定位座標與該信心度資訊至該自駕車控制器，供該自駕車控制器據以實施自駕車的運作。

本發明融合式車輛定位系統包含：一自駕車控制器；複數電子系統，分別輸出一第一即時座標、一第二即時座標、一車道辨識即時資料、複數車輛動態參數與複數車輛資訊；以及一處理器，電性連接該自駕車控制器與該複數電子系統，該處理器分別從該複數電子系統接收該第一即時座標、該第二即時座標、該車道辨識即時資料、該複數車輛動態參數與該複數車輛資訊，並根據先後接收到的該複數車輛動態參數與該複數車輛資訊計算並儲存複數位移量；該處理器根據該複數位移量對該第一即時座標、該第二即時座標與該車道辨識即時資料進行延遲修正；該處理器判斷該第一即時座標與該第二即時座標的等級、根據該複數位移量與具有較佳等級的該第一即時座標或該第二即時座標計算出一動態預估座標、根據該第一即時座標或該第二即時座標搭配一地圖資訊產生一橫向位置資料，並判斷該橫向位置資料與該車道辨識即時資料的誤差是否在容許值內，若是，根據該車道辨識即時資料或該橫向位置資料與具有較佳等級的該第一即時座標或該第二即時座標計算出一橫向輔助座標；該處理器根據該第一即時座標與其權重值、該第二即時座標與其權重值、該動態預估座標與其權重值以及該橫向輔助座標與其權重值產生一融合定位座標，並分別判斷該第一即時座標、該第二即時座標、該動態預估座標與該橫向輔助座標的信心度資訊；該處理器輸出延遲修正後的該第一即時座標、該第二即時座標與該車道辨識即時資料、該融合定位座標與該信心度資訊至該自駕車控制器，供該自駕車控制器據以實施自駕車的運作。

根據本發明的融合式車輛定位方法與系統，達成的功效包含：

1、本發明透過車輛動態參數的運用，車輛動態參數是由一慣性量測單元(Inertial measurement unit, IMU)所產生的資料，使本發明可以約50Hz的頻率更新位移量，故將所述位移量疊加、整合至該第一即時座標或該第二即時座標，即可提升自駕車定位更新速度，及補償定位延遲。

2、縱使定位信號發生短暫失效與飄動，本發明可透過所述位移量進行疊加，故仍可獲得該融合定位座標以提供給該自駕車控制器，達到即時定位的效果。

3、藉由提供該第一即時座標、該第二即時座標、該動態預估座標與該橫向輔助座標的信心度資訊至該自駕車控制器，可提升控制系統的強健性，當該處理器判斷出信心度低時，可提前進行安全防護行駛的動作。

本發明為融合式車輛定位方法與系統，請參考圖1，本發明系統可包含一自駕車控制器21、複數電子系統22與一處理器10，本發明方法於該處理器10執行，該自駕車控制器21、該複數電子系統22與該處理器10供設置在一自駕車20，該處理器10與該自駕車控制器21可為積體電路(IC)晶片，該處理器10電性連接該自駕車控制器21與該複數電子系統22以進行資料傳輸，其中，該處理器10、該自駕車控制器21與該複數電子系統22可透過一資料匯流排23彼此連線，該資料匯流排23可為(但不限於)控制器區域網路匯流排(Controller Area Network Bus, CAN Bus)，該自駕車控制器21亦透過該資料匯流排23連線該自駕車20的一轉向系統24(例如方向盤傳動機構)、一動力系統25(例如引擎或馬達)與制動系統(圖未示)…等，以實施諸如車速控制、煞車控制與航向控制等自動駕駛操作。本發明是由該處理器10從該複數電子系統22接收定位資料與感測資料，並據以融合而產生該自駕車20的定位資訊和該定位資訊的信心度，請配合參考圖2A、2B與圖3所示的流程圖，本發明的實施例詳述如後。

步驟S01：資料收集，該處理器10分別從該複數電子系統22接收一第一即時座標S1、一第二即時座標S2、一車道辨識即時資料S3、複數車輛動態參數S4與複數車輛資訊S5。舉例而言，請參考圖4，該複數電子系統22包含一第一定位系統221、一第二定位系統222、一車道辨識系統223、一慣性量測系統224和一車輛資訊系統225。該第一定位系統221輸出該第一即時座標S1，該第二定位系統222輸出該第二即時座標S2，該車道辨識系統223輸出該車道辨識即時資料S3，該慣性量測系統224輸出該複數車輛動態參數S4，且該慣性量測系統224又可稱作慣性量測單元(Inertial measurement unit, IMU)，該車輛資訊系統225輸出該複數車輛資訊S5。

本發明的實施例中，該第一定位系統221可為同步定位與地圖構建(Simultaneous Localization And Mapping, SLAM)系統，故該第一即時座標S1為一SLAM絕對座標，該SLAM絕對座標包含經度與緯度等位置資訊，該第一定位系統221還同時輸出對應於該SLAM絕對座標的一SLAM附屬資訊，該SLAM附屬資訊包含一SLAM運算時間、一分數(score)與一疊代次數，該SLAM運算時間包含該第一定位系統221從其感測器擷取訊號、訊號傳輸、訊號處理至輸出該SLAM絕對座標的總時間。該第二定位系統222可為即時動態全球定位(Real-Time Kinematic - Global Positioning System, RTK-GPS)系統，故該第二即時座標S2為一RTK-GPS絕對座標，該RTK-GPS絕對座標包含經度和緯度等位置資訊，該第二定位系統222還同時輸出對應於該RTK-GPS絕對座標的一RTK-GPS附屬資訊，該RTK-GPS附屬資訊包含一RTK-GPS運算時間、一RTK狀態與一慣性導航系統(Inertial Navigation System, INS)狀態，該RTK-GPS運算時間包含該第二定位系統222從其感測器擷取訊號、訊號傳輸、訊號處理至輸出該RTK-GPS絕對座標的總時間。請配合參考圖7，該車道辨識系統223所輸出的該車道辨識即時資料S3包含一左間隔距離S3L、一右間隔距離S3R與一車道辨識時間，其中，該車道辨識系統223可透過影像辨識手段偵測出該自駕車20所在車道的一左車道線31與一右車道線32，該左間隔距離S3L是該自駕車20至該左車道線31的最短距離，該右間隔距離S3R是該自駕車20至該右車道線32的最短距離，該車道辨識時間包含該車道辨識系統223從其影像感測器擷取訊號、訊號傳輸、訊號處理至輸出該車道辨識即時資料S3的總時間。該慣性量測系統224所輸出的該複數車輛動態參數S4可包含航向角φ、側滑角β、偏擺率ω、縱向加速度Ax與側向加速度g，其中，該縱向加速度Ax是指該自駕車20往前行進的加速度。該車輛資訊系統225可包含一轉向角度偵測器(圖未示)與一輪速偵測器(圖未示)，該轉向角度偵測器用以偵測該自駕車20的方向盤轉角，該輪速偵測器用以偵測該自駕車20的車速，故該車輛資訊系統225所輸出的該複數車輛資訊S5包含該方向盤轉角

與該車速V。

在本發明的實施例中，如圖3所示，該處理器10執行一車輛動態參數判別(步驟S01a)，該自駕車20從靜止狀態啟動時，該處理器10先將接收到的該複數車輛動態參數S4(包含：車速V、偏擺率ω、航向角φ、側滑角β)先做基準的偏差校正歸零，該自駕車20行駛中也會使用卡爾曼濾波器(Kalman filter)修正。另一方面，該處理器10計算一估計偏擺率

，並判斷該估計偏擺率

與該偏擺率ω之間的差異是否大於一偏擺率門檻值，該偏擺率門檻值為預設值。當該估計偏擺率

與該偏擺率ω之間的差異不大於(即：小於或等於)該偏擺率門檻值，則採用該偏擺率ω進行後續運算。其中，該估計偏擺率

表示如下：

上式中，

為該自駕車20的前輪角度；V為該車速；L代表該自駕車20的軸距，為預設值；

代表轉向不足係數，為預設值；g為該側向加速度；其中，該前輪角度

表示如下：

上式中，

代表該自駕車20的方向盤角度與前輪角度的比值，為預設值；

為該方向盤轉角。

綜上，如前所述的該第一定位系統221、該第二定位系統222、該車道辨識系統223、該慣性量測系統224和該車輛資訊系統225可採用習知系統，故其輸出該第一即時座標S1、該第二即時座標S2、該車道辨識即時資料S3、該複數車輛動態參數S4與該複數車輛資訊S5的方式是所屬技術領域的通常知識，本發明係根據該複數電子系統22所輸出的資料進行進一步演算，詳述如後。

需說明的是，該複數車輛動態參數S4與該複數車輛資訊S5是時變(time varing)的，本文提到的該複數車輛動態參數S4與該複數車輛資訊S5亦可以時間"t"來表示，代表某一時刻"t"當下取得的參數或資訊，例如將該航向角表示為φ(t)、將該側滑角表示為β(t)、將該偏擺率表示為ω(t)以及將該車速表示為V(t)。另一方面，該處理器10將該第一即時座標S1和該第二即時座標S2進行座標轉換，請配合參考圖5，從經緯度座標系轉換為一自駕車的座標系，該自駕車座標系包含一縱向軸x與一橫向軸y，該縱向軸x為該自駕車20前行的軸向(即：航向)，該橫向軸y垂直於該縱向軸x，是以，經過座標轉換後，該第一即時座標S1和該第二即時座標S2各自包含一縱向座標值與一橫向座標值。

步驟S02：隨著時間進行，該處理器10根據先後接收到的該複數車輛動態參數S4與該複數車輛資訊S5計算並儲存複數位移量。在本步驟中，如前所述，該處理器10從該慣性量測系統224更新該複數車輛動態參數S4的頻率是較高的，而該處理器10可隨時向該車輛資訊系統225請求讀取該複數車輛資訊S5。是以，本發明根據先後兩時間點接收到的該複數車輛動態參數S4和該複數車輛資訊S5計算該自駕車20在該先後兩時間之間的該位移量，隨著時間進行，該處理器10能依序儲存該自駕車20移動時的該複數位移量。為方便說明，各該位移量包含一縱向分量Shift_x(t)與一橫向分量Shift_y(t)，該縱向分量Shift_x(t)代表該自駕車20在先後兩時間點內往前或往後移動的幅度，該橫向分量Shift_y(t)代表該自駕車20在先後兩時間點內橫向移動的幅度。

本發明的實施例中，該處理器10根據該縱向加速度Ax與複數門檻值的大小將該車速V(t)轉換為一校正車速V1(t)，再利用該校正車速V1(t)計算該位移量。其中，該處理器10儲存有一第一門檻值th1、一第二門檻值th2、一第三門檻值th3、一第四門檻值th4與一第五門檻值th5，且th1＜th2＜th3＜th4＜th5，以及儲存有一第一調整值ad1、一第二調整值ad2、一第三調整值ad3、一第四調整值ad4、一第五調整值ad5與一第六調整值ad6，該第一門檻值至該第五門檻值th1~th5以及該第一調整值至該第六調整值ad1~sd6為預設值，且皆為實數。當該處理器10判斷出th4＞Ax≧th3，計算V1(t)=V(t) × ad4；當該處理器10判斷出th5＞Ax≧th4，計算V1(t)=V(t) × ad5；當該處理器10判斷出Ax≧th5，計算V1(t)=V(t) × ad6；當該處理器10判斷出th3＞Ax≧th2，計算V1(t)=V(t) × ad3；當該處理器10判斷出th2＞Ax≧th1，計算V1(t)=V(t) × ad2；當該處理器10判斷出th1＞Ax，計算V1(t)=V(t) × ad1。本發明的實施例中，th1= -1，th2= -0.4，th3=0，th4=0.2，th5=0.5，ad1=1，ad2=1.03，ad3=1.03，ad4=1.06，ad5=1.16，ad6=1.1。

於計算出該校正車速V1(t)後，該處理器10判斷最新的一筆該偏擺率ω是否大於或等於一偏擺率門檻值thw，該偏擺率門檻值thw例如可為0.05(rad/second)。若ω≧thw，該處理器10分別計算該縱向分量Shift_x(t)與該橫向分量Shift_y(t)如下：

其中，V1(t)為該校正車速，

為最新的一筆該偏擺率，

為該航向角，

為先後接收到之該偏擺率

的時間差。

若ω並非大於或等於thw，該處理器10分別計算該縱向分量Shift_x(t)與該橫向分量Shift_y(t)如下：

其中，V1(t)為該校正車速，

為該航向角，

為該側滑角，

為先後接收到之該偏擺率

或該側滑角

的時間差。

綜上，隨著時間進行，該處理器10根據先後接收到的該複數車輛動態參數S4與該複數車輛資訊S5計算並儲存複數位移量，每一筆該位移量包含在先後兩時間點的縱向分量Shift_x(t)與橫向分量Shift_y(t)。

步驟S03：延遲修正，該處理器10根據該複數位移量對該第一即時座標S1、該第二即時座標S2與該車道辨識即時資料S3進行延遲修正。可理解的是，該第一定位系統221、該第二定位系統222、該車道辨識系統223與該慣性量測系統224的功能彼此不同，其資料處理時間也彼此不同，故其向該處理器10進行資料更新頻的率彼此不同，一般而言，又以該慣性量測系統224的資料更新頻率高於該第一定位系統221、該第二定位系統222與該車道辨識系統223，例如該SLAM系統、該RTK-GPS系統與該車道辨識系統223的資料更新頻率皆低於或等於10Hz，該慣性量測系統224的資料更新頻率約為50Hz。舉例來說，請配合參考圖6，該處理器10接收到一筆該第一即時座標S1且其座標值為

，該處理器10可計算該第一即時座標S1與一前一筆第一即時座標S1’的一時間差t1，該時間差t1約為100ms，該處理器10約每20ms儲存一筆該位移量，故在該處理器10接收到最新一筆該第一即時座標S1時，在前已儲存多筆位移量SHn-1、SHn-2、SHn-3、SHn-4等，是以，該處理器10將多筆位移量SHn-1、SHn-2、SHn-3、SHn-4疊加至該第一即時座標S1，即可估測該第一即時座標S1與該前一筆第一即時座標S1’之間的位置。以下舉例該第一即時座標S1與前一筆位移量SHn-1的疊加方式，其疊加後的座標值為(

,

)，表示如下：

前述中，

和

中的

視該第一定位系統221的該SLAM運算時間而定，也就是將該SLAM運算時間代入

和

中的

，故當第一定位系統221的該SLAM運算時間越長，該縱向分量Shift_x(t)與該橫向分量Shift_y(t)也越大。依此類推，該處理器10接收到一筆該第二即時座標S2與該車道辨識即時資料S3，亦根據該縱向分量Shift_x(t)與該橫向分量Shift_y(t)進行延遲修正。

步驟S04：該處理器10判斷該第一即時座標S1與該第二即時座標S2的等級(level)。本發明的實施例中，該處理器10可根據該SLAM附屬資訊中的該分數與該疊代次數判斷該第一即時座標S1的等級，等級之數值較小代表等級較佳，當該分數與該疊代次數的數值越小，該第一即時座標S1的等級越高，下表為該處理器10判斷該第一即時座標S1之等級的範例，其中，下表中的各判斷值是預設值。

第一即時座標的等級	分數(score)	疊代次數
0	小於1	小於6次
1	小於2	小於15次
2	小於3	小於15次

類似的，該處理器10可根據該RTK-GPS附屬資訊中的該RTK狀態與該INS狀態判斷該第二即時座標S2的等級，等級之數值較小代表等級較佳，下表為該處理器10判斷該第二即時座標S2之等級的範例。

第二即時座標的等級	RTK狀態	INS狀態
0	固定 (fixed)	好 (Good)
1	浮動 (float)	好 (Good)
2	其他	差 (NG)

步驟S05：該處理器10根據該複數位移量與具有較佳等級的該第一即時座標S1或該第二即時座標S2計算出一動態預估座標。在此步驟中，當該處理器10收到該第一即時座標S1與該第二即時座標S2時，該自駕車20仍是行進中的狀態，故在接收到下一筆第一即時座標S1或第二即時座標S2之前，該自駕車20的位置是有改變的，該處理器10能儲存多筆新位移量，是以，為確保定位準確性，該處理器10僅採用具有較佳等級的該第一即時座標S1或該第二即時座標S2，並將該多筆新位移量疊加至具有較佳等級的該第一即時座標S1或該第二即時座標S2而形成該動態預估座標，即可在接收到下一筆該第一即時座標S1或該第二即時座標S2之前得到該動態預估座標。請參考圖6，係以該第一即時座標S1具有較佳等級為例，新的位移量SHn+1疊加至該第一即時座標S1即可得到該動態預估座標。

步驟S06：該處理器10根據具有較佳等級的該第一即時座標S1或該第二即時座標S2搭配一地圖資訊30產生一橫向位置資料，並判斷該橫向位置資料與該車道辨識即時資料S3的誤差是否在容許值內，若是，根據該車道辨識即時資料S3或該橫向位置資料與具有較佳等級的該第一即時座標S1或該第二即時座標S2計算出一橫向輔助座標。在此步驟中，請參考圖4，該地圖資訊30可儲存在一記憶體11，該處理器10電性連接該記憶體11以讀取該地圖資訊30，請參考圖7所示為該地圖資訊30的部分資料，該地圖資訊30包含一左車道線31的方程式資料和一右車道線32的方程式資料，是以，以具有較佳等級的該第一即時座標S1為例，該處理器10可計算該第一即時座標S1至該左車道線31的一左橫向距離CL，以及計算該第一即時座標S1至該右車道線32的一右橫向距離CR，該橫向位置資料即包含該左橫向距離CL和該右橫向距離CR。該處理器10判斷該左橫向距離CL與步驟S01的該左間隔距離S3L之差異是否小於一左橫向容許值，以及判斷該右橫向距離CR與步驟S01的該右間隔距離S3R之差異是否小於一右橫向容許值。當該左橫向距離CL與該左間隔距離S3L之差異小於該左橫向容許值，且該右橫向距離CR與該右間隔距離S3R之差異小於該右橫向容許值，代表該橫向位置資料與該車道辨識即時資料S3都是可採用的，該處理器10可根據該橫向位置資料與該車道辨識即時資料S3的其中之一者計算該橫向輔助座標。

步驟S07：該處理器10根據該第一即時座標S1與其權重值、該第二即時座標S2與其權重值、該動態預估座標與其權重值以及該橫向輔助座標與其權重值產生一融合定位座標。在此步驟中，該融合定位座標表示如下：

上式中，A1為該第一即時座標S1的權重值，A2為該第二即時座標S2的權重值，故透過A1與A2的設定，實施絕對位置權重分配；A3為該動態預估座標的權重值，A4為該橫向輔助座標的權重值，該處理器10可根據不同條件對應設定A1、A2、A3與A4，下表為一範例，其中，所述條件及A1至A4係儲存在該處理器10的資料，可視需求調整，也就是說，該處理器根據該SLAM絕對座標的等級與該RTK-GPS絕對座標的等級設定A1、A2、A3與A4。

	條件	A1 (%)	A2 (%)	A3 (%)	A4 (%)
1	該SLAM絕對座標的等級為0	100	0	0	0
2	該SLAM絕對座標的等級為2；該RTK-GPS絕對座標的等級為0	0	100	0	0
3	該SLAM絕對座標的等級為2；該RTK-GPS絕對座標的等級為1	0	40	30	30
4	該SLAM絕對座標的等級為2；該RTK-GPS絕對座標的等級為2	0	0	70	30
5	該SLAM絕對座標的等級為1；該RTK-GPS絕對座標的等級為0	30	40	15	15
6	該SLAM絕對座標的等級為1；該RTK-GPS絕對座標的等級為1	30	30	20	20

步驟S08：該處理器10分別判斷該第一即時座標S1、該第二即時座標S2、該動態預估座標與該橫向輔助座標的信心度資訊。因本發明採用該第一定位系統221、該第二定位系統222、該車道辨識系統223以及該慣性量測系統224等多系統，故針對每個系統產生的資料進行信心度判別，若某一電子系統22的信心度判別結果不佳，代表該電子系統22可能需要校正或檢修，說明如下。

(1) 絕對位置信心度

該處理器10判斷最新收到的該第一即時座標S1與該第二即時座標S2之間的誤差是否小於或等於一第一設定值，該第一設定值為一預設值，該處理器10並根據判斷結果輸出一第一信心度資訊，如下表所示。

	判斷結果	第一信心度資訊
該第一即時座標與該第二即時座標之間的誤差是否小於或等於一第一設定值	是	代表「可信」的文字訊息或代碼；該第一即時座標與該第二即時座標之間的誤差
否	代表「不可用」的文字訊息或代碼；該第一即時座標與該第二即時座標之間的誤差

(2) 動態定位信心度

該處理器10計算出該動態預估座標後，判斷該融合定位座標與該動態預估座標之間的誤差在一門檻時間內是否小於一第二設定值，並根據判斷結果輸出一第二信心度資訊。其中，該門檻時間與該第二設定值分別是預設值。

	判斷結果	第二信心度資訊
該融合定位座標與該動態預估座標之間的誤差在一門檻時間內是否小於一第二設定值	是	代表「信心度高」的文字訊息或代碼
否	代表「信心度低」的文字訊息或代碼

(3) 橫向位置信心度

如前所述，在步驟S06中，該處理器10已搭配該地圖資訊30產生該橫向位置資料，且在步驟S07中，該處理器10已產生該融合定位座標，是以，該處理器10判斷該融合定位座標與該橫向位置資料之間的誤差是否小於或等於一第三設定值，該第三設定值為一預設值，該處理器10並根據判斷結果輸出一第三信心度資訊，如下表所示。

	判斷結果	第三信心度資訊
該融合定位座標與該橫向位置資料之間的誤差是否小於或等於一第三設定值	是	代表「可信度高」的文字訊息或代碼
否	代表「橫向偏差過大」的文字訊息或代碼

綜上，在步驟S08中，該處理器10所判斷出的所述信心度資訊包含該第一信心度資訊、該第二信心度資訊和該第三信心度資訊。

步驟S09：輸出延遲修正後的該第一即時座標S1、該第二即時座標S2與該車道辨識即時資料S3、該融合定位座標與所述信心度資訊至該自駕車控制器21，供該自駕車控制器21據以實施自駕車的運作。其中，該處理器10通過前述步驟S01至S08的運作，產生的資訊包含輸出延遲修正後的該第一即時座標S1、該第二即時座標S2與該車道辨識即時資料S3(根據步驟S02)、該融合定位座標(根據步驟S07)與所述信心度資訊(根據步驟S08)，故將前述資料提供給該自駕車控制器21進行自動駕駛操作。

綜上，當該自駕車控制器21根據所述資料進行自動駕駛操作時，可提升自駕車20控制的精準度，及減少自駕車20接收到定位資訊短暫失效的現象，並且該自駕車控制器21在接收到信心度低的資訊後，可提前進行安全防護行駛的動作，例如操控該自駕車20的該轉向系統24及該動力系統25，使該自駕車20實施減速行駛或靠邊停車等動作，避免發生車禍或緊急煞車狀況。

10:處理器 11:記憶體 20:自駕車 21:自駕車控制器 22:電子系統 221:第一定位系統 222:第二定位系統 223:車道辨識系統 224:慣性量測系統 225:車輛資訊系統 23:資料匯流排 24:轉向系統 25:動力系統 30:地圖資訊 31:左車道線 32:右車道線 S1:第一即時座標 S2:第二即時座標 S3:車道辨識即時資料 S4:車輛動態參數 S5:車輛資訊 CL:左橫向距離 CR:右橫向距離 S3L:左間隔距離 S3R:右間隔距離

圖1：本發明融合式車輛定位系統的方塊示意圖。圖2A、圖2B：本發明融合式車輛定位方法的流程圖。圖3：本發明融合式車輛定位方法的簡化流程圖。圖4：本發明融合式車輛定位系統的實施例的方塊示意圖。圖5：本發明中，經緯度座標系與自駕車的座標系的示意圖。圖6：本發明中，第一即時座標與位移量的時序圖。圖7：本發明中，搭配地圖資訊產生橫向位置資料，並與車道辨識即時資料進行比對的示意圖。

Claims

一種融合式車輛定位方法，於一處理器執行，該處理器電性連接一自駕車控制器與複數電子系統，該融合式車輛定位方法包含：(a)分別從該複數電子系統接收一第一即時座標、一第二即時座標、一車道辨識即時資料、複數車輛動態參數與複數車輛資訊；(b)隨著時間進行，根據先後接收到的該複數車輛動態參數與該複數車輛資訊計算並儲存複數位移量；(c)根據該複數位移量對該第一即時座標、該第二即時座標與該車道辨識即時資料進行延遲修正；(d)判斷該第一即時座標與該第二即時座標的等級；(e)根據該複數位移量與具有較佳等級的該第一即時座標或該第二即時座標計算出一動態預估座標；(f)根據具有較佳等級的該第一即時座標或該第二即時座標搭配一地圖資訊產生一橫向位置資料，並判斷該橫向位置資料與該車道辨識即時資料的誤差是否在容許值內，若是，根據該車道辨識即時資料或該橫向位置資料與具有較佳等級的該第一即時座標或該第二即時座標計算出一橫向輔助座標；(g)根據該第一即時座標與其權重值、該第二即時座標與其權重值、該動態預估座標與其權重值以及該橫向輔助座標與其權重值產生一融合定位座標；(h)分別判斷該第一即時座標、該第二即時座標、該動態預估座標與該橫向輔助座標的信心度資訊；以及(i)輸出延遲修正後的該第一即時座標、該第二即時座標與該車道辨識即時資料、該融合定位座標與該信心度資訊至該自駕車控制器，供該自駕車控制器據以實施自駕車的運作。
如請求項1所述之融合式車輛定位方法，在步驟(a)中：該第一即時座標為一同步定位與地圖構建(SLAM)絕對座標；該第二即時座標為一即時動態全球定位系統(RTK-GPS)絕對座標；該車道辨識即時資料包含一左間隔距離與一右間隔距離；該複數車輛動態參數包含一航向角、一側滑角、一偏擺率、一縱向加速度與一側向加速度；該複數車輛資訊包含方向盤轉角與車速。
如請求項1所述之融合式車輛定位方法，其中，該處理器計算一估計偏擺率，並判斷該估計偏擺率與該複數車輛動態參數中之一偏擺率之間的差異是否大於一偏擺率門檻值；若否，採用該偏擺率進行運算。
如請求項1所述之融合式車輛定位方法，在步驟(b)中，各該位移量包含一縱向分量Shift_x(t)與一橫向分量Shift_y(t)；當該處理器判斷該複數車輛動態參數中最新的一偏擺率大於或等於一偏擺率門檻值，該縱向分量Shift_x(t)與該橫向分量Shift_y(t)分別表示如下：

其中，V1(t)為一校正車速，ω(t)為最新的該偏擺率，φ(t)為該複數車輛動態參數中之一航向角，△t為先後接收到之該偏擺率ω(t)的時間差；當該處理器判斷該複數車輛動態參數中最新的一偏擺率小於該偏擺率門檻值，該縱向分量Shift_x(t)與該橫向分量Shift_y(t)分別表示如下：Shift_x(t)=V1(t)cos(φ(t)+β(t))＊△t Shift_y(t)=V1(t)sin(φ(t)+β(t))＊△t 其中，V1(t)為一校正車速，φ(t)為該複數車輛動態參數中之一航向角，β(t)為該複數車輛動態參數中之一側滑角，△t為先後接收到之該偏擺率ω(t)或該側滑角β(t)的時間差；該處理器根據該複數車輛動態參數中之一縱向加速度與複數門檻值而將該複數車輛資訊中之一車速轉換為該校正車速。
如請求項1所述之融合式車輛定位方法，在步驟(c)中，該處理器將多筆所述位移量疊加至該第一即時座標、該第二即時座標與該車道辨識即時資料以進行延遲修正。
如請求項1所述之融合式車輛定位方法，在步驟(a)中，該車道辨識即時資料包含一左間隔距離與一右間隔距離；在步驟(f)中，該地圖資訊包含一左車道線的方程式資料和一右車道線的方程式資料；該處理器計算具有較佳等級的該第一即時座標或該第二即時座標至該左車道線的一左橫向距離，以及計算具有較佳等級的該第一即時座標或該第二即時座標至該右車道線的一右橫向距離，其中，該橫向位置資料包含該左橫向距離和該右橫向距離；該處理器判斷該左橫向距離與該左間隔距離之差異是否小於一左橫向容許值，以及判斷該右橫向距離與該右間隔距離之差異是否小於一右橫向容許值；當該處理器判斷該左橫向距離與該左間隔距離之差異小於該左橫向容許值，且該右橫向距離與該右間隔距離之差異小於該右橫向容許值，該處理器根據該橫向位置資料與該車道辨識即時資料的其中之一者計算該橫向輔助座標。
如請求項1所述之融合式車輛定位方法，在步驟(g)中，該融合定位座標表示如下：融合定位座標=[(第一即時座標＊A1%+第二即時座標＊A2%)+動態預估座標＊A3%]+橫向輔助座標＊A4%上式中，A1、A2、A3與A4分別為權重值，該處理器根據該第一即時座標的等級與該第二即時座標的等級設定A1、A2、A3與A4，其中，該第一即時座標為同步定位與地圖構建(SLAM)絕對座標，該第二即時座標為即時動態全球定位系統(RTK-GPS)絕對座標。
如請求項1所述之融合式車輛定位方法，在步驟(h)中：該處理器判斷最新收到的該第一即時座標與該第二即時座標之間的誤差是否小於或等於一第一設定值，並根據判斷結果輸出一第一信心度資訊，該第一信心度資訊為文字訊息或代碼；該處理器計算出該動態預估座標後，判斷該融合定位座標與該動態預估座標之間的誤差在一門檻時間內是否小於一第二設定值，並根據其判斷結果輸出一第二信心度資訊，該第二信心度資訊為文字訊息或代碼；該處理器判斷該融合定位座標與該橫向位置資料之間的誤差是否小於或等於一第三設定值，並根據其判斷結果輸出一第三信心度資訊，該第三信心度資訊為文字訊息或代碼。
一種融合式車輛定位系統，包含：一自駕車控制器；複數電子系統，分別輸出一第一即時座標、一第二即時座標、一車道辨識即時資料、複數車輛動態參數與複數車輛資訊；以及一處理器，電性連接該自駕車控制器與該複數電子系統，該處理器分別從該複數電子系統接收該第一即時座標、該第二即時座標、該車道辨識即時資料、該複數車輛動態參數與該複數車輛資訊，並根據先後接收到的該複數車輛動態參數與該複數車輛資訊計算並儲存複數位移量；該處理器根據該複數位移量對該第一即時座標、該第二即時座標與該車道辨識即時資料進行延遲修正；該處理器判斷該第一即時座標與該第二即時座標的等級、根據該複數位移量與具有較佳等級的該第一即時座標或該第二即時座標計算出一動態預估座標、根據具有較佳等級的該第一即時座標或該第二即時座標搭配一地圖資訊產生一橫向位置資料，並判斷該橫向位置資料與該車道辨識即時資料的誤差是否在容許值內，若是，根據該車道辨識即時資料或該橫向位置資料與具有較佳等級的該第一即時座標或該第二即時座標計算出一橫向輔助座標；該處理器根據該第一即時座標與其權重值、該第二即時座標與其權重值、該動態預估座標與其權重值以及該橫向輔助座標與其權重值產生一融合定位座標，並分別判斷該第一即時座標、該第二即時座標、該動態預估座標與該橫向輔助座標的信心度資訊；該處理器輸出延遲修正後的該第一即時座標、該第二即時座標與該車道辨識即時資料、該融合定位座標與該信心度資訊至該自駕車控制器，供該自駕車控制器據以實施自駕車的運作。
請求項9所述之融合式車輛定位系統，其中，該複數電子系統包含：一第一定位系統，為同步定位與地圖構建(Simultaneous Localization And Mapping,SLAM)系統，其輸出該第一即時座標；一第二定位系統，為即時動態全球定位(Real-Time Kinematic-Global Positioning System,RTK-GPS)系統，其輸出該第二即時座標；一車道辨識系統，輸出該車道辨識即時資料；一慣性量測系統，輸出該複數車輛動態參數；以及一車輛資訊系統，輸出該複數車輛資訊；該地圖資訊儲存在一記憶體，該處理器電性連接該記憶體以讀取該地圖資訊。