[發(fā)明專利]一種基于多傳感器融合的定位方法及裝置

在審

• 申請(qǐng)?zhí)枺?/td>: 202010686900.1
• 申請(qǐng)日: 2020-07-16
• 公開(kāi)（公告）號(hào): CN113945206A
• 公開(kāi)（公告）日: 2022-01-18
• 發(fā)明（設(shè)計(jì)）人: 潘亮亮;趙季;季開(kāi)進(jìn)
• 申請(qǐng)（專利權(quán)）人: 北京圖森未來(lái)科技有限公司
• 主分類號(hào): G01C21/00
• 分類號(hào): G01C21/00;G01C21/16;G01S17/86
• 代理公司: 暫無(wú)信息
• 代理人: 暫無(wú)信息

1.一種基于多傳感器融合的定位方法，其特征在于，應(yīng)用于一種搭載有多種傳感器的可移動(dòng)物體上，所述方法，包括：

實(shí)時(shí)獲得可移動(dòng)物體上搭載的各種傳感器采集的傳感器數(shù)據(jù)；

對(duì)各種傳感器采集的傳感器數(shù)據(jù)進(jìn)行時(shí)間空間同步，形成時(shí)間空間同步后的各種傳感器數(shù)據(jù)；

將進(jìn)行時(shí)間空間同步后的各種傳感器數(shù)據(jù)進(jìn)行數(shù)據(jù)預(yù)處理和數(shù)據(jù)關(guān)聯(lián)，形成用于聯(lián)合優(yōu)化的傳感器數(shù)據(jù)；

獲得預(yù)先設(shè)置的滑動(dòng)窗口中在當(dāng)前時(shí)刻之前的各時(shí)刻的狀態(tài)信息；

根據(jù)所述用于聯(lián)合優(yōu)化的傳感器數(shù)據(jù)和滑動(dòng)窗口中在當(dāng)前時(shí)刻之前的各時(shí)刻的狀態(tài)信息，進(jìn)行聯(lián)合優(yōu)化求解，確定可移動(dòng)物體的當(dāng)前位姿狀態(tài)。

2.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述各種傳感器包括慣性測(cè)量單元IMU、輪速計(jì)和激光雷達(dá)；其中，所述IMU包括加速度計(jì)和陀螺儀。

3.根據(jù)權(quán)利要求2所述的方法，其特征在于，所述狀態(tài)信息為狀態(tài)變量，所述狀態(tài)變量包括平移向量、速度向量、旋轉(zhuǎn)矩陣、IMU中加速度計(jì)偏置和IMU中陀螺儀偏置；

所述獲得預(yù)先設(shè)置的滑動(dòng)窗口中在當(dāng)前時(shí)刻之前的各時(shí)刻的狀態(tài)信息，包括：

在用于聯(lián)合優(yōu)化的傳感器數(shù)據(jù)未經(jīng)過(guò)初始化時(shí)，實(shí)時(shí)確定預(yù)先設(shè)置的滑動(dòng)窗口中的數(shù)據(jù)是否滿足初始化條件；所述初始化條件包括所述滑動(dòng)窗口中的數(shù)據(jù)觀測(cè)時(shí)刻數(shù)大于等于預(yù)先設(shè)置的數(shù)量閾值，且所述滑動(dòng)窗口中的IMU數(shù)據(jù)充分激勵(lì)；

在所述滑動(dòng)窗口中的數(shù)據(jù)觀測(cè)時(shí)刻數(shù)大于等于預(yù)先設(shè)置的數(shù)量閾值，且所述滑動(dòng)窗口中的IMU數(shù)據(jù)充分激勵(lì)時(shí)，對(duì)狀態(tài)變量進(jìn)行初始化，形成初始化狀態(tài)變量；

在對(duì)狀態(tài)變量進(jìn)行初始化完成后，且當(dāng)前時(shí)刻之前的上一時(shí)刻進(jìn)行聯(lián)合優(yōu)化求解結(jié)束后，獲得當(dāng)前時(shí)刻之前的各時(shí)刻的聯(lián)合優(yōu)化后的當(dāng)前狀態(tài)變量。

