[發(fā)明專(zhuān)利]一種基于車(chē)聯(lián)網(wǎng)的多車(chē)聯(lián)合絕對(duì)定位的方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010141079.5
• 申請(qǐng)日: 2020-03-04
• 公開(kāi)（公告）號(hào): CN111288984B
• 公開(kāi)（公告）日: 2021-12-14
• 發(fā)明（設(shè)計(jì)）人: 錢(qián)闖;張紅娟;唐健;李文卓;溫景仁;劉暉
• 申請(qǐng)（專(zhuān)利權(quán)）人: 武漢大學(xué)
• 主分類(lèi)號(hào): G01C21/00
• 分類(lèi)號(hào): G01C21/00;G01S17/86;G01S19/47;G06T7/246;G06T7/277;G06T7/73
• 代理公司: 武漢宇晨專(zhuān)利事務(wù)所(普通合伙) 42001
• 代理人: 王敏鋒

1.一種基于車(chē)聯(lián)網(wǎng)的多車(chē)聯(lián)合絕對(duì)定位的方法，其特征在于，包括以下步驟：

步驟1，各輛車(chē)根據(jù)當(dāng)前GNSS的定位結(jié)果與INS進(jìn)行EKF松組合獲取當(dāng)前歷元k的導(dǎo)航信息包括位置姿態(tài)速度其中n表示導(dǎo)航系，b表示INS坐標(biāo)系，姿態(tài)表示b系相對(duì)于n系的方向余弦矩，k表示卡爾曼濾波器中第k個(gè)濾波歷元；

步驟2.將車(chē)輛上激光雷達(dá)采集的靜態(tài)參照物的點(diǎn)云數(shù)據(jù)進(jìn)行坐標(biāo)轉(zhuǎn)換，使當(dāng)前歷元點(diǎn)云觀測(cè)按當(dāng)前幀點(diǎn)云觀測(cè)時(shí)相對(duì)于前一歷元占有似然圖的激光雷達(dá)三維位置和姿態(tài)角更新前一歷元占有似然圖中柵格占有概率，形成概率擬合像，即當(dāng)前歷元的占有似然圖，從而將b系的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到n系下占有似然圖中；

所述占有似然圖由q層虛擬柵格平面組成，每層虛擬柵格平面是由q×q個(gè)大小相同的虛擬柵格組成正方形平面，所述柵格能根據(jù)柵格占有概率更新規(guī)則形成點(diǎn)云數(shù)據(jù)的概率擬合像；

步驟2包括以下步驟：

步驟2.1.根據(jù)標(biāo)定好的LiDAR相對(duì)于INS的安裝角將LiDAR點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到INS坐標(biāo)系b系下；

步驟2.2.生成占有似然圖，所述占有似然圖的范圍為400×400×400m，并具有以下四級(jí)網(wǎng)格：第一級(jí)柵格的分辨率為5×5×5cm，第二級(jí)柵格的分辨率為10×10×10cm，第三級(jí)柵格的分辨率為20×20×20cm，第四級(jí)柵格的分辨率為40×40×40cm；

設(shè)置占有似然圖中柵格占有概率更新規(guī)則為：

初始化各個(gè)占有似然圖的初始占有概率為0并存儲(chǔ)，設(shè)此時(shí)的占有似然圖為M₀；

激光腳點(diǎn)所在的柵格為中心柵格，中心柵格、非中心柵格的占有概率value按下式計(jì)算：

其中abs(DX)、abs(DY)占有似然圖XOY平面上為非中心柵格在占有似然圖XOY平面的X和Y方向距離中心柵格的柵格個(gè)數(shù)，abs(DZ)為占有似然圖ZOY平面上非中心柵格在占有似然圖ZOY平面的Z方向距離中心柵格的柵格個(gè)數(shù)，Γ表示立面位置系數(shù)；

比較計(jì)算得到的各柵格占有概率于已存的各柵格占有概率的大小，按下式更新各柵格占有概率：

value₀為該柵格已存在的柵格占有概率值；

步驟2.3.第一幀點(diǎn)云Scan₁觀測(cè)時(shí)，設(shè)激光雷達(dá)中心點(diǎn)的三維位置p_x,p_y,p_z設(shè)為(0，0，0)，設(shè)其相對(duì)于占有似然圖的旋轉(zhuǎn)余弦矩陣為為根據(jù)步驟1求得的第一幀點(diǎn)云觀測(cè)時(shí)的余弦矩陣，基于三維位置和余弦矩陣將觀測(cè)的點(diǎn)云進(jìn)行坐標(biāo)轉(zhuǎn)換，坐標(biāo)轉(zhuǎn)換的公式如下：

