CN112882053B

CN112882053B - 一种主动标定激光雷达和编码器外参的方法

Info

Publication number: CN112882053B
Application number: CN202110081298.3A
Authority: CN
Inventors: 梁斌; 易彤; 兰斌; 王学谦
Original assignee: Shenzhen International Graduate School of Tsinghua University
Current assignee: Shenzhen International Graduate School of Tsinghua University
Priority date: 2021-01-21
Filing date: 2021-01-21
Publication date: 2023-07-18
Anticipated expiration: 2041-01-21
Also published as: CN112882053A

Abstract

本发明公开了一种主动标定激光雷达和编码器外参的方法，包括：S1、将标定物和激光雷达置于同一水平面，并以编码器里程计的坐标系作为移动机器人的坐标系；指定标定物在世界坐标系中的位置和移动机器人的起始位置，根据环境设定观测噪声和运动噪声，并初始化移动机器人的状态变量和状态变量的协方差矩阵；S2、利用轨迹规划算法计算出所述移动机器人的线速度和角速度；S3、使所述移动机器人以步骤S2计算出的线速度和角速度运动一个时间间隔Δt，利用扩展卡尔曼滤波进行状态变量及其协方差矩阵的更新；S4、重复步骤S2和S3，直至状态变量收敛，完成标定。

Description

一种主动标定激光雷达和编码器外参的方法

技术领域

本发明涉及机器人技术领域，具体涉及一种主动标定激光雷达和编码器外参的方法。

背景技术

由于装有多传感器的自主导航移动机器人具有高效性和更高的精度，引起了全球学界的关注。激光雷达和编码器是移动机器人中最常用的两个传感器，特别是在工业和民用领域的轮式机器人中，如自主导航车，无人车和轮式服务机器人等。与基于惯性测量单元的里程计相比，安装在移动机器人驱动电机上的编码器可以为机器人提供更准确、更直接的里程计数据。但是，当移动机器人上下坡或车轮打滑时，编码器会出现严重的漂移。而激光雷达可以提供周围环境丰富的场景信息，这些信息可以用来估计机器人的状态和检测环境中的物体。然而，受到成本和技术的限制，大多数的移动机器人采用的是单一的2D激光雷达，在复杂环境中，由于其无法感知三维环境信息的缺点，会出现定位精度低，建图不完整等问题。因此编码器和2D激光雷达可以弥补各自的缺点。例如激光雷达的重定位能力有助于减少编码器里程计的漂移问题，基于编码器里程计提供的精确的近距离变换，能够提升激光雷达数据关联的鲁棒性。并且基于编码器和2D激光雷达融合的里程计可以提升对环境的感知和自身定位的能力。然而，在实际应用中，为了方便制作，编码器和激光雷达总是被放置在不同的位置，因此，两传感器的坐标系不重合。在实际使用这两种传感器之前，需要对不同传感器坐标系之间的相对位置进行校准，以融合不同的传感器信息。可见，准确的外部参数标定结果对移动机器人系统的状态估计和环境感知具有重要的意义。

在常见的激光-编码器里程计外参标定方案中，需要有经验的标定工程师去控制小车的运动，通过小车编码器读取的信息和激光雷达对标定物扫描获取的信息，计算出各自的相对运动，然后通过构建多个矩阵组，利用非线性优化的方法求解出外参数。此标定算法的最大缺点是需要专业的标定工程师对小车进行控制或令小车在标定参照物前走大量的轨迹才能保证标定结果的准确性，这就需要工程师具备一定的操作经验才能有效的进行标定，并且十分费时；另外，此标定算法要求激光雷达需实时感知到标定物，当标定物在激光雷达感知域外时，标定算法无法工作。

发明内容

鉴于此，本发明提出一种主动标定激光雷达和编码器外参的方法，以解决现有的激光-编码器里程计外参标定方法所存在的标定过程费时费力、普通人可操作性低且标定结果准确度不高的问题。

为解决上述问题，本发明采用以下技术方案：

一种主动标定激光雷达和编码器外参的方法，包括：S1、将标定物和激光雷达置于同一水平面，并以编码器里程计的坐标系作为移动机器人的坐标系；指定标定物在世界坐标系中的位置和移动机器人的起始位置，根据环境设定观测噪声和运动噪声，并初始化移动机器人的状态变量和状态变量的协方差矩阵；S2、利用轨迹规划算法计算出所述移动机器人的线速度和角速度；S3、使所述移动机器人以步骤S2计算出的线速度和角速度运动一个时间间隔Δt，利用扩展卡尔曼滤波进行状态变量及其协方差矩阵的更新；S4、重复步骤S2和S3，直至状态变量收敛，完成标定。

本发明实现了激光雷达和编码器里程计两个传感器之间的自动外参标定，无需专业人士进行操作，根据算法在线进行轨迹规划，使得标定结果的不确定度最小；此外，即使标定物暂时在激光雷达感知区域外，算法也可以慢慢找到标定物，从而实现主动外参标定。

