[發明專利]一種主動標定激光雷達和編碼器外參的方法

有效

• 申請號: 202110081298.3
• 申請日: 2021-01-21
• 公開（公告）號: CN112882053B
• 公開（公告）日: 2023-07-18
• 發明（設計）人: 梁斌;易彤;蘭斌;王學謙
• 申請（專利權）人: 清華大學深圳國際研究生院
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01S7/497;G01C25/00
• 代理公司: 深圳新創友知識產權代理有限公司 44223
• 代理人: 江耀純

1.一種主動標定激光雷達和編碼器外參的方法，其特征在于，包括：

S1、將標定物和激光雷達置于同一水平面，并以編碼器里程計的坐標系作為移動機器人的坐標系；指定標定物在世界坐標系中的位置和移動機器人的起始位置，根據環境設定觀測噪聲和運動噪聲，并初始化移動機器人的狀態變量和狀態變量的協方差矩陣；

S2、利用軌跡規劃算法計算出所述移動機器人的線速度和角速度；

S3、使所述移動機器人以步驟S2計算出的線速度和角速度運動一個時間間隔Δt，利用擴展卡爾曼濾波進行狀態變量及其協方差矩陣的更新；

S4、重復步驟S2和S3，直至狀態變量收斂，完成標定；

所述狀態變量為：

其中，k表示離散狀態下的k時刻，x_r(k)＝[x(k),y(k),θ(k)]^T表示移動機器人k時刻所處的狀態，上標T表示矩陣的轉置，由于移動機器人在二維平面運動，因此z方向上的平移量以及繞x軸、y軸的角度都為0，其中x(k),y(k)表征移動機器人坐標系原點在世界坐標系中的位置，θ(k)表示移動機器人坐標系相對于世界坐標系繞z軸旋轉的角度；

x_c(k)＝[Δx(k),Δy(k),Δθ(k)]^T為標定參數，Δx(k),Δy(k)表征激光雷達坐標系原點在移動機器人坐標系中的位置，Δθ(k)表征激光雷達坐標系與移動機器人坐標系之間的角度偏移；

步驟S2包括：

首先，給出移動機器人在離散時間狀態下的非線性運動模型和觀測模型，分別為式(2)和式(3)：

z(k)＝h[X(k),n(k)]??????(3)

其中：

其中，m(k)是運動噪聲，n(k)是觀測噪聲，兩者相互獨立且均服從零均值的高斯分布；

u(k)＝[v(k),ω(k)]^T，v(k)和ω(k)分別表示移動機器人在k時刻的線速度和角速度，Δt表示離散狀態下的時間間隔；

其中，r(k)表示激光雷達坐標系原點距離標定物的距離，b(k)表示標定物在激光雷達坐標系下的角度；

A(k)＝l_x(k)-x(k)-Δx(k)·cos(θ(k))+Δy(k)·sin(θ(k))?????(6)

B(k)＝l_y(k)-y(k)-Δx(k)·sin(θ(k))+Δy(k)·cos(θ(k))?????(7)

其中，l_x(k),l_y(k)表示標定物在世界坐標系中的位置；

根據k時刻的狀態變量及其協方差矩陣，利用擴展卡爾曼濾波的方法估計出X(k+1)的估計值，在將式(4)代入式(2)之后，方程中僅線速度v(k)和角速度ω(k)為未知量，另外由于協方差矩陣的跡是線速度和角速度的非線性函數，此時再結合k時刻協方差矩陣的跡的表達式，即可求解得到k時刻的線速度v(k)和角速度ω(k)。

2.如權利要求1所述的主動標定激光雷達和編碼器外參的方法，其特征在于，步驟S3包括：

將步驟S2求解得到的線速度v(k)和角速度ω(k)作為輸入，控制移動機器人以線速度v(k)和角速度ω(k)運動一個時間間隔Δt，進入k+1時刻；

利用擴展卡爾曼濾波的方法求解出k+1時刻的標定參數Δx(k+1)、Δy(k+1)和Δθ(k+1)，并更新狀態變量及其協方差矩陣。