本發(fā)明是一種物體定位方法，特別是指一種協(xié)同式物體定位(Cooperative positioning)優(yōu)化方法。

背景技術(shù)：

在車輛中配置感測器(sensor)以檢測周圍環(huán)境的相關(guān)技術(shù)已發(fā)展許久，例如使用全球定位系統(tǒng)(GPS)、雷達(RADAR)、光達(LIDAR)、行車記錄器等不同感測器可提供多元的環(huán)境資訊。但是對車輛本身而言，透過環(huán)境資訊感測器所取得的環(huán)境資訊仍會受到許多因素的限制，舉例而言，請參考圖9所示，在十字路口的環(huán)境中，行駛在縱向車道的第一車輛101可得知該方向的環(huán)境資訊，例如可看到有物體200(如行人、車輛、動物)正快速沖出路口，但對于在橫向車道的第二車輛102而言，因受限于其所在位置可能受周圍建筑物遮蔽而產(chǎn)生盲點，即使第二車輛102上具有感測器，但仍無法察覺該物體200的突發(fā)情況而與其撞擊。因此，若僅單獨依據(jù)車輛本身的感測器提供環(huán)境資訊，在實際應(yīng)用時仍存在有盲點。

為此，車輛協(xié)同式定位方法遂發(fā)展出來，協(xié)同式的概念是表示透過周圍其鄰車輛或周圍的感測裝置(例如路側(cè)感測單元(Road side unit),RSU)分享各自感測到的資訊，使本車接收其它車輛的資訊而擴增自己的感測范圍，沿用上述圖9的例子說明，若該第二車輛102可接收該第一車輛提供的資訊，便可得知其右方路口將有物體突然出現(xiàn)，有足夠的時間緊急應(yīng)變。

然而目前協(xié)同式定位技術(shù)面臨準確性不佳的技術(shù)缺點，請參考圖10所示范例，第一車輛101表示本車，第二車輛102～第四車輛104表示鄰近的其它車輛。以第二車輛102而言，其配備有一商用GPS及一攝影機，而一般商用GPS的定位誤差約為5～15公尺，攝影機的定位誤差約為5公尺，因此，該第二車輛102利用自身GPS所測得的自身位置具有一GPS誤差范圍A1，第二車輛102利用攝影機感測到第三車輛103的所在位置時，由攝影機所測得的第三 車輛103位置具有一攝影機誤差范圍A2；因此，若第二車輛102將其感測到的第三車輛的位置資訊轉(zhuǎn)傳分享給第一車輛101時，第一車輛101所接收到的第三車輛103的位置資訊即有誤差累積的問題，如GPS及攝影機累加誤差范圍A3所示。

再進一步而言，若第一車輛101將該筆第三車輛103的位置資訊二度轉(zhuǎn)傳分享給第四車輛104時，將會再進一步加上第一車輛101自身的位置誤差，導(dǎo)致第四車輛104所接收的位置資訊產(chǎn)生更多的累積誤差量。因此，當(dāng)資訊經(jīng)過多次的轉(zhuǎn)傳分享后，對物體的定位準確性將會顯著下降，甚至不具參考價值。

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

鑒于既有協(xié)同式定位技術(shù)存在有位置資訊精確度不佳的問題，本發(fā)明的主要目的是提供一種車輛協(xié)同式物體定位優(yōu)化方法，以擴增車輛的感范圍、提高位置資訊的精準度而提升車輛行駛的安全性。

為達到前述目的，本發(fā)明的方法是利用一設(shè)置在本車內(nèi)部的協(xié)同定位裝置執(zhí)行，該方法包含有：

由本車接收一資訊封包，該資訊封包中包含一鄰車提供的車輛原始坐標及至少一物體原始坐標，該車輛原始坐標及該物體原始坐標分別具有各自的定位精準度；

對該車輛原始坐標及物體原始坐標進行時間延遲補償，以分別得到鄰車補償后的一車輛坐標及一物體坐標；

執(zhí)行一優(yōu)化程序，其包含：

比對本車的車輛坐標與鄰車的車輛坐標，判斷何者具有較高的定位精準度；

優(yōu)先對具有較高精準度的車輛坐標進行一優(yōu)化運算，再對具有較低定位精準度的車輛坐標進行優(yōu)化運算，其中，該優(yōu)化運算執(zhí)行：

a)根據(jù)本車的車輛坐標與鄰車的車輛坐標計算出多個參考位置；及