附图说明

图1是本发明实施例的主动标定激光雷达和编码器外参的系统示意图；

图2是本发明实施例的主动标定激光雷达和编码器外参的方法流程图。

具体实施方式

下面结合附图和具体的实施方式对本发明作进一步说明。

根据本发明的一实施例，提出一种主动标定激光雷达和编码器外参的方法，图2所示为该方法的流程图，请参考图2，该方法包括步骤S1-S4：

步骤S1、将标定物和激光雷达置于同一水平面，并以编码器里程计的坐标系作为移动机器人的坐标系；指定标定物在世界坐标系中的位置和移动机器人的起始位置，根据环境设定观测噪声和运动噪声，并初始化移动机器人的状态变量和状态变量的协方差矩阵。

如图1所示为本发明实施例一种主动标定激光雷达和编码器外参的系统示意图，该标定系统包括移动机器人100(后续简称“机器人”)和标定物30，机器人100具有编码器里程计10、激光雷达20。标定之前，可先按照图1所示摆放好机器人和平面激光雷达20的位置，再将标定物30摆放在与激光雷达20处于同一水平面高度的位置，这样一来，标定物在世界坐标系中的位置和机器人的起始位置都是已知的，另外，可以根据所处环境设定观测噪声和运动噪声，观测噪声是指影响激光雷达观测物体的噪声，运动噪声是指影响机器人移动过程的噪声。

步骤S2、利用轨迹规划算法计算出所述移动机器人的线速度和角速度。

本发明实施例基于滤波器标定原理进行标定。具体而言，采用的是扩展卡尔曼(EKF)滤波的方法。首先，给出机器人的状态变量：

其中，k代表离散状态下的k时刻，X(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)表征激光雷达坐标系原点在机器人坐标系中的位置(x轴坐标和y轴坐标)，Δθ(k)表示激光雷达坐标系与机器人坐标系之间的角度偏移。可以看出，机器人的状态变量包含6个参数，状态变量是一个6×1的矩阵，在初始时刻，我们通过给定的方式来给出状态变量的初始值。对应地，状态变量的协方差矩阵为6×6的对角矩阵，在初始时刻也通过给定的方式来给出协方差矩阵的初始值。

然后，基于上述的状态变量，给出机器人在离散时间状态下的非线性运动模型和观测模型，分别为式(2)和式(3)：

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

式(2)中：

其中，m(k)是运动噪声，n(k)是观测噪声，两者相互独立且均服从零均值的高斯分布，可事先给定，属于已知量；u(k)＝[v(k),ω(k)]^T，v(k)和ω(k)分别表示机器人在k时刻的线速度和角速度，Δt表示离散状态下的时间间隔；

式(3)中：

其中，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)表示标定物在世界坐标系中的x轴，y轴坐标，即位置。

标定算法的目的在于使标定参数状态的不确定度最小，因此利用协方差矩阵或信息矩阵(协方差矩阵的逆)进行轨迹规划，在每一步规划过程中，以输入线速度和角速度为未知变量得到协方差矩阵的表达式，本发明采用协方差矩阵的迹来衡量标定的精度，其值越小，标定的结果精度越高。因此标定过程最终可视为求解使协方差矩阵的迹最小的线速度和角速度的优化控制问题，使用模型预测控制(MPC)进行在线滚动优化，从而保持最优控制。优化算法如下：

min trace(Σ(k))

其中，trace(Σ(k))表示协方差矩阵Σ(k)的迹。

根据k时刻的状态变量X(k)及其协方差矩阵，利用扩展卡尔曼滤波的方法估计出X(k+1)的估计值，在将式(4)代入式(2)之后，方程中仅线速度v(k)和角速度ω(k)为未知量，另外由于协方差矩阵的迹是线速度和角速度的非线性函数，此时再结合k时刻协方差矩阵的迹的表达式，即可求解得到k时刻的线速度v(k)和角速度ω(k)。

步骤S3、使机器人以步骤S2计算出的线速度和角速度运动一个时间间隔Δt，利用扩展卡尔曼滤波进行状态变量及其协方差矩阵的更新。

比如在k＝0初始时刻，经过前述步骤S2之后计算得到了即将作为输入来控制下一时刻机器人运动的线速度和角速度，接着控制机器人以当前计算的线速度和角速度运动一个时间间隔，即来到了k+1时刻，此时可以利用扩展卡尔曼滤波的方法求解出k+1时刻的标定参数Δx(k+1)、Δy(k+1)和Δθ(k+1)，并更新状态变量及其协方差矩阵，即得到k+1时刻的状态变量和其协方差矩阵。

扩展卡尔曼滤波的过程包括：

其中，k+1代表离散状态下k时刻的下一时刻，表示在扩展卡尔曼滤波过程中状态变量的估计值，比如/>表示k+1时刻状态变量X(k+1)的估计值，/>表示k时刻状态变量X(k)的估计值；K(k)表示k时刻的卡尔曼滤波增益，z(k+1)表示k+1时刻的真实观测，表示似然观测；

