一种大型规则地底圆形隧道环境综合定位感知方法与流程

本发明涉及工业机器人领域，具体涉及一种大型规则地底圆形隧道环境综合定位感知方法。

背景技术：

1、核电机组运行时的最终热阱是海水冷源，负责带走停堆时的堆芯预热、冷却各种核安全设备，在核电运行中具有重要作用，取水口运行状态的好坏直接影响电厂的安全运行性和可靠性。机组运行时产生的热量会导致海生物在冷源隧道附着，可以通过移动载体搭载机械臂对隧道壁面进行自主清理，要达到自主清理的目的，需要对隧道中的载体进行精确实时定位，对隧道内壁环境实时感知，从而使载体能够通过定位数据进行稳定的运动控制，使机械臂能够获取相关环境参数进行轨迹规划，也可以通过感知障碍物信息使移动载体和机械臂安全运行。地底隧道环境不能接收卫星信号进行定位，而且形状规则的隧道没有足够的纹理支撑视觉或者激光匹配定位，现有隧道定位技术中，绝大多数都是采用的在隧道中布置无线通信模块，利用载波测距技术实现隧道内的定位。但无线通信模块测距量程短，测距方式导致移动中的物体测距精度不高，而且单个价格不便宜，在长隧道中往往需要布置大量的通信模块，成本较高。

技术实现思路

1、本发明的目的在于提供一种大型规则地底圆形隧道环境综合定位感知方法，主要解决在大型规则地底隧道环境定位感知问题，通过在底盘上搭载不同的传感器，实现底盘全局二维定位、相对于隧道的左右偏差和朝向偏差定位、隧道半径感知和障碍物感知四个方面的功能。

2、本发明的技术方案如下：一种大型规则地底圆形隧道环境综合定位感知方法，包括如下步骤：

3、步骤1：全局二维定位；

4、步骤2：局部姿态定位；

5、步骤3：隧道环境感知。

6、所述的步骤1包括如下：

7、步骤11：获取传感器数据；

8、步骤12：反光板定位解算；

9、步骤13：定位初始化；

10、步骤14：惯性解算；

11、步骤15：使用误差状态卡尔曼滤波算法实现高精度定位；

12、步骤16：实时记录当前位姿。

13、所述的步骤11中所述的数据包括惯性测量单元的三轴加速度和三轴角速度数据、激光识别的反光板的相对坐标数据、车辆轮速计(里程计)的轮速(里程)数据。

14、所述的步骤12中包括精确定位和粗定位，

15、所述的精确定位为通过激光扫描到多个反光板的相对位置{xl1，xl2，…，xli}，可知此时车辆在起点附近启动或才开始运动，已知估算的车身位置得到反光板的全局位置与地图中的反光板进行最近邻匹配，并计算位置误差使用最小二乘优化：

16、

17、即可得到精确定位；

18、所述的粗定位为单个反光板时，车辆处于实时定位中，根据当前位置选取已知地图中最近的反光板位置xrw，激光扫描到的反光板相对位置x′l，激光位置为：

19、xlidar＝xrw*(x′f)-1

20、由激光与车身的相对变换tl2b可得：

21、xb＝tl2b*xlidar。

22、所述的步骤13为在反光板地图中实现初始定位，在划定起点范围内，利用精确定位初始化位置，如果不在初始点启动，赋值初始时刻位置、初始时刻速度和初始时刻姿态；同时初始化状态量、状态量方差、过程噪声与观测噪声。

23、所述的步骤14为获取惯性传感器的数据为加速度向量α，角速度向量ω，传感器的误差为bα，bω，根据惯导模型解算得出位置，速度和姿态。

24、所述的步骤15分为预测更新和观测更新两个步骤；

25、所述的预测更新：主要分为两个步骤。首先不考虑imu的误差，计算出单个时间不长内卡尔曼系统中的状态量(位置、速度、姿态、偏差和重力)变化，对系统名义状态进行更新；然后对误差状态及其协方差矩阵进行更新。

26、所述的观测更新：当系统接收到轮速计(里程计)信息或当系统接收到反光板定位的位姿时，根据不同的观测方程，计算卡尔曼增益、更新误差状态和协方差矩阵，合并更新后的误差状态和名义状态，求得机器人的精确定位信息，同事重置误差状态，进行下一次预测更新流程。

