CN110587600A - 基于点云的带电作业机器人自主路径规划方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于带电作业机器人自主作业领域，特别是一种基于点云的带电作业机器人自主路径规划方法。

背景技术

相较于传统的机器人应用的工厂环境，配电线路维护机器人需要在非结构化环境下进行作业，要实现机器人能够自主规划出无碰撞作业路径，机器人系统必须首先具备对周围环境的感知能力。然而带电作业的作业环境大多是非结构化环境，不同地点的作业环境均不相同，因此手工测量并构建固定作业场景模型的方法并不可行，则利用深度相机对作业场景进行三维重建尤为重要，利用深度相机对真实存在的三维场景或物体进行扫描，并根据获得的信息构建出机器人的作业场景。然而配电线路的带电作业需要在高空进行，而构建完整的作业环境需要从多个角度对作业环境信息进行采集，常用的多深度相机支架的方法无法直接使用。

此外，机器人在作业过程中不能与周围的电路设备以及环境中的其他障碍物发生碰撞，因此基于点云重构作业环境的障碍物分布，再根据机械臂起始和目标位姿，选择一条从起始点到达目标点的路径，然而多自由度机械臂就需要在高维空间进行路径规划，目前已有的路径规划算法大多是针对的二维或三维空间，对于高维度空间的路径规划存在实时性差且规划的路径难以最优等问题，目前已有的技术大多数是通过遥操作控制机械臂绕开障碍物，无法实现非结构化环境下的自主无障碍路径规划。

发明内容

本发明的目的在于提供一种带电作业机器人自主路径规划方法，以解决在高空非结构化环境中机械臂作业时可能会与环境中障碍物发生碰撞产生的安全问题。

实现本发明目的的技术解决方案为：一种基于点云的带电作业机器人自主路径规划方法，包括以下步骤：

步骤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；若达到，则分别求取左、右机械臂的手眼标定矩阵即相机坐标系到机械臂末端坐标系的转换矩阵，所用公式为：

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

进一步地，结合图4，步骤2中进行双机械臂基坐标系标定具体为：

利用标定板相对于两个机械臂基座的位置不变，结合手眼标定获得的两个机械臂的手眼关系和建立如下关系：

式中，分别为标定板坐标系到左、右相机坐标系的转换矩阵，分别为左、右机械臂的末端坐标系到其各自的基坐标系的转换矩阵；

由此求得双机械臂基坐标系的标定矩阵即两个机械臂基坐标系之间的转换关系

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

进一步地，步骤4中将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在机械臂基坐标系中的位置坐标信息，具体为：

步骤4-1、根据深度相机内参，将每一张RGB-D图像转换为点云的PCD格式，并计算获得当前视角下的点云信息；

步骤4-2、计算每个图像的外参，即每张图拍摄时相机在世界坐标系中的位姿，具体包括：

步骤4-2-1、利用D-H参数法建立机械臂的正运动学模型，即机械臂基坐标系到机械臂末端坐标系的转换矩阵T_r ^e(θ_i)，其中θ_i表示第i个关节所对应的角度，i＝1,...,6；

步骤4-2-2、结合手眼标定矩阵、正运动学模型计算两个相机分别相对于其所在机械臂的基坐标系的位姿转换矩阵T_r ^c，所用公式为：

式中，为左相机相对于左机械臂的基坐标系的位姿转换矩阵，为右相机相对于右机械臂的基坐标系的位姿转换矩阵，为左机械臂基坐标系到左机械臂末端坐标系的转换矩阵，为右机械臂基坐标系到右机械臂末端坐标系的转换矩阵；

步骤4-2-3、将其中一个机械臂的基坐标系作为世界坐标系，结合双机械臂基坐标系的标定矩阵将两个相机的位姿统一转换到世界坐标系中；假设将左机械臂的基坐标系作为世界坐标系，则右机械臂上的相机在世界坐标系中的位姿为：

式中，为右相机相对于其所在右机械臂的基坐标系的位姿转换矩阵；

步骤4-3、结合相机在世界坐标系中的位姿，将两个相机坐标系中的点云数据分别转换到世界坐标系中，获得工作空间中障碍物在世界坐标系中的位置坐标，左机械臂的相机获得的点云数据转换公式为：

式中，为左相机相对于其所在左机械臂的基坐标系的位姿转换矩阵，X₁为左相机获得的点云坐标，X₁'为左相机对应点云在世界坐标系下的坐标；

右机械臂的相机获得的点云数据转换公式为：

式中，X₂为右相机获得的点云坐标，X₂'为右相机对应点云在世界坐标系下的坐标。

示例性地，步骤3中θ°＝60°。

进一步地，步骤5中改进的路径规划算法具体包括：

(1)扩展新节点时，按照规划树中的节点与随机点之间的路径代价大小升序排列，然后依次选最小的N个构型与随机构型进行碰撞检测，若不发生碰撞，则跳出判断循环，建立该节点与随机点间的连接；所述构型表示笛卡尔空间内的节点对应到机械臂关节空间内各个关节的角度值；

(2)对于父节点的选择也通过先排序再检测的方法；

(3)若规划出从姿态A至姿态B的安全路径，则将该路径加入数据库，在后续重新规划新路径时优先考虑将已有的安全路径加入新的规划路径中；

(4)在路径规划过程中，采用离散型的碰撞检测算法对机械臂是否与环境中的障碍物发生碰撞进行检测，对空间机械臂模型和障碍进行简化和数学描述，利用相互叠加的球体对采集到的障碍物的点云模型进行包络，将机械臂连杆的径向最大包络半径叠加到障碍物的厚度上，并将判断过的碰撞位姿和安全位姿分别加入对应的数据库，后续在规划新路径时对已判断过的位姿状态进行查询。