k+1时刻状态变量的协方差矩阵为

Σ(k+1)＝Γ(k+1)-K(k)H(k)Γ(k)(9)

式(9)中，Γ(k+1)表示k+1时刻的状态变量的预测协方差矩阵，且

其中，F(k)是f关于X(k)的雅克比矩阵，M(k)是f关于m(k)的雅克比矩阵；另，式(9)中：

K(k)＝Γ(k)H(k)^T(H(k)Γ(k)H(k)^T+N(k)N(k)^T)^-1(13)

其中，H(k)是h[X(k),n(k)]关于X(k)的雅克比矩阵，N(k)是h[X(k),n(k)]关于n(k)的雅克比矩阵；

利用扩展卡尔曼滤波器，按照前述的扩展卡尔曼滤波的过程，可以求出标定参数Δx(k+1)、Δy(k+1)和Δθ(k+1)。

在标定过程中，可能会出现标定物不在激光雷达感知区域内的情况，此时感知域内没有梯度信息，因此步骤S2的规划算法不起作用。鉴于此，本发明实施例通过使用sigmoid函数使得测量可以在激光雷达的感知域外得到，解决了感知域不连续的问题。此时的卡尔曼滤波增益变为：

K(k)＝Γ(k)H(k)^TΔ(k+1)(Δ(k+1)H(k)Γ(k)H(k)^TΔ(k+1)+N(k)N(k)^T)^-1Δ(k+1)其中，Δ(k+1)＝diag[δ(k+1)^T]，α为调节sigmoid函数幅度的控制变量，/>为标定物和激光雷达坐标系原点的距离。

步骤S4、重复步骤S2和S3，直至状态变量收敛，完成标定。

从初始时刻开始，计算线速度和角速度作为输入控制机器人运动一个时间间隔，进入下一时刻，并得到更新的状态变量及其协方差矩阵，更新的状态变量中包含了更新的标定参数(外部参数)；再返回到步骤S2，基于最新的状态变量和协方差矩阵继续进行轨迹规划，再次计算得到新的线速度和角速度，又使机器人继续以最新的角速度和线速度运动，如此按照时间间隔循环，直到状态变量收敛时，当前最新的状态变量中的标定参数即为最终的标定结果。

应当理解的是，判断状态变量是否达到收敛的条件，可以预先设定收敛条件，收敛条件在不同的环境中是存在差别的，主要受到噪声的影响。因此可以根据当前环境的观测噪声和运动噪声设定收敛条件，当达到收敛条件时，认为状态变量收敛，即可输出最终的标定结果。

综上，本发明实施例的主动标定激光雷达和编码器外参的方法，具有如下几点优势：

1)此标定方法通过路径规划算法计算出机器人在标定过程中的线速度和角速度，对应的是完成外参标定不确定度最低的运动路线，因此可以进行自主标定，解放人力，提高标定精确度；

2)对于看不见标定物的时刻，可使用sigmoid算法逐步靠近标定物，而不会出现视野中没有标定物时机器人就不知道往哪个方向走的情况；

3)此标定方法使用mpc控制算法，使得标定算法能够更准确的预测标定物的位置，从而实现更精确，更快速的算出标定结果；

4)此标定算法基于扩展卡尔曼滤波，在机器人运动过程中会自动算出标定结果。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims

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

3.如权利要求2所述的主动标定激光雷达和编码器外参的方法，其特征在于，扩展卡尔曼滤波的过程包括：

其中，k+1代表离散状态下k时刻的下一时刻，表示在扩展卡尔曼滤波过程中状态变量的估计值，K(k)表示k时刻的卡尔曼滤波增益，z(k+1)表示k+1时刻的真实观测，表示似然观测；

k+1时刻状态变量的协方差矩阵为

Σ(k+1)＝Γ(k+1)-K(k)H(k)Γ(k) (9)

其中，Γ(k+1)表示k+1时刻的状态变量的预测协方差矩阵，且

其中，F(k)是f关于X(k)的雅克比矩阵，M(k)是f关于m(k)的雅克比矩阵；

另，式(9)中：

K(k)＝Γ(k)H(k)^T(H(k)Γ(k)H(k)^T+N(k)N(k)^T)^-1 (13)

利用扩展卡尔曼滤波器，求出标定参数Δx(k+1)、Δy(k+1)和Δθ(k+1)。

4.如权利要求3所述的主动标定激光雷达和编码器外参的方法，其特征在于，当标定物不在激光雷达的感知区域内时，利用sigmoid函数调节卡尔曼滤波增益，此时的卡尔曼滤波增益变为：

5.如权利要求1所述的主动标定激光雷达和编码器外参的方法，其特征在于，步骤S4中，当状态变量满足预设的收敛条件时，完成标定；其中，所述收敛条件根据预先给定的观测噪声和运动噪声来设定。