CN112731354A - Agv上激光雷达位姿的自标定方法 - Google Patents

Abstract

本发明公开了一种AGV上激光雷达位姿的自标定方法，包括如下步骤：S1、基于相邻点云帧间的距离误差匹配出雷达实时位姿；S3、控制AGV原地旋转，获取AGV中心在雷达坐标系下的坐标(A,B)；S4、控制AGV直行，计算雷达相对AGV的偏角theta；S5、通过雷达坐标系与AGV坐标系间的转换，获取激光雷达在AGV坐标系中的位置；雷达相对AGV的偏角theta及激光雷达在AGV坐标系中的位置即构成为雷达在AGV坐标系中的位姿。不依赖其它标记物即可标定，可实现自动标定，提高了标定效率。标定结果的计算公式数据都是通过多数据拟合，提高了标定精度。

Description

AGV上激光雷达位姿的自标定方法

技术领域

本发明属于位姿标定技术领域，更具体地，本发明涉及一种AGV上激光雷达位姿的自标定方法。

背景技术

随着AGV在现代仓储物流领域中应用得越来越广泛，其定位导航技术的精度要求也越发重要，其中定位导航精度非常依赖传感器在AGV上的位姿精度，这就需要高精度测量传感器在AGV上的位姿。传统方法是通过机械设计在AGV上设计固定位姿的定位孔，然后将激光雷达安装在AGV的雷达定位孔上。然而机械制造是有误差的，工装配合也包含误差，甚至激光雷达数据坐标也并不一定在雷达正中心，所以直接使用机械设计的位姿坐标包含很大的误差，导致AGV的定位精度不高。

CN 108555908A，激光定位传感器在AGV小车中位置标定方法，该方案在平坦的地面上先控制AGV直行，记录激光雷达定位传感器输出的位置姿态值，用最小二乘法将这些点拟合成直线，计算激光雷达定位传感器在小车坐标系下的姿态；控制AGV小车原地旋转，记录激光雷达定位传感器的姿态值，用最小二乘法将这些点拟合出圆的方程，计算出传感器在小车坐标系下的位置。然而该方法存在如下问题：

该方案需要先计算雷达的方位角，再通过方位角计算雷达在小车中的位置。而其方位角计算具有单次不确定性会导致位置计算不准确。该方法中激光定位传感器在AGV小车坐标系中的方位角为

其中β₁选用x值最大的那组定位数据记为(x₁,y₁,β₁)，该β₁的选用具有单次不确定性，导致方位角计算出现偏差，进而导致位置计算出现偏差。

发明内容

本发明提供了一种AGV上激光雷达位姿的自标定方法，旨在改善上述问题。

本发明是这样实现的，一种AGV上激光雷达位姿的自标定方法，所述方法具体包括如下步骤：

S1、基于相邻点云帧间的距离误差匹配出雷达实时位姿；

S3、控制AGV原地旋转，获取AGV中心在雷达坐标系下的坐标(A,B)；

S4、控制AGV直行，计算雷达相对AGV的偏角theta；

S5、通过雷达坐标系与AGV坐标系间的转换，获取激光雷达在AGV坐标系中的位置；

雷达相对AGV的偏角theta及激光雷达在AGV坐标系中的位置即构成为雷达在AGV坐标系中的位姿。

进一步的，所述步骤S1具体包括如下步骤：

S21、在全局变量中的点云帧数量大于等于2时，将T时刻的点云帧投影到(T-1)时刻的点云帧上，查找T时刻点云帧中各点p_i在(T-1)时刻点云帧中的两个最近点

获取该两最近点所在直线的法向量

计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S22、将T时刻点云帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取雷达两时刻的最优相对位姿R&t，转化为位姿矩阵Q_T；

其中，p_j表示点集中的第j个点，

表示点p_j在上一点云帧距其距离最近的一个点；

S23、基于在前各时刻的位姿矩阵计算雷达的实时位姿(x_i,y_i,θ_i)，并存到全局位姿变量。

进一步的，AGV中心在雷达坐标系下的坐标(A,B)获取方法具体如下：

控制AGV原地旋转，从全局变量获取雷达的最新位置数据(x_i,y_i),构造如下公式圆方程的残差公式，然后进行高斯牛顿迭代优化，得到圆心坐标和半径参数(A，B，Radius)：

