基于点云的带电作业机器人自主路径规划方法技术

本发明专利技术公开了一种基于点云的带电作业机器人自主路径规划方法，包括以下步骤：在双机械臂末端均安装一个深度相机；进行手眼标定和双机械臂基坐标系标定，实现两个相机视野共享；控制双机械臂的基座关节旋转360°，采集多帧图像；将采集到的图像进行拼接，获得机械臂工作空间的完整环境信息；采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库。本发明专利技术采用眼在手上的安装方式在机械臂末端安装深度相机，结合机械臂的运动学模型，构建整个作业环境的三维点云图，使系统具备对环境障碍物的感知能力；再应用改进的路径规划算法，确保机械臂不与障碍物发生碰撞的同时，能够实时规划机械臂的最优运动路径。

Autonomous path planning method of live working robot based on point cloud

【技术实现步骤摘要】
基于点云的带电作业机器人自主路径规划方法
本专利技术属于带电作业机器人自主作业领域，特别是一种基于点云的带电作业机器人自主路径规划方法。
技术介绍
相较于传统的机器人应用的工厂环境，配电线路维护机器人需要在非结构化环境下进行作业，要实现机器人能够自主规划出无碰撞作业路径，机器人系统必须首先具备对周围环境的感知能力。然而带电作业的作业环境大多是非结构化环境，不同地点的作业环境均不相同，因此手工测量并构建固定作业场景模型的方法并不可行，则利用深度相机对作业场景进行三维重建尤为重要，利用深度相机对真实存在的三维场景或物体进行扫描，并根据获得的信息构建出机器人的作业场景。然而配电线路的带电作业需要在高空进行，而构建完整的作业环境需要从多个角度对作业环境信息进行采集，常用的多深度相机支架的方法无法直接使用。此外，机器人在作业过程中不能与周围的电路设备以及环境中的其他障碍物发生碰撞，因此基于点云重构作业环境的障碍物分布，再根据机械臂起始和目标位姿，选择一条从起始点到达目标点的路径，然而多自由度机械臂就需要在高维空间进行路径规划，目前已有的路径规划算法大多是针对的二维或三维空间，对于高维度空间的路径规划存在实时性差且规划的路径难以最优等问题，目前已有的技术大多数是通过遥操作控制机械臂绕开障碍物，无法实现非结构化环境下的自主无障碍路径规划。
技术实现思路
本专利技术的目的在于提供一种带电作业机器人自主路径规划方法，以解决在高空非结构化环境中机械臂作业时可能会与环境中障碍物发生碰撞产生的安全问题。实现本专利技术目的的技术解决方案为：一种基于点云的带电作业机器人自主路径规划方法，包括以下步骤：步骤1、在带电作业机器人双机械臂的末端均安装固定一个深度相机；步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；步骤3、控制双机械臂的基座关节旋转360°，且每旋转θ°深度相机采集一帧图像，获得当前视野中的环境信息；步骤4、将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在世界坐标系中的位置坐标信息；其中，世界坐标系为某一机械臂的基坐标系；步骤5、采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库以供后续新的路径规划参考使用。本专利技术与现有技术相比，其显著优点为：1)通过眼在手上的安装方式，结合机械臂的运动，仅用两个深度相机即可获得机械臂整个工作空间的环境信息，解决了高空作业环境难以重构的难题；2)通过手眼标定和双机械臂基坐标系标定，可以直接获取安装在机械臂末端的相机位姿，再结合RGB-D图像和相机内参，可直接计算任何一个像素在世界坐标系下的位置，直接对点云图像进行拼接，相较于传统的采用配准等算法进行拼接，实时性更高；3)采用的快速手眼标定方法，通过固定标定板以及控制机械臂自主运动，大大缩减了手眼标定的时间；4)通过改进已有的路径规划算法，在扩展新节点和父节点时均采用先按照设定的代价函数排序，之后再进行碰撞检测的方式，减少了碰撞检测的次数，提高了对随机构型的利用率；5)通过球体包围盒叠加的方式对障碍物的点云模型进行简化描述，并将机械臂连杆的最大包络半径叠加到障碍物的厚度上，降低了碰撞检测的运算量，解决高维机械臂路径规划实时性较差的问题；6)通过实时更新安全路径数据库，将规划出的可行路径实时更新到安全路径库中，在走相同或附近路径时能够尽可能应用数据库中的已有可行路径，提高了路径规划的效率。下面结合附图对本专利技术作进一步详细描述。附图说明图1为基于点云的带电作业机器人自主路径规划方法流程图。图2为本专利技术基于深度相机的手眼关系示意图。图3为本专利技术双机械臂基坐标系标定示意图。图4为本专利技术快速手眼标定流程图。图5为本专利技术双机械臂基坐标系标定流程图。具体实施方式结合图1，本专利技术一种基于点云的带电作业机器人自主路径规划方法，包括以下步骤：步骤1、在带电作业机器人双机械臂的末端均安装固定一个深度相机；步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；步骤3、控制双机械臂的基座关节旋转360°，且每旋转θ°深度相机采集一帧图像，获得当前视野中的环境信息；步骤4、将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在世界坐标系中的位置坐标信息；其中，世界坐标系为某一机械臂的基坐标系；步骤5、采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库以供后续新的路径规划参考使用。结合图1至图3，进一步地，步骤2中进行手眼标定，具体为：步骤2-1、初始化机械臂的状态，使机械臂处于指定位姿；步骤2-2、根据预先设定好的机械臂的初始位姿，移动标定板至指定位置；步骤2-3、控制机械臂运动，每移动一次获取标定图像和当前机械臂的各关节角度；步骤2-4、判断是否达到指定的循环次数，若未达到，则返回步骤2-3；若达到，则分别求取左、右机械臂的手眼标定矩阵即相机坐标系到机械臂末端坐标系的转换矩阵，所用公式为：式中，Pr表示各特征点在机械臂基坐标系下的坐标向量的集合矩阵，表示第i次采集的机械臂末端坐标系到机械臂基坐标系的转换矩阵，Pc(i)表示第i次采集的图像中的特征点在相机坐标系下的三维坐标的集合矩阵，Tec表示机械臂的手眼标定矩阵，Constant表示常数，i＝1,2,…,s，s表示指定的循环次数。进一步地，结合图4，步骤2中进行双机械臂基坐标系标定具体为：利用标定板相对于两个机械臂基座的位置不变，结合手眼标定获得的两个机械臂的手眼关系和建立如下关系：式中，分别为标定板坐标系到左、右相机坐标系的转换矩阵，分别为左、右机械臂的末端坐标系到其各自的基坐标系的转换矩阵；由此求得双机械臂基坐标系的标定矩阵即两个机械臂基坐标系之间的转换关系利用最小二乘法求得最优解。进一步地，步骤4中将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在机械臂基坐标系中的位置坐标信息，具体为：步骤4-1、根据深度相机内参，将每一张RGB-D图像转换为点云的PCD格式，并计算获得当前视角下的点云信息；步骤4-2、计算每个图像的外参，即每张图拍摄时相机在世界坐标系中的位姿，具体包括：步骤4-2-1、利用D-H参数法建立机械臂的正运动学模型，即机械臂基坐标系到机械臂末端坐标系的转换矩阵Tre(θi)，其中θi表示第i个关节所对应的角度，i＝1,...,6；步骤4-2-2、结合手眼标定矩阵、正运动学模型计算两个相机分别相对于其所在机械臂的基坐标系的位姿转换矩阵Trc，所用公式为：式中，为左相机相对于左机械臂的基坐标系的位姿转换矩阵，为右相机相对于右机械臂的基坐标系的位姿转换矩阵，为左机械臂本文档来自技高网...