其中[laser_x laser_y laser_z]^T為轉(zhuǎn)換前的腳點(diǎn)坐標(biāo)，為轉(zhuǎn)換后的腳點(diǎn)坐標(biāo)，根據(jù)轉(zhuǎn)換后激光腳點(diǎn)的坐標(biāo)找到對(duì)應(yīng)的柵格，并根據(jù)步驟2.2中的更新規(guī)則對(duì)步驟2.2生成的占有似然圖中各柵格占有概率進(jìn)行更新，得到M₁；

步驟2.4.利用高斯牛頓迭代法將下一幀點(diǎn)云Scan₂與M₁進(jìn)行scan-to-map匹配獲得點(diǎn)云觀測(cè)時(shí)的激光雷達(dá)三維位置與相對(duì)于占有似然圖旋轉(zhuǎn)的姿態(tài)角；

高斯牛頓法實(shí)現(xiàn)scan-to-map匹配的準(zhǔn)則如下：

為Scan₂觀測(cè)時(shí)，激光雷達(dá)的中心點(diǎn)的三維位置和余弦矩陣對(duì)應(yīng)的姿態(tài)角向量的初值，其三維位置p_x,p_y,p_z的初值設(shè)為上一幀點(diǎn)云Scan₁觀測(cè)時(shí)的位置，橫滾角θ、俯仰角γ和航向角的初值通過(guò)IMU的機(jī)械編排從上一幀點(diǎn)云Scan₁觀測(cè)時(shí)的姿態(tài)角來(lái)推算獲取，△T為相對(duì)于初始位置和初始姿態(tài)角的改正數(shù)，其中i為Scan₂的第i個(gè)激光腳點(diǎn)，為點(diǎn)云中包含的點(diǎn)數(shù)，t為點(diǎn)云幀觀測(cè)的歷元，S(T)為激光腳點(diǎn)基于T進(jìn)行坐標(biāo)轉(zhuǎn)換，M(S)為轉(zhuǎn)換后坐標(biāo)所占柵格的柵格占有概率，根據(jù)高斯牛頓法求解有：

其中為在所占柵格處的梯度，為在T處的導(dǎo)數(shù)，則匹配得到的三維位置和姿態(tài)角為T(mén)^*＝T+△T；

scan-to-map匹配的過(guò)程中按照網(wǎng)格級(jí)數(shù)從后向前逐級(jí)匹配，后一級(jí)柵格匹配結(jié)果作為前一級(jí)柵格的初始值，直到第一級(jí)柵格，即從低分辨率網(wǎng)格匹配開(kāi)始逐步到與高分辨率網(wǎng)格，低分辨率網(wǎng)格匹配的結(jié)果作為高分辨率網(wǎng)格匹配的初始值直到最高分辨率網(wǎng)格；

步驟2.5.根據(jù)步驟2.4的匹配獲得的三維位置和姿態(tài)角根據(jù)步驟2.4的匹配獲得的三維位置和姿態(tài)角，將姿態(tài)角轉(zhuǎn)換為余弦矩陣，轉(zhuǎn)換公式為：再根據(jù)步驟2.3的坐標(biāo)轉(zhuǎn)換公式將Scan₂進(jìn)行坐標(biāo)轉(zhuǎn)換，進(jìn)而根據(jù)轉(zhuǎn)換后的腳點(diǎn)坐標(biāo)所占的柵格位置，對(duì)M₁中柵格的占有概率進(jìn)行更新，得到M₂；

步驟2.6.重復(fù)步驟2.4～步驟2.5迭代，利用后續(xù)的點(diǎn)云幀數(shù)據(jù)對(duì)占有似然圖進(jìn)行更新；

步驟3.根據(jù)步驟2中得到的車(chē)輛及周?chē)?lián)網(wǎng)車(chē)輛的點(diǎn)云觀測(cè)相對(duì)各自車(chē)輛的占有似然圖的激光雷達(dá)三維位置和姿態(tài)角，將車(chē)輛當(dāng)前的點(diǎn)云幀分別與周?chē)?lián)網(wǎng)車(chē)輛存儲(chǔ)的占有似然圖依據(jù)進(jìn)行匹配，從而獲取車(chē)輛點(diǎn)云觀測(cè)相對(duì)周?chē)?lián)網(wǎng)車(chē)輛的占有似然圖的激光雷達(dá)三維位置和姿態(tài)角，從而獲取車(chē)輛與周?chē)?lián)網(wǎng)車(chē)輛的在n系下的相對(duì)位置；

