基于激光定位与趋势补偿的激光雷达自动抗差标定方法与流程

本发明属于激光测距定位，具体地说涉及一种基于激光定位与趋势补偿的激光雷达自动抗差标定方法。

背景技术：

1、现有激光雷达相对于agv车体的安装位置（外参）标定方案主要包括：

2、（1）手动机械测量法：使用卡尺、三坐标测量仪等工具，对雷达安装支架的物理尺寸进行测量，再通过几何换算得到雷达在车体坐标系下的位置参数（x，y，yaw）。该方法依赖人工操作，效率低，一致性难以保证。

3、（2）基于特定标定物的自动标定法：在agv工作环境中专门布置棋盘格、反光贴、角反射器等具有已知精确位置的标定物。agv通过执行预设运动，使车载激光雷达观测标定物，再通过点云匹配或特征提取算法求解雷达外参。此方法需要预先布置并持续维护外部标定物。

4、（3）基于自旋运动的标定法：使agv执行原地旋转运动，利用里程计（或车体运动学模型）与某种外部参考定位（如反光柱）的位置差异，计算雷达安装的xy偏移。现有方法通常只进行单一旋转方向（顺时针或逆时针）的单次或简单多次采集后取平均。

5、手动法及标定物法的工程局限性：手动法无法适应量产agv的快速标定需求；标定物法则依赖于外部环境的改造和维护，无法在真实工作场景中随时启动在线标定。

6、现有自旋标定法对底盘运动误差的忽视（核心缺点）：在实际工程中，受agv底盘零位角标定不准、轮径差异、传动间隙、地面打滑等多重因素影响，agv自旋运动的真实旋转中心并非一个理想的固定点，而是会产生持续的定向漂移。现有的单一方向、简单平均的自旋标定方法，完全未考虑此底盘运动误差，导致其标定结果中包含了由底盘运动精度决定的系统偏差。尤其地，顺时针与逆时针旋转引起的误差分量具有方向相反的特性，现有方法无法对此进行识别和消除。因此，现有方案的标定鲁棒性差，标定结果高度依赖于该辆agv当时的底盘状态，重复定位精度难以有效保证。

技术实现思路

1、针对现有技术中现有激光雷达相对于agv车体的安装位置（外参）标定方案的鲁棒性和重复定位精度有待提高的问题，现提出一种基于激光定位与趋势补偿的激光雷达自动抗差标定方法。本发明提供如下技术方案：

2、一种基于激光定位与趋势补偿的激光雷达自动抗差标定方法，agv在已建图环境中运行，由车载激光定位系统输出的现成实时定位数据，并结合agv自身的轮式里程计数据进行以下步骤：

3、s1、偏航角初步标定；

4、s2、安装位置（x，y）抗差标定；

5、步骤s2包括以下子步骤：

6、s21、数据采集；双向多圈四角度位差采集；

7、s22、第一层抗差：多圈趋势补偿，消除累积漂移；

8、s23、逐圈解算与单方向平均；

9、s24、第二层抗差；

10、s3、输出结果。

11、优选的，步骤s1中，控制agv沿当前航向直线行驶预设距离，通过采集行驶起点与终点的激光定位数据，计算实际运动方向与初始航向的夹角，得出偏航角修正值。

12、优选的，步骤s21中，agv在原地分别进行顺时针和逆时针两个方向的多圈旋转运动；每圈运动中，agv依次旋转至互为正交关系的角度组，并短暂停止，同步采集记录当前角度下的里程计位置与激光定位位置。

13、优选的，步骤s22中，通过趋势分析主动量化此漂移斜率，并以第一圈为基准，将后续各圈差值数据中由非里程计可感知因素引起的累积漂移量扣除，得到扣除累积漂移量后的差值数据。

14、优选的，步骤s22中，差值数据的计算方式如下：

15、设共采集n圈（n≥2），对于预设角度j（j=0，1，2，3分别对应0°，90°，180°，270°），可获得差值序列：

16、；

17、差值序列：

18、；

19、其中圈序号i=0，1，…，n-1。

20、以圈序号i为自变量，差值为因变量，使用最小二乘法拟合漂移斜率：

21、；

22、该斜率代表该角度下每旋转一圈的平均漂移量；随后，对第i圈的差值进行修正：

23、；

