[發(fā)明專利]一種機(jī)器人定位方法和系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201811366331.1
• 申請(qǐng)日: 2018-11-16
• 公開(kāi)（公告）號(hào): CN109405826B
• 公開(kāi)（公告）日: 2020-12-29
• 發(fā)明（設(shè)計(jì)）人: 董又維;徐江偉;李劍平;王浩然;李偉
• 申請(qǐng)（專利權(quán)）人: 北京兆維電子（集團(tuán)）有限責(zé)任公司
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 北京輕創(chuàng)知識(shí)產(chǎn)權(quán)代理有限公司 11212
• 代理人: 徐琪琦

1.一種機(jī)器人定位方法，其特征在于，包括以下步驟：

S1：獲取待測(cè)物體在當(dāng)前測(cè)量間隔內(nèi)包括時(shí)間戳的激光數(shù)據(jù)和n組包括時(shí)間戳的初始IMU實(shí)時(shí)數(shù)據(jù)，n＝1，2，3…；

S2：分別將每組所述初始IMU實(shí)時(shí)數(shù)據(jù)減去預(yù)設(shè)值得到n組IMU實(shí)時(shí)數(shù)據(jù)并保存；

S3：將所述激光數(shù)據(jù)的時(shí)間戳映射到每組IMU數(shù)據(jù)的時(shí)間戳的線性時(shí)間軸上；

S4：按照線性關(guān)系計(jì)算所述激光數(shù)據(jù)時(shí)間戳投影點(diǎn)對(duì)應(yīng)的IMU數(shù)據(jù)，并作為最終IMU數(shù)據(jù)；

S5：根據(jù)所述最終IMU數(shù)據(jù)與上一測(cè)量間隔內(nèi)計(jì)算得到的最終IMU數(shù)據(jù)計(jì)算得到所述待測(cè)物體的底盤(pán)姿態(tài)變化矩陣；

S6：根據(jù)所述底盤(pán)姿態(tài)變化矩陣得到激光坐標(biāo)系姿態(tài)變化矩陣；

S7：根據(jù)所述激光坐標(biāo)系姿態(tài)變化矩陣和激光匹配算法得到定位結(jié)果。

2.根據(jù)權(quán)利要求1所述的機(jī)器人定位方法，其特征在于：步驟S1具體為：獲取待測(cè)物體在當(dāng)前測(cè)量間隔內(nèi)包括時(shí)間戳的激光數(shù)據(jù)和3組包括時(shí)間戳的初始IMU實(shí)時(shí)數(shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的機(jī)器人定位方法，其特征在于：步驟S4中，根據(jù)以下公式得到最終IMU數(shù)據(jù)：

當(dāng)timestamp_scan＝timestamp_imu_2時(shí)，curr_angle＝angle_1+(angle_2–angle_1)*(timestamp_scan-timestamp_imu_1)/(timestamp_imu_2-timestamp_imu_1)；

當(dāng)timestamp_scantimestamp_imu_2時(shí)，curr_angle＝angle_2+(angle_3–angle_2)*(timestamp_scan-timestamp_imu_2)/(timestamp_imu_3-timestamp_imu_2)；

其中，timestamp_imu_1、timestamp_imu_2、timestamp_imu_3依次代表3個(gè)按時(shí)間先后排序的IMU實(shí)時(shí)數(shù)據(jù)，所述IMU實(shí)時(shí)數(shù)據(jù)包括朝向角度值，angle_1為timestamp_imu_1對(duì)應(yīng)的朝向角度值，angle_2為timestamp_imu_2對(duì)應(yīng)的朝向角度值，angle_3為timestamp_imu_3對(duì)應(yīng)的朝向角度值，timestamp_scan為所述激光數(shù)據(jù)時(shí)間戳，curr_angle為最終IMU數(shù)據(jù)的角度值，然后將curr_angle轉(zhuǎn)換為四元數(shù)形式保存在IMU數(shù)據(jù)中得到最終IMU數(shù)據(jù)。

4.根據(jù)權(quán)利要求1所述的機(jī)器人定位方法，其特征在于：步驟S5中，具體包括：如果所述最終IMU數(shù)據(jù)與上一組IMU實(shí)時(shí)數(shù)據(jù)的差值Δangle大于等于π，則Δangle＝Δangle-sign(Δangle)*2π，然后將Δangle轉(zhuǎn)換為四元數(shù)形式，然后將四元數(shù)形式的Δangle轉(zhuǎn)換為tf矩陣ΔTFbase。

5.根據(jù)權(quán)利要求4所述的機(jī)器人定位方法，其特征在于：步驟S6中的激光坐標(biāo)系姿態(tài)變化矩陣根據(jù)以下公式計(jì)算獲得：ΔTFlaser＝TFlaser-base*TFbase-world*ΔTFbase*TFworld-base*TFbase-laser；其中TFlaser-base為機(jī)器人底盤(pán)坐標(biāo)系相對(duì)于激光坐標(biāo)系的相對(duì)位姿，TFbase-world為世界坐標(biāo)系相對(duì)于機(jī)器人底盤(pán)坐標(biāo)系的相對(duì)位姿，TFworld-base為機(jī)器人底盤(pán)坐標(biāo)系相對(duì)于世界坐標(biāo)系的相對(duì)位姿，TFbase-laser為激光坐標(biāo)系相對(duì)于機(jī)器人底盤(pán)坐標(biāo)系的相對(duì)位姿。