b)根據(jù)各參考位置的權(quán)重值，計算出一車輛優(yōu)化坐標；

對物體坐標進行優(yōu)化，是比較鄰車的原始車輛坐標及其車輛優(yōu)化坐標之間的差異量，依據(jù)該差異量對鄰車提供的物體坐標進行補償以得到一物體優(yōu)化坐標；并比較本車的原始車輛坐標及本車的車輛優(yōu)化坐標之間的差異量，依該差 異量對本車提供的物體坐標進行補償以得到一物體優(yōu)化坐標。

藉由本發(fā)明的定位優(yōu)化方法，可獲得本車及鄰車優(yōu)化后的車輛坐標、物體坐標，該優(yōu)化后的坐標可更加接近實際化置，有助于在行車過程中準確地判斷周圍環(huán)境的物體、車輛分布狀態(tài)，提升車輛行駛的安全性。

附圖說明

圖1是本發(fā)明協(xié)同定位裝置的架構(gòu)方塊圖。

圖2是本發(fā)明車輛協(xié)同式物體定位優(yōu)化方法的流程圖。

圖3是本發(fā)明BSM數(shù)據(jù)封包格式示意圖。

圖4是本發(fā)明對車輛原始位置執(zhí)行時間延遲補償?shù)氖疽鈭D。

圖5是本發(fā)明位置優(yōu)化模塊所執(zhí)行的優(yōu)化程序流程圖。

圖6A是本發(fā)明取得第一參考位置H1的示意圖。

圖6B是本發(fā)明取得第一參考位置H2的示意圖。

圖6C是本發(fā)明取得第一參考位置H3的示意圖。

圖6D是本發(fā)明取得第一參考位置H4的示意圖。

圖7是本發(fā)明利用多個參考位置計算車輛優(yōu)化坐標的示意圖。

圖8是本發(fā)明中多臺車輛的資訊分享示意圖。

圖9是車輛行經(jīng)交叉路口的示意圖。

圖10是協(xié)定式定位的誤差累積示意圖。

其中，附圖標記：

10 車輛協(xié)同定位裝置

11 無線傳輸接口

12 延遲修正模塊

13 位置優(yōu)化模塊

14 定位比對模塊

15 車身感知器

21 第一部分

22 第二部分

101 第一車輛

102 第二車輛

103 第三車輛

104 第四車輛

200 物體

A1 GPS誤差范圍

A2 攝影機誤差范圍

A3 GPS及攝影機累加誤差范圍

H 本車

R 鄰車

H1～H4 第一參考位置～第四參考位置

R1 基準位置

M1～M4 第一涵蓋范圍～第四涵蓋范圍

具體實施方式

請參考圖1所示，本發(fā)明利用配備在各車輛上的一車身感知器15，包含GPS接收器及其它多種感測器，例如雷達、攝影機等設(shè)備取得本車坐標與車輛周圍的環(huán)境資訊，并透過無線通信技術(shù)將本車坐標與環(huán)境資訊轉(zhuǎn)傳分享給周圍的鄰近車輛。因此，就任何一臺車輛而言，是與周圍車輛進行接收及傳輸?shù)碾p向無線通信。

本發(fā)明在車輛內(nèi)部設(shè)置一車輛協(xié)同定位裝置10，該車輛協(xié)同定位裝置10包含有一無線傳輸接口11、一延遲修正模塊12、一位置優(yōu)化模塊13及一定位比對模塊14。該車輛協(xié)同定位裝置10執(zhí)行一車輛協(xié)同式物體定位優(yōu)化方法，該方法如圖2所示，包含有以下步驟：

S10：接收一資訊封包，該資訊封包中包含鄰車的一車輛原始坐標及至少一物體原始坐標以及該車輛原始坐標與物體原始坐標各自的定位精準度；

S20：對該車輛原始坐標及物體原始坐標進行時間延遲補償，以分別得到鄰車補償后的的一車輛坐標及一物體坐標；

S30：執(zhí)行一優(yōu)化程序；

S40：坐標比對融合。

在步驟S10：該無線傳輸接口11負責(zé)本車與鄰車之間的數(shù)據(jù)雙向傳輸，在本實施例中是采用一短距無線通信接口(DSRC)，于車輛之間周期性地收發(fā)數(shù) 據(jù)封包，數(shù)據(jù)封包格式是采用基本安全信息(Basic Safety Message,BSM)封包，請參考圖3，該數(shù)據(jù)封包的信息格式大致上包含一msgID欄位、第一部分21(Part I)及第二部分22(Part II)，其中第一部分21定義為必要資訊，包含基本的安全信息內(nèi)容，為每個數(shù)據(jù)封包必然包含的部分，但第二部分22為非必要部分(optional)，可由使用者視應(yīng)用需求將所需的資訊加入在第二部分22，屬于自行定義的范圍。