24、即从原始差值中剔除因持续漂移而累加的误差分量，使所有圈次的数据被统一到第一圈（i=0）的基准上；对采用完全相同的处理方式；此修正过程独立应用于每一个目标角度。

25、优选的，步骤s23中，将每一圈的一组经过漂移修正的四个角度差值代入四角度正交解耦模型，单独计算出一个该圈的雷达安装偏移量，采集n圈的雷达安装偏移量作算术平均得到该旋转方向下消除了累积漂移影响的雷达安装偏移量估计。

26、优选的，预设雷达安装位置相对于车体旋转中心的真实偏移量为，四角度正交解耦模型为：

27、

28、

29、。

30、优选的，步骤s24中，对另一个旋转方向的数据执行完全相同的处理流程，得到另一组安装偏移量估计；最后，将顺时针与逆时针两个方向的结果再次取算术平均：

31、；

32、；

33、得到无偏的雷达安装位置估计；其中，为顺时针方向下的x轴上的安装偏移量估计，为逆时针方向下的x轴上的安装偏移量估计。

34、优选的，步骤s3中，将计算所得的(，)与当前系统配置的(，)相加，即得到雷达应安装的最终位置（，），其中

35、；

36、；

37、将偏航角标定结果与安装位置标定结果一并保存输出，完成整个自动标定流程。

38、优选的，还包括步骤：s00、标定过程健康监控；实时检测激光定位数据的置信度是否达标、数据是否超时中断；若检测到异常，则立即停止agv运动并报错。

39、相对于现有技术，本发明具有以下有益效果：

40、1、本发明可对在相同目标角度下多次采集的位姿差值序列进行趋势分析（如线性回归），主动量化并剥离agv自旋运动过程中的定向累积漂移误差，从数据源头提升信噪比；

41、2、本发明分别执行并综合利用顺时针和逆时针两个方向的标定数据，利用两种旋转方向下底盘方向性误差近似反向的特性，通过求平均将其抵消，从而获得无偏的安装位置估计；

42、3、本发明利用在0°、90°、180°、270°四个特殊角度下，雷达安装偏移量与激光/里程计位置差值之间的正交几何关系，构建简洁的解耦方程，直接解算安装偏移，计算高效。

1.一种基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，agv在已建图环境中运行，由车载激光定位系统输出的现成实时定位数据，并结合agv自身的轮式里程计数据进行以下步骤：

2.根据权利要求1所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s1中，控制agv沿当前航向直线行驶预设距离，通过采集行驶起点与终点的激光定位数据，计算实际运动方向与初始航向的夹角，得出偏航角修正值。

3.根据权利要求1所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s21中，agv在原地分别进行顺时针和逆时针两个方向的多圈旋转运动；每圈运动中，agv依次旋转至互为正交关系的角度组，并短暂停止，同步采集记录当前角度下的里程计位置与激光定位位置。

4.根据权利要求1或3所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s22中，通过趋势分析主动量化此漂移斜率，并以第一圈为基准，将后续各圈差值数据中由非里程计可感知因素引起的累积漂移量扣除，得到扣除累积漂移量后的差值数据。

5.根据权利要求4所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s22中，差值数据的计算方式如下：

6.根据权利要求1所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s23中，将每一圈的一组经过漂移修正的四个角度差值代入四角度正交解耦模型，单独计算出一个该圈的雷达安装偏移量，采集n圈的雷达安装偏移量作算术平均得到该旋转方向下消除了累积漂移影响的雷达安装偏移量估计。

7.根据权利要求6所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，预设雷达安装位置相对于车体旋转中心的真实偏移量为，四角度正交解耦模型为：

8.根据权利要求7所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s24中，对另一个旋转方向的数据执行完全相同的处理流程，得到另一组安装偏移量估计；最后，将顺时针与逆时针两个方向的结果再次取算术平均：

9.根据权利要求8所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，步骤s3中，将计算所得的(，)与当前系统配置的(，)相加，即得到雷达应安装的最终位置（，），其中

10.根据权利要求1所述的基于激光定位与趋势补偿的激光雷达自动抗差标定方法，其特征在于，还包括步骤：s00、标定过程健康监控；实时检测激光定位数据的置信度是否达标、数据是否超时中断；若检测到异常，则立即停止agv运动并报错。