其中,d為傳感器的安裝角度,即在水平面中傳感器探測范圍的中心方向與車輛行駛方向間的夾角,posx和posy為傳感器在車輛坐標系中的安裝位置;xveh,vxveh,axveh,yveh,vyveh,ayveh分別指車輛坐標系下目標與本車之間的縱向距離,相對縱向速度,相對縱向加速度,橫向距離,相對橫向速度和相對橫向加速度;xsen,vxsen,axsen,ysen,vysen,aysen分別指傳感器坐標系下傳感器檢測到的目標與傳感器之間的縱向距離,相對縱向速度,相對縱向加速度,橫向距離,相對橫向速度和相對橫向加速度;
根據融合中心接收到傳感器數據的時刻與融合中心當前工作時刻之間的差值,對車輛坐標系下目標和本車之間的縱向距離,相對縱向速度,相對縱向加速度,橫向距離,相對橫向速度和相對橫向加速度在這段時間差內發生的變化進行補償,得到車輛坐標系下補償之后目標與本車之間的縱向距離,相對縱向速度,相對縱向加速度,橫向距離,相對橫向速度和相對橫向加速度構成的目標運動狀態;
對各個傳感器檢測到的所有目標進行全局跟蹤,并對重復檢測到的目標進行融合,使融合中心能夠得到各個傳感器檢測范圍內的所有目標的連續運動軌跡,即運動狀態,具體包含如下步驟:
將各個傳感器檢測到的每個目標都記為測量目標,并將每個測量目標分別與全部歷史目標進行關聯匹配并判斷;關聯匹配成功的測量目標將會用于與之匹配的歷史目標的運動狀態更新,關聯失敗的測量目標均記為備選新目標;
對于與歷史目標關聯匹配成功的測量目標,根據其所關聯的歷史目標的運動狀態估計值應用卡爾曼濾波原理對該歷史目標在本周期的運動狀態和協方差進行估計,而且對于同時與多個測量目標關聯匹配成功的歷史目標還需要進行目標融合;
必一運動官網
對本周期的關聯失敗所形成的備選新目標和歷史目標中還未判斷其航跡生成成功的目標進行航跡生成判斷,其中判斷航跡生成成功的原則是:該目標連續3個周期與與其對應的歷史目標關聯匹配成功;
對于航跡生成成功的目標,之后的每個周期都要對其進行航跡消亡判斷,其中航跡消亡成功的判斷準則是:該目標連續5個周期與與其對應的歷史目標關聯匹配失敗;
在全體目標中刪除航跡生成失敗和航跡消亡成功的目標,得到航跡管理后本周期的所有目標及其運動狀態;
將航跡管理后得到的目標作為下一周期的歷史目標,根據ca運動學模型對這些目標在下一周期的運動狀態和協方差進行預測,并將預測結果用于步驟二中作為下一周期的目標關聯,運動狀態和協方差的預測公式如下:
其中,p(k+1k)分別為根據本周期的目標運動狀態預測得到的歷史目標的下一周期的運動狀態和協方差。
以航跡管理模塊輸出的本周期所有目標及其運動狀態作為輸入,篩除其中的備選新目標,即航跡生成判斷還沒完成的目標;將其余目標及其運動狀態作為本周期融合中心最終輸出的多傳感器融合的障礙物檢測結果。
2.根據權利要求1所述的一種智能汽車的目標級多傳感器融合方法,其特征在于所述步驟一中的時間同步是將目標和本車在融合中心接收傳感器數據的時刻與融合中心當前工作時刻之間的時間差所造成的運動狀態變化使用ca模型進行補償,補償公式如下:
其中,xcom,vxcom,axcom,ycom,vycom,aycom分別指車輛坐標系下補償之后目標與本車之間的縱向距離,相對縱向速度,相對縱向加速度,橫向距離,相對橫向速度和相對橫向加速度;δt表示融合中心接收傳感器數據的時刻與融合中心當前工作時刻之間的時間差。
3.根據權利要求2所述的一種智能汽車的目標級多傳感器融合方法,其特征在于所述步驟二的目標關聯中關聯匹配判斷的具體過程如下:
采用馬氏距離計算各個測量目標分別與前一時刻全體歷史目標之間的距離,并以矩陣的形式記錄,其中該距離矩陣的行數代表傳感器的測量目標個數m,列數代表歷史目標個數n,得到各個傳感器測量目標與前一時刻全體歷史目標的距離矩陣如下:
其中,g是每個歷史目標的距離門限值,當某個測量目標與某個歷史目標之間的馬氏距離小于或等于該距離門限值時才會被關聯;cij表示距離矩陣中第i行第j列元素的取值,i=1,2,...,m,j=1,2,...,n;cmax是人為設定的一個常數值,代表該測量目標在歷史目標的距離門限值g之外;表示第i個測量目標與第j個歷史目標之間的馬氏距離值;
然后采用kuhn-munkres分配算法,以全局距離代價最小為目標,將所有滿足距離門限值g的測量目標分配給歷史目標,使得每個歷史目標與與其對應的測量目標一一關聯匹配;由于存在多個傳感器,因此對每個傳感器的每個測量目標都重復上述過程,若一個歷史目標與多個測量目標都關聯匹配成功,則該歷史目標需要進行后續的融合過程,關聯匹配失敗的測量目標則作為備選新目標;
所述的第i個歷史目標與某個傳感器第j個測量目標之間馬氏距離的求取方法如下:
其中,t表示轉置運算,zj為第j個測量目標的運動狀態,即第j個測量目標的xcom,vxcom,axcom,ycom,vycom,aycom構成的目標運動狀態;zj-hxi(kk-1)表示殘差,殘差的協方差為s(k)=hp(kk-1)ht+r,其中h為測量矩陣,r為傳感器噪聲矩陣,p(kk-1)為歷史目標預測值的協方差。
4.根據權利要求3所述的一種智能汽車的目標級多傳感器融合方法,其特征在于所述步驟二的狀態估計與新目標生成中運動狀態的估計和目標融合的具體過程如下:
卡爾曼濾波中使用的運動模型為ca運動模型,故歷史目標的運動狀態的狀態空間方程可表示為如下形式,以常加速度運動,
其中,x為歷史目標在本周期的運動狀態向量(包括縱向距離x,橫向距離y,相對縱向速度vx,相對橫向速度vy,相對縱向加速度ax和相對橫向加速度ay,即x=[x,y,vx,vy,ax,ay]t);z為車輛坐標系下傳感器對該歷史目標運動狀態向量的測量值,即z=[xcom,ycom,vxcom,vycom,axcom,aycom]t;傳感器測量噪聲為v(k)服從n(0,r)的正態分布,ca運動模型噪聲w(k)服從n(0,q)的正態分布,q,r分別為模型噪聲和傳感器測量噪聲的協方差矩陣,各個矩陣的具體值如下,
其中a為ca運動模型的狀態轉移矩陣,γ為模型噪聲增益矩陣,h為測量矩陣,ts為融合中心的運行周期;
根據上述狀態空間方程,使用卡爾曼濾波的方法對關聯成功的歷史目標在本周期的運動狀態進行估計,運動狀態估計的過程如下,
其中,k為卡爾曼增益矩陣,p(kk-1)分別為上一周期的歷史目標的運動狀態預測值和協方差預測值,p(kk)分別為本周期狀態估計得到的歷史目標運動狀態估計值和協方差估計值;
若存在與多個測量目標都關聯匹配成功的歷史目標還需要對這個歷史目標進行航跡融合,將與多個測量目標都關聯匹配成功的歷史目標的運動狀態估計值融合為一個,航跡融合的公式如下,
其中,為與多個傳感器測量目標都關聯匹配成功的歷史目標分別與關聯匹配成功的第1...n個傳感器測量目標所得到的本周期的運動狀態估計值,p1...pn分別為的協方差;;p為融航跡融合之后的歷史目標在本周期的運動狀態估計值和協方差;對于關聯失敗的歷史目標,則將其運動狀態估計值作為本周期的運動狀態;對于關聯失敗的測量目標,將其作為備選新目標,并將該測量目標對應的運動狀態測量值作為該目標的初始運動狀態。
本發明屬于傳感器融合技術領域,具體涉及一種智能汽車的目標級多傳感器融合方法;將智能汽車配備的多個傳感器對目標的目標級檢測信息進行時空配準,即目標的位置、速度、加速度信息,通過對多傳感器檢測到的所有目標進行全局跟蹤,并對重復檢測到的目標進行融合,使融合中心能夠得到各個傳感器檢測范圍內的所有目標的連續運動軌跡,再對全體目標進行航跡管理,篩除航跡管理后本周期所有目標中的備選新目標,即航跡生成判斷還沒完成的目標,將其余目標及其運動狀態作為本周期融合中心最終輸出的多傳感器融合的障礙物檢測結果。本發明可實現對車輛周圍目標進行穩定、準確地檢測和跟蹤,并且可避免對重疊檢測區域的目標重復檢測等問題。
無人駕駛車輛運營管理系統、總控平臺、分控平臺、車載計算裝置和計算機可讀存儲介質的制造方法