【技术保护点】
1.一种基于点云的带电作业机器人自主路径规划方法，其特征在于，包括以下步骤：/n步骤1、在带电作业机器人双机械臂的末端均安装固定一个深度相机；/n步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；/n步骤3、控制双机械臂的基座关节旋转360°，且每旋转θ°深度相机采集一帧图像，获得当前视野中的环境信息；/n步骤4、将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在世界坐标系中的位置坐标信息；其中，世界坐标系为某一机械臂的基坐标系；/n步骤5、采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库以供后续新的路径规划参考使用。/n

【技术特征摘要】
1.一种基于点云的带电作业机器人自主路径规划方法，其特征在于，包括以下步骤：
步骤1、在带电作业机器人双机械臂的末端均安装固定一个深度相机；
步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；
步骤3、控制双机械臂的基座关节旋转360°，且每旋转θ°深度相机采集一帧图像，获得当前视野中的环境信息；
步骤4、将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在世界坐标系中的位置坐标信息；其中，世界坐标系为某一机械臂的基坐标系；
步骤5、采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库以供后续新的路径规划参考使用。


2.根据权利要求1所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤2所述进行手眼标定，具体为：
步骤2-1、初始化机械臂的状态，使机械臂处于指定位姿；
步骤2-2、根据预先设定好的机械臂的初始位姿，移动标定板至指定位置；
步骤2-3、控制机械臂运动，每移动一次获取标定图像和当前机械臂的各关节角度；
步骤2-4、判断是否达到指定的循环次数，若未达到，则返回步骤2-3；若达到，则分别求取左、右机械臂的手眼标定矩阵即相机坐标系到机械臂末端坐标系的转换矩阵，所用公式为：



式中，Pr表示各特征点在机械臂基坐标系下的坐标向量的集合矩阵，表示第i次采集的机械臂末端坐标系到机械臂基坐标系的转换矩阵，Pc(i)表示第i次采集的图像中的特征点在相机坐标系下的三维坐标的集合矩阵，表示机械臂的手眼标定矩阵，Constant表示常数，i＝1,2,…,s，s表示指定的循环次数。


3.根据权利要求1或2所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤2所述进行双机械臂基坐标系标定具体为：
利用标定板相对于两个机械臂基座的位置不变，结合手眼标定获得的两个机械臂的手眼关系和建立如下关系：



式中，分别为标定板坐标系到左、右相机坐标系的转换矩阵，分别为左、右机械臂的末端坐标系到其各自的基坐标系的转换矩阵；
由此求得双机械臂基坐标系的标定矩阵即两个机械臂基坐标系之间的转换关系



利用最小二乘法求得最优解。


4.根据权利要求3所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤4所述将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在机械臂基坐标系中的位置坐标信息，具体为：
步骤4-1、根据深度相机内参，将每一张RGB-D图像转换为点云的PCD格式，并计算获得当前视角下的点云信息；
步骤4-2、计算每个图像的外参，即每张图拍...

【专利技术属性】
技术研发人员：郭毓，肖潇，吴巍，杨光，张冕，蔡梁，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏;32