[发明专利]基于点云的带电作业机器人自主路径规划方法

权利要求书

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

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

步骤2、进行手眼标定和双机械臂基坐标系标定，实现两个深度相机的视野共享；所述两个深度相机包括左、右相机；所述进行手眼标定，具体为：

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

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

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

步骤2-4、判断是否达到指定的循环次数，若未达到，则返回步骤2-3；若达到，则求取机械臂的手眼标定矩阵，则根据步骤2-1至步骤2-4分别求取左、右机械臂的手眼标定矩阵

所述进行双机械臂基坐标系标定具体为：

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

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

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

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

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

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

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

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

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

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