[发明专利]一种主动标定激光雷达和编码器外参的方法

权利要求书

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)，并更新状态变量及其协方差矩阵。