在BSM數(shù)據(jù)封包的第一部分21中，即必然存在有車輛的經(jīng)緯度資訊，也就是車輛的原始坐標，表示利用車輛內(nèi)的GPS接收器所測得的本車位置。

在BSM數(shù)據(jù)封包中的第二部分22中，本發(fā)明加入兩類資訊，第一類資訊為輸出該車輛原始坐標的感測器種類(例如RTK、GPS)及該感測器的定位精準度。第二類資訊包含有物體(object)原始坐標、產(chǎn)生該物體原始坐標的感測器種類、或進一步包含該感測器的精準度，該物體原始坐標是指利用本車的其它感測器(例如radar,lidar,camera等)感測本車以外的其它物體的資訊，該物體可能是車輛、行人、移動物或固定物等。各車輛可將本車的車輛原始坐標與物體原始坐標對外發(fā)送，供周圍鄰車利用；而本車同樣可接收來自周圍鄰車提供的車輛原始坐標與物體原始坐標。

在步驟S20：該延遲修正模塊12透過該無線傳輸接口11接收鄰車傳輸出來的的BSM數(shù)據(jù)封包，獲得鄰車的車輛原始坐標與物體原始坐標，此外，該位置優(yōu)化模塊13也進一步接收本車的感測器提供的各種感測結(jié)果，例如車輛坐標(GPS)、物體坐標。該延遲修正模塊12針對鄰車提供的車輛原始坐標與物體原始坐標進行時間延遲補償，請參考圖4所示，一第一車輛101表示本車，在其附近的一第二車輛102表示鄰車，當(dāng)?shù)谝卉囕v101接收到第二車輛102發(fā)出的數(shù)據(jù)封包時，第一車輛101可根據(jù)記錄在封包內(nèi)的一封包發(fā)出時間及其本身接收到該封包時的一封包接收時間，計算出該封包發(fā)出時間及封包接收時間之間的一時間延遲量n。因為第二車輛102在送出封包后，仍持續(xù)行駛前進由原始位置P_A到位置P_B，因此第一車輛101所接收到的第二車輛102原始坐標只是代表原始位置P_A，而延遲修正模塊12即根據(jù)該時間延遲量n推算第二車輛102所移動的一補償距離，加上該補償距離而得知即時位置P_B，其推算公式可表示如下：

P_B＝P_A+V×n，其中，V代表第二車輛102的車速。

當(dāng)延遲修正模塊12推算出第二車輛102的即時位置P_B，可根據(jù)兩位置P_A、P_B的補償距離，一并對第二車輛102提供的物體原始坐標加入該補償距離。在車輛原始坐標、物體原始坐標都經(jīng)過時間補償后，即分別得到一車輛坐標及一物體坐標，提供給該位置優(yōu)化模塊13進行后續(xù)處理。

在步驟S30：該位置優(yōu)化模塊13接收到經(jīng)時間補償后的鄰車的車輛坐標及物體坐標，并接收本車的車輛坐標及本車的感測器測得的物體坐標，并執(zhí)行一優(yōu)化程序如圖5所示，該優(yōu)化程序包含有以下流程S31～S33:

S31：比較定位精準度，是比較本車的車輛坐標與鄰車的車輛坐標，判斷何者的精準度較高。舉例而言，若本車的車輛坐標是利用一即時動態(tài)測量(RTK)裝置所得到的坐標，而鄰車的車輛坐標是利用一般GPS接收器接收，則可判斷RTK提供的坐標具有較高精準度；又例如本車與鄰車皆是利用相同等級的GPS接收器提供坐標，則可根據(jù)兩GPS信號中的信任度來判斷何者精準度較高，例如根據(jù)GPS信號中包含的GGA信息判斷兩GPS信號何者的信任度較高。

S32：車輛定位優(yōu)化，在判斷出本車與鄰車兩者的車輛坐標何者精準度較高后，將先針對精準度較高者進行優(yōu)化運算，其次再對精準度較低者進行優(yōu)化運算。無論是針對本車或鄰車的位置資訊進行優(yōu)化運算，其作法是：

a)根據(jù)本車與鄰車的車輛坐標計算出多個參考位置；