27、所述的步骤2包括如下步骤：

28、步骤21：车辆获取实时激光数据，提取隧道半径高度一定范围内的激光点，作为隧道壁拟合范围，并将激光点栅格化；

29、步骤22：分割两侧栅格点，转为二值图像坐标和数据；

30、步骤23：利用ransac算法分别拟合直线，并转换为激光坐标以点斜式输出，点斜式直线方程y＝kx+b；

31、步骤24：由点斜式斜率k得到隧道壁相对车身角度：θ＝arctan(k)，则车身相对航向角为-θ，由车身中心相对激光的左右偏差(dx，dy)可计算出车身到隧道壁的距离

32、所述的步骤3包括如下步骤：

33、步骤31：对激光点云数据筛选范围，并降采样，减少点云数量，降低计算量；

34、步骤32：对采样后的点云以ransac算法拟合圆柱，并输出圆心坐标、半径以及圆面法向量等参数；

35、步骤33：拟合圆柱之后，得到内点和外点，对外点进行k-means聚类，当聚类点数目大于20个点时，判断该物体为障碍物，用立方体包裹该聚类点，以立方体的中心点作为感知障碍物的中心点。

36、本发明的有益效果在于：在没有卫星信号和无线通信靶标的情形下，通过融合里程计、惯性测量单元和激光雷达靶标位置信息，实现在地底规则隧道中的全局精确定位，方便远程显示和作业进程监控；通过多线激光雷达和惯性测量单元，获得底盘相对于隧道壁的横向偏差定位、朝向偏差定位和翻滚角，指导底盘运动控制和机械臂轨迹规划及作业；利用多线激光雷达、uwb基站和毫米波雷达可以感知得到隧道半径、障碍物和人员距离等信息，指导底盘和机械臂稳定安全的运行。

技术特征：

1.一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于，包括如下步骤：

2.如权利要求1所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于，所述的步骤1包括如下：

3.如权利要求2所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于：所述的步骤11中所述的数据包括惯性测量单元的三轴加速度和三轴角速度数据、激光识别的反光板的相对坐标数据、车辆轮速计(里程计)的轮速(里程)数据。

4.如权利要求2所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于：所述的步骤12中包括精确定位和粗定位，

5.如权利要求2所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于：所述的步骤13为在反光板地图中实现初始定位，在划定起点范围内，利用精确定位初始化位置，如果不在初始点启动，赋值初始时刻位置、初始时刻速度和初始时刻姿态；同时初始化状态量、状态量方差、过程噪声与观测噪声。

6.如权利要求2所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于：所述的步骤14为获取惯性传感器的数据为加速度向量α，角速度向量ω，传感器的误差为bα，bω，根据惯导模型解算得出位置，速度和姿态。

7.如权利要求2所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于：所述的步骤15分为预测更新和观测更新两个步骤；

8.如权利要求1所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于，所述的步骤2包括如下步骤：

9.如权利要求1所述的一种大型规则地底圆形隧道环境综合定位感知方法，其特征在于，所述的步骤3包括如下步骤：

技术总结
本发明涉及工业机器人领域，具体涉及一种大型规则地底圆形隧道环境综合定位感知方法。包括如下步骤：步骤1：全局二维定位；步骤2：局部姿态定位；步骤3：隧道环境感知。本发明的有益效果在于：在没有卫星信号和无线通信靶标的情形下，通过融合里程计、惯性测量单元和激光雷达靶标位置信息，实现在地底规则隧道中的全局精确定位，方便远程显示和作业进程监控；通过多线激光雷达和惯性测量单元，获得底盘相对于隧道壁的横向偏差定位、朝向偏差定位和翻滚角，指导底盘运动控制和机械臂轨迹规划及作业；利用多线激光雷达、UWB基站和毫米波雷达可以感知得到隧道半径、障碍物和人员距离等信息，指导底盘和机械臂稳定安全的运行。

技术研发人员：陈勇,杨斌,明平良,刘强,彭军,彭谊宏
受保护的技术使用者：中核武汉核电运行技术股份有限公司
技术研发日：
技术公布日：2024/1/13