所述車(chē)輛與周?chē)?lián)網(wǎng)車(chē)輛通過(guò)短程通信進(jìn)行通信連接，短程通信距離不大于所述激光雷達(dá)每次掃描的點(diǎn)云觀測(cè)的畫(huà)面橫向距離，步驟3中的車(chē)輛及周?chē)?lián)網(wǎng)車(chē)輛的點(diǎn)云觀測(cè)是各車(chē)輛的激光雷達(dá)按相同頻率從初始掃描方向角開(kāi)始沿相同旋轉(zhuǎn)方向旋轉(zhuǎn)掃描的同一歷元的點(diǎn)云觀測(cè)，所述旋轉(zhuǎn)方向即激光雷達(dá)沿順時(shí)針或逆時(shí)針對(duì)車(chē)輛四周360°掃描；

步驟3包括以下步驟：

車(chē)輛c1觀測(cè)到的當(dāng)前點(diǎn)云幀為其存儲(chǔ)的占有似然圖為其對(duì)應(yīng)的坐標(biāo)系為n1系,從步驟2中獲取的車(chē)輛c1當(dāng)前歷元的點(diǎn)云幀的激光雷達(dá)三維位置和姿態(tài)角信息為T(mén)^*c1為n1系下的位姿，同樣的，周?chē)?lián)網(wǎng)車(chē)輛c2觀測(cè)到的當(dāng)前點(diǎn)云幀為其存儲(chǔ)的OLM占有似然圖為其對(duì)應(yīng)的坐標(biāo)系為n2系,從步驟2中獲取的周?chē)?lián)網(wǎng)車(chē)輛c2當(dāng)前歷元的點(diǎn)云幀的激光雷達(dá)三維位置和姿態(tài)角信息為T(mén)^*c2，根據(jù)和T^*c1，基于步驟2.4中的高斯牛頓的scan-to-map匹配方法獲取到周?chē)?lián)網(wǎng)車(chē)輛c2當(dāng)前歷元下在n1系下的激光雷達(dá)三維位置和姿態(tài)角信息從而獲取當(dāng)前歷元下車(chē)輛c1和周?chē)?lián)網(wǎng)車(chē)輛c2在n系下的相對(duì)位置：

根據(jù)和T^*c2，基于步驟2.4中的高斯牛頓的scan-to-map匹配方法獲取到車(chē)輛c1當(dāng)前歷元下在n2系下的激光雷達(dá)三維位置和姿態(tài)角信息T^*c2-c1，從而獲取車(chē)輛c1當(dāng)前歷元下和周?chē)?lián)網(wǎng)車(chē)輛c2在n系下的相對(duì)位置對(duì)兩次求得的車(chē)輛c1和周?chē)?lián)網(wǎng)車(chē)輛c2在n系下的相對(duì)位置取平均值更新車(chē)輛c1和周?chē)?lián)網(wǎng)車(chē)輛c2的在n系下的相對(duì)位置為：

步驟4.根據(jù)兩兩組隊(duì)的車(chē)輛的GNSS觀測(cè)數(shù)據(jù)和步驟3獲得的車(chē)輛與周?chē)?lián)網(wǎng)車(chē)輛的在n系下的相對(duì)位置，利用GNSS RTK的方法解算兩兩組隊(duì)的車(chē)輛間的相對(duì)模糊度

步驟5.將當(dāng)前歷元所有參與解算的聯(lián)網(wǎng)車(chē)輛的GNSS觀測(cè)，與附近的基準(zhǔn)站ref數(shù)據(jù)構(gòu)成聯(lián)合RTK雙差觀測(cè)方程，將步驟3計(jì)算的車(chē)輛間相對(duì)位置加入到聯(lián)合RTK觀測(cè)方程中，再將步驟4計(jì)算車(chē)輛間的相對(duì)模糊度作為約束條件，利用附加約束條件的最小二乘的方法對(duì)模糊度浮點(diǎn)解進(jìn)行求解，然后利用LAMBDA搜索整數(shù)模糊度，獲取模糊度固定解及相應(yīng)的位置固定解，從而解算所有參與解算的聯(lián)網(wǎng)車(chē)輛當(dāng)前歷元的絕對(duì)位置；

步驟6.將步驟5中得到的車(chē)輛的絕對(duì)位置重復(fù)步驟1與INS進(jìn)行EKF松組合，獲取第K個(gè)歷元下的車(chē)輛的組合導(dǎo)航信息，包括位置姿態(tài)速度