[发明专利]基于路口点和路段的变电站作业机器人路径规划及导航定位方法

权利要求书

1.基于路口点和路段的变电站作业机器人路径规划及导航定位方法，其特征在于按以下步骤进行：

S1、机器人自身位置与姿态检测：机器人通过激光雷达、惯性测量单元和里程计感知自身在环境中的位置和姿态；具体步骤为：S1.1机器人内的激光雷达扫描周围环境，激光雷达发射出的激光在物体表面反射，然后被机器人内的接收器检测到，从而计算出激光雷达自身与目标物体的距离，激光雷达以较高的频率扫描周围的环境，从而计算出机器人自身与周围环境中物体的距离，利用周围环境的特征和地图数据计算出机器人自身在地图中的位置和方向；S1.2机器人内的惯性测量单元反馈自身姿态：惯性测量单元实时检测出机器人自身在空间中的X/Y/Z三个方向上的线加速度和角速度，根据积分初步计算出机器人自身在空间中的姿态信息；S1.3机器人内的里程计反馈自身位姿：机器人在移动的过程中，机器人的编码器能实时反馈各个轮子转过的角度值，利用机器人左右轮子的转速计算出机器人在空间中的移动速度和旋转速度，间接计算出机器人在空间的位置和方向；

S2、构建变电站环境地图：控制机器人在变电站中沿着道路移动，让激光雷达能扫描到所有需要进行巡检的区域，然后利用扫描到的激光雷达数据和惯性测量单元数据构建出与变电站环境匹配的地图数据；

S3、地图数据优化：通过上述步骤S2构建出的变电站环境地图存在一些扭曲和杂乱的区域，这些数据不利于后续的机器人定位处理，因此需要对地图数据进行相应的处理，消除地图的孤立点并调整扭曲变形的区域；

S4、机器人定位：包括初步定位和精确定位，初步定位：使用主动蒙特卡罗粒子滤波定位算法对机器人进行初步定位；精确定位：在初步定位的基础上，再通过模板匹配的方法实现高精度的定位；

S5、路网构造：通过对导航路线的分层描述，优化任务路径的计算，提升机器人巡检效率的同时延长机器人的运行时间；具体为：S5.1路口点数据层：机器人在路线与路线的交点或路线的末端点原地旋转或曲线行进；S5.2路段数据层：相邻路口之间的路线，有固定的朝向，若路段上有待巡检的位置点，则机器人在此路段上需要按照给定的朝向前进或者后退；若路段上没有待巡检的位置点，则机器人根据实际情况自由选择朝向和移动方向；S5.3巡检点数据层：机器人需要在巡检的位置点停止并进行巡检操作，所有的巡检的位置点都位于某条路段上；

S6、局部路径规划：通过Dijkstra算法或者A*算法计算出机器人到给定的巡检点之间的最短路径；

S7、全局路径规划：机器人在执行导航任务时，需要对所有的巡检点进行计算，求出一条经过所有的巡检位置而且路径最短的路线；具体步骤为：S7.1找出起始点SP所在的路段L1；S7.2找出L1的头部端点P11；S7.3找出L2的尾部端点P12；S7.4找出下一个巡检点NP所在的路段L2；S7.5找出L2的头部端点P21；S7.6找出L2的尾部端点P22；S7.7采用Dijkstra算法或A*算法结合二叉树结构对上述各端点进行筛选，找出SP到达 NP的最短路径；S7.8重复上述操作，直到机器人能依次经过所有指定的巡检点；

S8、机器人自主导航：包括指定位置点自主导航、指定巡检点自主导航和多巡检点自主导航；指定位置点自主导航：给出任一位置点，机器人计算出到达此位置点的路径，然后沿着该路径向目标位姿点移动；指定巡检点自主导航：给出任一巡检点，机器人计算出到达此巡检点的路径，然后沿着该路径向目标巡检点移动；多巡检点自主导航：给出所有待巡检的巡检点集合，机器人计算出经过所有巡检点的路径，然后沿着该路径依次停靠所有的巡检点；

S9、路径中断与障碍处理。

2.根据权利要求1所述的基于路口点和路段的变电站作业机器人路径规划及导航定位方法，其特征在于：所述S3地图数据优化具体步骤为：

S3.1图像处理：设定阈值处理变电站环境地图中的噪声、杂点、边缘和色阶操作；

S3.2人工处理：调整变电站环境地图中的扭曲变形的区域和连续中断的区域操作。

3.据权利要求1所述的基于路口点和路段的变电站作业机器人路径规划及导航定位方法，其特征在于：所述S9路径中断与障碍处理具体步骤为：

S9.1机器人的超声波传感器或红外传感器传感设备实时检测机器人周围的环境；

S9.2检测到路径前方无法通行；

S9.3根据预设的执行策略选择“停止”、“一键返 航”或“绕行”操作，如果为“停止”则执行S9.4，如果为“一键返 航”则执行S9.5，如果为“绕行”则执行S9.6；

S9.4取消当前导航任务，原地待命；

S9.5取消当前导航任务，计算出一条通往充电点的路径，并按照该路径移动到充电点；

S9.6取消当前导航任务，重新计算出一条通往目标导航点的路径，并按照该路径移动到目标导航点，然后继续执行剩下的导航任务。