residual＝[(x-A)²+(y-B)²-Radius²]→0

其中(A,B)就是AGV中心点在雷达坐标系下的坐标。

进一步的，雷达相对AGV的偏角theta的获取方法具体如下：

控制AGV直行，从全局变量获取雷达的最新的位置数据(x_i,y_i)，通过线性最小二乘拟合直线斜率k，由直线拟合斜率k得到雷达相对底盘的偏角theta：

theta＝arctan(k)，theta∈[-π/2.0,π/2.0]。

进一步的，激光雷达在AGV坐标系中的位置获取方法具体如下：

通过坐标系变换公式(5)得到雷达坐标系下的点在AGV坐标系下的坐标：

P_agv＝R*P_lidar+t (5)

其中：P_lidar是雷达坐标下的点坐标，P_agv是AGV坐标系下的点坐标；R&t就是雷达在AGV坐标系中的位姿矩阵，其中，

由于(A,B)是AGV中心点在雷达坐标系下的坐标，则P_lidar＝(A,B),P_agv＝(0,0)；

代入坐标系变换公式(5)，可以得到激光雷达在AGV坐标系中的位置:

进一步的，在步骤S1之前好包括：对雷达获取点云帧进行体素滤波。

本发明提出的AGV上激光雷达位姿的自标定方法，不依赖其它标记物即可标定，按照附图流程可实现自动标定，提高了标定效率。标定结果的计算公式数据都是通过多数据拟合，提高了标定精度。

附图说明

图1为本发明实施例提供的AGV上激光雷达位姿的自标定方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图1为本发明实施例提供的AGV上激光雷达位姿的自标定方法流程图，该方法具体包括如下步骤：

S1、启动雷达驱动获取点云并滤波；

首先获取雷达最新时刻点云帧，因为原始点云帧中的点云数太多，影响后续的匹配效率，先进行体素滤波，在保证点云微观特征的基础上降低点云帧中的点云密度，处理之后将点云帧存到全局变量。

S2、基于相邻点云帧间的距离误差匹配出雷达实时位姿；

在本发明实施例中，雷达实时位姿的获取方法具体包括如下步骤：

S21、在全局变量中的点云帧数量大于等于2时，将T时刻的点云帧投影到(T-1)时刻的点云帧上，查找T时刻点云帧中各点p_i在(T-1)时刻点云帧中的两个最近点

获取该两最近点所在直线的法向量

那么可以通过公式(1)计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S22、将T时刻点云帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取(T-1)时刻至T时刻的雷达最优的相对位姿R&t；

其中，p_j表示点集中的第j个点，

表示点p_j在上一点云帧距其距离最近的一个点。从而得到，两个时刻的相位姿矩阵Q_T，表示如下：

那么雷达在任意t时刻的位姿表示如下：

pose_t＝(x_t,y_t,θ_t)＝Q₀Q₁Q₂…Q_t

(x_i,y_i,θ_i)即雷达的实时位姿，并存到全局位姿变量。

S3、控制AGV原地旋转，获取AGV中心在雷达坐标系下的坐标(A,B)；

控制AGV原地旋转，从全局变量获取雷达的最新位置数据(x_i,y_i),构造如下公式圆方程的残差公式，然后进行高斯牛顿迭代优化，得到圆心坐标和半径参数(A，B，Radius)：

residual＝[(x-A)²+(y-B)²-Radius²]→0

其中(A,B)就是AGV中心点在雷达坐标系下的坐标。

S4、控制AGV直行，计算雷达相对底盘的偏角theta；

控制AGV原地旋转，从全局变量获取雷达的最新的位置数据(x_i,y_i)，通过线性最小二乘拟合直线斜率k，公式如下：

由直线拟合斜率k得到雷达相对底盘的偏角theta：

theta＝arctan(k)，theta∈[-π/2.0,π/2.0] (4)

S5、通过如下的坐标系变换公式(5)得到雷达坐标系下的点在AGV坐标系下的坐标：

P_agv＝R*P_lidar+t (5)

其中：P_lidar是雷达坐标下的点坐标；P_agv是AGV坐标系下的点坐标；R&t就是雷达在AGV坐标系中的位姿矩阵，其中R如下：

由于(A,B)是AGV中心点在雷达坐标系下的坐标，那么有：

P_lidar＝(A,B),P_agv＝(0,0)

