[發(fā)明專利]一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201911424943.6
• 申請(qǐng)日: 2019-12-30
• 公開(kāi)（公告）號(hào): CN110954100A
• 公開(kāi)（公告）日: 2020-04-03
• 發(fā)明（設(shè)計(jì)）人: 蘇澤榮;周雪峰;唐觀榮;吳鴻敏;鄢武
• 申請(qǐng)（專利權(quán)）人: 廣東省智能制造研究所
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 佛山市廣盈專利商標(biāo)事務(wù)所(普通合伙) 44339
• 代理人: 李俊

1.一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述方法包括：

采集足式機(jī)器人本體的IMU數(shù)據(jù)以及激光數(shù)據(jù)；

對(duì)所述IMU數(shù)據(jù)與所述激光數(shù)據(jù)進(jìn)行時(shí)間戳同步與預(yù)處理；

在所述時(shí)間戳同步與預(yù)處理之后，對(duì)所述IMU數(shù)據(jù)進(jìn)行積分，得到積分?jǐn)?shù)據(jù)，同時(shí)對(duì)所述激光數(shù)據(jù)進(jìn)行幀間點(diǎn)線匹配，得到增量運(yùn)動(dòng)變化；

將所述積分?jǐn)?shù)據(jù)作為狀態(tài)預(yù)測(cè)輸入至遞歸貝葉斯濾波器，且將所述得到的增量運(yùn)動(dòng)變化作為狀態(tài)校正與更新輸入至所述遞歸貝葉斯濾波器；

所述遞歸貝葉斯濾波器通過(guò)對(duì)比所述狀態(tài)預(yù)測(cè)所輸入的數(shù)據(jù)與所述狀態(tài)校正與更新所輸入的數(shù)據(jù)來(lái)進(jìn)行誤差測(cè)量，得到所述足式機(jī)器人本體運(yùn)動(dòng)狀態(tài)。

2.根據(jù)權(quán)利要求1所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述采集足式機(jī)器人本體的IMU數(shù)據(jù)以及激光數(shù)據(jù)包括：

通過(guò)IMU傳感器中的加速度計(jì)采集足式機(jī)器人本體的線性加速度；

通過(guò)IMU傳感器中的陀螺儀采集足式機(jī)器人本體的角速度；

通過(guò)激光雷達(dá)采集足式機(jī)器人本體的激光數(shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述對(duì)所述IMU數(shù)據(jù)與所述激光數(shù)據(jù)進(jìn)行時(shí)間戳同步與預(yù)處理包括：

對(duì)所述IMU數(shù)據(jù)與所述激光數(shù)據(jù)進(jìn)行時(shí)間戳同步，得到同步的IMU數(shù)據(jù)和激光數(shù)據(jù)；

基于所述IMU數(shù)據(jù)和所述激光數(shù)據(jù)，對(duì)所述IMU數(shù)據(jù)與所述激光數(shù)據(jù)進(jìn)行預(yù)處理，其中，所述預(yù)處理包括將所述IMU數(shù)據(jù)和所述激光數(shù)據(jù)進(jìn)行去噪，以及將所述IMU數(shù)據(jù)和所述激光數(shù)據(jù)進(jìn)行坐標(biāo)系轉(zhuǎn)化。

4.根據(jù)權(quán)利要求3所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述將所述IMU數(shù)據(jù)進(jìn)行坐標(biāo)系轉(zhuǎn)化包括：

基于所述加速度計(jì)所采集的線性加速度和所述陀螺儀所采集的角速度，在IMU坐標(biāo)系中，使用適當(dāng)?shù)男D(zhuǎn)和平移將所述IMU坐標(biāo)系轉(zhuǎn)化為足式機(jī)器人本體的坐標(biāo)系。

5.根據(jù)權(quán)利要求1所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述對(duì)所述IMU數(shù)據(jù)進(jìn)行積分，得到積分?jǐn)?shù)據(jù)包括：

對(duì)所述IMU數(shù)據(jù)中的線性加速度進(jìn)行一次積分和二次積分，分別得到速度和位置；

對(duì)所述IMU數(shù)據(jù)中的角速度進(jìn)行一次積分得到角度，并將所述角度進(jìn)行四元數(shù)轉(zhuǎn)化，得到質(zhì)心處由所述四元數(shù)表示的姿態(tài)信息。

6.根據(jù)權(quán)利要求1所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述對(duì)所述激光數(shù)據(jù)進(jìn)行幀間點(diǎn)線匹配，得到增量運(yùn)動(dòng)變化包括：

基于點(diǎn)線ICP中的PLICP算法，對(duì)所述激光掃描數(shù)據(jù)進(jìn)行幀間匹配；

基于所述幀間匹配的結(jié)果，通過(guò)計(jì)算得到增量運(yùn)動(dòng)變化。

7.根據(jù)權(quán)利要求6所述的一種基于激光與慣導(dǎo)融合的足式機(jī)器人本體狀態(tài)估計(jì)方法，其特征在于，所述基于點(diǎn)線ICP中的PLICP算法，對(duì)所述激光掃描數(shù)據(jù)進(jìn)行幀間匹配包括：

在兩點(diǎn)集的位姿變換估計(jì)下，將待匹配掃描點(diǎn)p_j集映射到參考掃描點(diǎn)集所在的坐標(biāo)系中，并記為映射的轉(zhuǎn)換關(guān)系具體計(jì)算公式如下：

其中，p_j為待匹配掃描點(diǎn)，為p_j映射到參考掃描點(diǎn)集坐標(biāo)系下的點(diǎn)，Q_n＝(R_n，T_n)表示兩組點(diǎn)集的位姿變換估計(jì)值，R_n和T_n分別表示兩組點(diǎn)集位姿變化估計(jì)的旋轉(zhuǎn)變換矩陣和平移變換矩陣；

對(duì)于所述每個(gè)點(diǎn)均在所述參考掃描點(diǎn)集中找到距離最近的兩個(gè)點(diǎn)的索引和

基于所述距離最近的兩個(gè)點(diǎn)的索引和用一個(gè)元組進(jìn)行表示點(diǎn)p_j到線段相匹配。