本发明针对带电作业机器人在非结构化的高空作业环境中运动时易与周围环境障碍物发生碰撞的难题，提出了一种基于点云的自主路径规划方法，通过采用眼在手上的安装方式在双机械臂末端安装深度相机，结合机械臂的运动学模型，构建整个作业环境的三维点云图，使系统具备对环境障碍物的感知能力；再应用改进的路径规划算法，确保机械臂不与环境障碍物发生碰撞的同时，能够实时规划出机械臂的最优运动路径。

Claims (6)

1.一种基于点云的带电作业机器人自主路径规划方法，其特征在于，包括以下步骤：

步骤1、在带电作业机器人双机械臂的末端均安装固定一个深度相机；

步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；

步骤3、控制双机械臂的基座关节旋转360°，且每旋转θ°深度相机采集一帧图像，获得当前视野中的环境信息；

步骤4、将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在世界坐标系中的位置坐标信息；其中，世界坐标系为某一机械臂的基坐标系；

步骤5、采用改进的路径规划算法，实时规划机械臂的无碰撞最优运动路径，同时将该路径加入数据库以供后续新的路径规划参考使用。

2.根据权利要求1所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤2所述进行手眼标定，具体为：

步骤2-1、初始化机械臂的状态，使机械臂处于指定位姿；

步骤2-2、根据预先设定好的机械臂的初始位姿，移动标定板至指定位置；

步骤2-3、控制机械臂运动，每移动一次获取标定图像和当前机械臂的各关节角度；

步骤2-4、判断是否达到指定的循环次数，若未达到，则返回步骤2-3；若达到，则分别求取左、右机械臂的手眼标定矩阵即相机坐标系到机械臂末端坐标系的转换矩阵，所用公式为：

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

3.根据权利要求1或2所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤2所述进行双机械臂基坐标系标定具体为：

利用标定板相对于两个机械臂基座的位置不变，结合手眼标定获得的两个机械臂的手眼关系和建立如下关系：

式中，分别为标定板坐标系到左、右相机坐标系的转换矩阵，分别为左、右机械臂的末端坐标系到其各自的基坐标系的转换矩阵；

由此求得双机械臂基坐标系的标定矩阵即两个机械臂基坐标系之间的转换关系

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

4.根据权利要求3所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤4所述将步骤3采集到的图像进行拼接，获得机械臂工作空间的完整环境信息，即工作空间中障碍物在机械臂基坐标系中的位置坐标信息，具体为：

步骤4-1、根据深度相机内参，将每一张RGB-D图像转换为点云的PCD格式，并计算获得当前视角下的点云信息；

步骤4-2、计算每个图像的外参，即每张图拍摄时相机在世界坐标系中的位姿，具体包括：

步骤4-2-1、利用D-H参数法建立机械臂的正运动学模型，即机械臂基坐标系到机械臂末端坐标系的转换矩阵其中θ_i表示第i个关节所对应的角度，i＝1,...,6；

步骤4-2-2、结合手眼标定矩阵、正运动学模型计算两个相机分别相对于其所在机械臂的基坐标系的位姿转换矩阵所用公式为：

式中，为左相机相对于左机械臂的基坐标系的位姿转换矩阵，为右相机相对于右机械臂的基坐标系的位姿转换矩阵，为左机械臂基坐标系到左机械臂末端坐标系的转换矩阵，为右机械臂基坐标系到右机械臂末端坐标系的转换矩阵；

步骤4-2-3、将其中一个机械臂的基坐标系作为世界坐标系，结合双机械臂基坐标系的标定矩阵将两个相机的位姿统一转换到世界坐标系中；假设将左机械臂的基坐标系作为世界坐标系，则右机械臂上的相机在世界坐标系中的位姿为：

式中，为右相机相对于其所在右机械臂的基坐标系的位姿转换矩阵；

步骤4-3、结合相机在世界坐标系中的位姿，将两个相机坐标系中的点云数据分别转换到世界坐标系中，获得工作空间中障碍物在世界坐标系中的位置坐标，左机械臂的相机获得的点云数据转换公式为：

式中，X₁为左相机获得的点云坐标，X₁'为左相机对应点云在世界坐标系下的坐标；

右机械臂的相机获得的点云数据转换公式为：

式中，X₂为右相机获得的点云坐标，X₂'为右相机对应点云在世界坐标系下的坐标。

5.根据权利要求1所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤3所述θ°＝60°。

6.根据权利要求1所述的基于点云的带电作业机器人自主路径规划方法，其特征在于，步骤5所述改进的路径规划算法具体包括：

(1)扩展新节点时，按照规划树中的节点与随机点之间的路径代价大小升序排列，然后依次选最小的N个构型与随机构型进行碰撞检测，若不发生碰撞，则跳出判断循环，建立该节点与随机点间的连接；所述构型表示笛卡尔空间内的节点对应到机械臂关节空间内各个关节的角度值；

(2)对于父节点的选择也通过先排序再检测的方法；

(3)若规划出从姿态A至姿态B的安全路径，则将该路径加入数据库，在后续重新规划新路径时优先考虑将已有的安全路径加入新的规划路径中；

(4)在路径规划过程中，采用离散型的碰撞检测算法对机械臂是否与环境中的障碍物发生碰撞进行检测，对空间机械臂模型和障碍进行简化和数学描述，利用相互叠加的球体对采集到的障碍物的点云模型进行包络，将机械臂连杆的径向最大包络半径叠加到障碍物的厚度上，并将判断过的碰撞位姿和安全位姿分别加入对应的数据库，后续在规划新路径时对已判断过的位姿状态进行查询。