4.根據(jù)權(quán)利要求3所述的方法，其特征在于，所述實(shí)時(shí)獲得可移動(dòng)物體上搭載的各種傳感器采集的傳感器數(shù)據(jù)，包括：

實(shí)時(shí)獲得IMU測(cè)量的IMU數(shù)據(jù)、輪速計(jì)測(cè)量的可移動(dòng)物體輪速數(shù)據(jù)和激光雷達(dá)測(cè)量的點(diǎn)云數(shù)據(jù)。

5.根據(jù)權(quán)利要求4所述的方法，其特征在于，對(duì)各種傳感器采集的傳感器數(shù)據(jù)進(jìn)行時(shí)間空間同步，形成時(shí)間空間同步后的各種傳感器數(shù)據(jù)，包括：

根據(jù)IMU、輪速計(jì)和激光雷達(dá)之間預(yù)先標(biāo)定的外參，將可移動(dòng)物體輪速數(shù)據(jù)和點(diǎn)云數(shù)據(jù)對(duì)齊到IMU坐標(biāo)系下，并根據(jù)IMU、輪速計(jì)和激光雷達(dá)各自的數(shù)據(jù)采集幀率，將可移動(dòng)物體輪速數(shù)據(jù)和點(diǎn)云數(shù)據(jù)的采集時(shí)的每幀與IMU的時(shí)間戳對(duì)齊，形成IMU坐標(biāo)系下的，時(shí)間戳對(duì)齊的IMU數(shù)據(jù)、可移動(dòng)物體輪速數(shù)據(jù)和點(diǎn)云數(shù)據(jù)。

6.根據(jù)權(quán)利要求4所述的方法，其特征在于，將進(jìn)行時(shí)間空間同步后的各種傳感器數(shù)據(jù)進(jìn)行數(shù)據(jù)預(yù)處理和數(shù)據(jù)關(guān)聯(lián)，形成用于聯(lián)合優(yōu)化的傳感器數(shù)據(jù)，包括：

將進(jìn)行時(shí)間空間同步后的IMU數(shù)據(jù)中處于連續(xù)兩幀激光雷達(dá)時(shí)間戳之間的IMU數(shù)據(jù)使用預(yù)先設(shè)置的積分算法進(jìn)行傳播，并通過(guò)預(yù)先設(shè)置的預(yù)積分方法進(jìn)行處理，得到位移預(yù)積分量、速度預(yù)積分量和旋轉(zhuǎn)增量預(yù)積分量；

根據(jù)當(dāng)前時(shí)刻的上一時(shí)刻在預(yù)先設(shè)置的滑動(dòng)窗口中聯(lián)合優(yōu)化后的狀態(tài)變量，以及進(jìn)行時(shí)間空間同步后的IMU數(shù)據(jù)進(jìn)行下一時(shí)刻的狀態(tài)變量預(yù)測(cè)，得到下一時(shí)刻的狀態(tài)變量預(yù)測(cè)數(shù)據(jù)；

將進(jìn)行時(shí)間空間同步后的點(diǎn)云數(shù)據(jù)，采用下一時(shí)刻的狀態(tài)變量預(yù)測(cè)數(shù)據(jù)進(jìn)行去畸變處理；

對(duì)去畸變處理后的點(diǎn)云數(shù)據(jù)進(jìn)行線面特征提取，獲得線特征數(shù)據(jù)和面特征數(shù)據(jù)；

將所述線特征數(shù)據(jù)和面特征數(shù)據(jù)與預(yù)先生成的特征地圖上的線特征和面特征分別進(jìn)行配準(zhǔn)，得到線特征約束關(guān)系數(shù)據(jù)和面特征約束關(guān)系數(shù)據(jù)；

將進(jìn)行時(shí)間空間同步后的可移動(dòng)物體輪速數(shù)據(jù)，通過(guò)預(yù)先設(shè)置的車輛動(dòng)力學(xué)模型，確定在IMU坐標(biāo)系下前進(jìn)方向線速度、水平方向線速度和偏航角速度。