b)根據(jù)各參考位置的權(quán)重值，計算出一車輛優(yōu)化坐標。

在本實施例中，假設(shè)本車與鄰車皆配備有GPS接收器及其它感測器，并根據(jù)四個參考位置計算出車輛優(yōu)化坐標，且當(dāng)判斷本車H的車輛坐標與鄰車的車輛坐標后，得知本車H的車輛坐標具有較高的精準度，故以本車H為中心，優(yōu)先對本車的車輛坐標進行優(yōu)化，其步驟詳述如后。首先，請參考圖6A所示，本車及鄰車分別以H、R表示，兩車可根據(jù)自身的GPS接收器得知自己的車輛坐標，其中，本車H根據(jù)GPS接收器得知的車輛坐標作為第一參考位置H1，該第一參考位置H1與本車H的實際位置有差異是因為GPS接收器的誤差量導(dǎo)致，鄰車R的GPS接收器感測出的車輛坐標作為一基準位置R1。

請參考圖6B所示，鄰車R的感測器因為可感知本車H的存在，因此可得知鄰車R與本車H與之間的相對距離D1與相對角度θ1，即得知本車H的相對坐標。鄰車R即以自已的基準位置R1為參考基準，根據(jù)距離D1及角度θ1將本車 H的相對坐標轉(zhuǎn)換為一經(jīng)緯度坐標，該經(jīng)緯度坐標即作為第二參考位置H2。由鄰車R發(fā)出的數(shù)據(jù)封包，即包含有該第二參考位置H2的經(jīng)緯度坐標。

請參考圖6C所示，本車H的感測器因為可感知鄰車R的存在，因此可得知本車H與鄰車R與之間的相對距離D2與相對角度θ2。本車H取得與鄰車R之間的相對距離D2及相對角度θ2之后，以鄰車R的基準位置R1作為參考基準反推出本車H的經(jīng)緯度坐標，可得到一第三參考位置H3。

請參考圖6D所示，鄰車R與本車H之間以無線信號傳輸數(shù)據(jù)封包，因此，可根據(jù)無線信號收、發(fā)之間的強度衰減程度，推估兩車之間的相對距離D3，例如鄰車R發(fā)射出的無線信號功率預(yù)設(shè)為-10dBi，而本車H接收到的無線信號功率己成為-30dBi，即顯示兩車之間的距離讓無線信號衰減了20dBi，因為衰減幅度與距離成正比且可預(yù)先建立一衰減關(guān)系表，因此根據(jù)該20dBi的衰減量可推測或查表得知兩車之間的相對距離D3。另一方面，因為本車H與鄰車R利用GPS接收器測知的車輛坐標為已知，即第一參考位置H1及基準位置R1均為已知，可根據(jù)兩位置之間的一直向延伸線推測出本車H相對于鄰車R的所在方向。因此，以鄰車R的基準位置R1為基準，根據(jù)相對距離D3及方向算出一第四參考位置H4。

請參考圖7，當(dāng)取得第一參考位置H1～第四參考位置H4后，以各參考位置H1～H4為圓心相當(dāng)于可決定出第一涵蓋范圍M1～第四涵蓋范圍M4，每個涵蓋范圍的大小是根據(jù)其參考位置的來源感測器的精準度而定，假設(shè)本車H的GPS接收器具有最好的精準度，則第一參考位置H1的涵蓋范圍最小，且精準度越高者，該參考位置也會具有一較高的權(quán)重值ω，其中GPS接收器會提供其本身的精準度。若其它感測器無法提供自身精準度，可根據(jù)該感測器推知后的位置其所在距離而決定其權(quán)重值，距離越遠，其權(quán)重越低。而本發(fā)明即是在第一涵蓋范圍M1～第四涵蓋范圍M4的交集區(qū)域(如斜線區(qū)域表示)計算出一車輛優(yōu)化坐標H(x,y)。

在步驟b)中，是根據(jù)各參考位置的權(quán)重值ω，計算出一車輛優(yōu)化坐標H(x,y)，計算方式可表示如下：

其中，

在上式中，有m表示參考位置的數(shù)目，故本實施例m＝4；(x_i,y_i)分別表示第一～第四參考位置H1～H4的坐標；權(quán)重值ω_i的其中一種計算方式可采用Adaboost演算法或其它演算法。

除了Adaboost演算法，在此提供一種權(quán)重值的計算方式。首先，假設(shè)第一參考位置H1～第四參考位置H4的誤差值分別為3、6、4、5公尺，可利用一誤差反比演算法計算四個不同的權(quán)重值，計算方式如下：