代入坐标系变换公式，可以得到激光雷达在AGV坐标系中的位置:

最终得到标定结果，即3x1的位姿矩阵R&t，位姿由位置和姿态组成：

本发明提出的AGV上激光雷达位姿的自标定方法具有如下有益技术效果：

1)不依赖于激光定位传感器的定位数据，而是误差匹配实现雷达的定位，不受限于激光定位传感器的定位精度；

2)不依赖于里程计，里程计精度低，有累计误差；

3)不需要计算方位角度，方位角计算具有单次不确定性会导致位置计算不准确，算法参数输入少，标定精度高；

综上所述，本方法不依赖其它标记物即可标定，按照附图流程可实现自动标定，提高了标定效率。标定结果的计算公式数据都是通过多数据拟合，提高了标定精度。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (6)

1.一种AGV上激光雷达位姿的自标定方法，其特征在于，所述方法具体包括如下步骤：

S1、基于相邻点云帧间的距离误差匹配出雷达实时位姿；

S2、控制AGV原地旋转，获取AGV中心在雷达坐标系下的坐标(A,B)；

S3、控制AGV直行，计算雷达相对AGV的偏角theta；

S4、通过雷达坐标系与AGV坐标系间的转换，获取激光雷达在AGV坐标系中的位置；

雷达相对AGV的偏角theta及激光雷达在AGV坐标系中的位置即构成为雷达在AGV坐标系中的位姿。

2.如权利要求1所述AGV上激光雷达位姿的自标定方法，其特征在于，所述步骤S1具体包括如下步骤：

S21、在全局变量中的点云帧数量大于等于2时，将T时刻的点云帧投影到(T-1)时刻的点云帧上，查找T时刻点云帧中各点p_i在(T-1)时刻点云帧中的两个最近点

获取该两最近点所在直线的法向量

计算点p_i到两最近点所在直线的距离，该距离值即表示误差值：

S22、将T时刻点云帧中的所有点放入点集中，若点p_i对应的距离dis_err大于设定的误差阈值，则在点集中删除该点p_i，遍历点集中的所有点，基于公式(2)来获取雷达两时刻的最优相对位姿R&t，转化为位姿矩阵Q_T；

其中，p_j表示点集中的第j个点，

表示点p_j在上一点云帧距其距离最近的一个点；

S23、基于在前各时刻的位姿矩阵计算雷达的实时位姿(x_i,y_i,θ_i)，并存到全局位姿变量。

3.如权利要求1所述AGV上激光雷达位姿的自标定方法，其特征在于，AGV中心在雷达坐标系下的坐标(A,B)获取方法具体如下：

控制AGV原地旋转，从全局变量获取雷达的最新位置数据(x_i,y_i),构造如下公式圆方程的残差公式，然后进行高斯牛顿迭代优化，得到圆心坐标和半径参数(A，B，Radius)：

residual＝[(x-A)²+(y-B)²-Radius²]→0

其中(A,B)就是AGV中心点在雷达坐标系下的坐标。

4.如权利要求1所述AGV上激光雷达位姿的自标定方法，其特征在于，雷达相对AGV的偏角theta的获取方法具体如下：

控制AGV直行，从全局变量获取雷达的最新的位置数据(x_i,y_i)，通过线性最小二乘拟合直线斜率k，由直线拟合斜率k得到雷达相对底盘的偏角theta：

theta＝arctan(k)，theta∈[-π/2.0,π/2.0]。

5.如权利要求1所述AGV上激光雷达位姿的自标定方法，其特征在于，激光雷达在AGV坐标系中的位置获取方法具体如下：

通过坐标系变换公式(5)得到雷达坐标系下的点在AGV坐标系下的坐标：

P_agv＝R*P_lidar+t (5)

其中：P_lidar是雷达坐标下的点坐标，P_agv是AGV坐标系下的点坐标；R&t就是雷达在AGV坐标系中的位姿矩阵，其中，

由于(A,B)是AGV中心点在雷达坐标系下的坐标，则P_lidar＝(A,B),P_agv＝(0,0)；

代入坐标系变换公式(5)，可以得到激光雷达在AGV坐标系中的位置:

6.如权利要求1所述AGV上激光雷达位姿的自标定方法，其特征在于，在步骤S1之前好包括：对雷达获取点云帧进行体素滤波。

Images