計算總誤差量，∑_iε_i＝3+4+5+6＝18

分別計算總誤差量與各誤差值的差異量，

計算差異量的總量，

四個權(quán)重值分別為：

本車H的車輛優(yōu)化坐標其中(x_i,y_i)分別表示四個參考位置的坐標；H(x,y)代表車輛優(yōu)化坐標。

當(dāng)完成本車H的車輛坐標的優(yōu)化運算之后，其次再以鄰車R為中心，對鄰車R的車輛坐標進行優(yōu)化運算，其運算過程如同上述，只是互換本車與鄰車之間的角色，換言之，將鄰車數(shù)據(jù)視為是上述運算中的本車數(shù)據(jù)，并將本車數(shù)據(jù)視為是上述運算中的鄰車數(shù)據(jù)。因此，同樣可以得到代表鄰車的車輛優(yōu)化坐標R(x,y)。

S33：物體定位優(yōu)化，即針對鄰車R、本車H的感測器所測得的物體坐標皆進行優(yōu)化。在本車H部分，因為已得知本車H優(yōu)化后的車輛優(yōu)化坐標H(x,y)，故根據(jù)本車H感測器所測得的物體坐標，可以比較本車H的原始車輛坐標及其車 輛優(yōu)化坐標H(x,y)之間的差異量，于計算本車H感測器測得的物體的物體優(yōu)化坐標時，即根據(jù)該差異量對本車H感測器測得的差異量進行補償，而得到一物體優(yōu)化坐標，完成對本車H測得的物體坐標的優(yōu)化運算。同理，在鄰車R部分，因為已得知鄰車R優(yōu)化后的車輛優(yōu)化坐標，故根據(jù)鄰車R感測器所測得的物體坐標，可以比較鄰車R的原始車輛坐標及其車輛優(yōu)化坐標之間的差異量，在重新計算該對鄰車R感測器測得的物體的物體優(yōu)化坐標時，即根據(jù)該差異量對鄰車R感測器測得的差異量進行補償，而得到一物體優(yōu)化坐標。

在步驟S40，本發(fā)明可利用該定位比對模塊14融合來自不同鄰車R的多筆坐標。舉例來說，請參考圖8所示，對第一車輛101而言，其它任一車輛102～104均會感測到周圍的物體并分享給第一車輛101，第一車輛101會接收到多筆關(guān)于同一物體(例如第二車輛102)的坐標并暫存在一緩沖器(buffer)內(nèi)，因此，第一車輛101內(nèi)的定位比對模塊14會從緩沖器內(nèi)取出相似物體的坐標，并加以融合成單一個位置數(shù)據(jù)，不同的物體坐標則單獨新增。其中一種融合方式是透過K-Means分群演算法將多筆坐標計算出一平均值，將同一車輛的車輛優(yōu)化坐標平均計算出一車輛代表坐標，將同一物體的物體優(yōu)化坐標平均計算出一物體代表坐標。

綜上所述，本發(fā)明車輛協(xié)同式物體定位優(yōu)化方法具備有下述優(yōu)點：

1.利用本車與鄰車之間交換各自感測出的數(shù)據(jù)，可擴增各車輛的感測范圍，取得更多的環(huán)境數(shù)據(jù)。

2.透過協(xié)同定位裝置重新校正車輛及周圍物體的坐標數(shù)據(jù)，相較于單獨參考GPS接收器測知的坐標數(shù)據(jù)，本發(fā)明可以得到更精準的坐標，無論是應(yīng)用于自動駕駛系統(tǒng)，或是用于提前警示駕駛者周圍環(huán)境的現(xiàn)況，均可提升行車安全性。

3.利用本發(fā)明的協(xié)同定位方法，當(dāng)本車完成優(yōu)化運算后，可以得到優(yōu)化完成后的本車定位數(shù)據(jù)、鄰車定位數(shù)據(jù)和一或多個物體定位數(shù)據(jù)，這三份數(shù)據(jù)會以BSM數(shù)據(jù)封包的格式傳送給周圍的鄰車，這時候的鄰車接收到BSM數(shù)據(jù)封包時，亦可以單獨執(zhí)行本發(fā)明的優(yōu)化運算，因此，周圍各車可分散運算，隨著時間的逐漸累積，車輛彼此之間的定位數(shù)據(jù)亦會漸漸提高精準度。