CN107214701A - 一种基于运动基元库的带电作业机械臂自主避障路径规划方法 - Google Patents

Abstract

本发明提出一种基于运动基元库的带电作业机械臂自主避障路径搜索方法。建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；分类设计运动基元库，为各类库设计相应运动基元作为库成员；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)以及启发规则，实现启发搜索；计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件进行路径规划。本发明能快速、精确构建机械臂作业场景信息，保证避障路径搜索结果具有次优性和完备性，满足动力学约束前提下能快速完成路径规划，保证机械臂运动过程的平稳性。

Description

一种基于运动基元库的带电作业机械臂自主避障路径规划 方法

技术领域

本发明属于带电作业机器人领域，具体涉及多自由度机械臂工作环境三维重建及自主动态避障路径规划，尤其是面向自主带电作业机械臂路径规划的方法。

背景技术

目前我国高压带电作业方式仍停留在人工作业和机械臂遥操作作业阶段，人工作业时技术人员暴露在高压环境中，危险性极大；遥操作需要确保操作过程中机械臂不与杆塔元器件碰撞且最多只与一根电线接触，这对技术人员在现场观察、预判及精细操作方面的能力提出了很高的要求。

C-空间法在解决移动机器人路径规划问题方面是一种经典且有效的算法，但如何在高维C-空间中快速搜索出满足运动学和障碍物约束的路径并保证路径最优，是多自由度机械臂路径规划问题的关键。基于采样的路径规划算法解决了高维C-空间计算量庞大的问题，但其性能取决于采样方法和节点间连接方式且具有随机性；人工势场法在高维C-空间中使用时计算量庞大和局部最小值问题仍未得到很好的解决；启发式搜索算法通过建立代价方程保证路径的次优且能确保搜索的完备性，但对于多自由度机械臂问题，传统启发式搜索算法庞大的计算量使之难以实现工程应用。对于上述方法，还存在一个重要问题，它们无法在搜索过程中保证搜索结果满足关节速度、加速度的运动学约束以及机械臂运动过程的平稳性。

机器人作业环境的三维实时重构技术领域，基于点云和深度相机的现场重建技术得到广泛运用，基于双目相机的三维测量技术也取得了一定的成功，但如何提高复杂环境下三维重建的精度和实时性仍是一个有待突破的问题。

发明内容

本发明提出一种基于运动基元库的带电作业机械臂自主避障路径搜索方法，能快速、精确构建机械臂作业场景信息，保证避障路径搜索结果具有次优性和完备性，满足动力学约束前提下能快速完成路径规划，保证机械臂运动过程的平稳性。

为了解决上述技术问题，本发明提供一种基于运动基元库的带电作业机械臂自主避障路径规划方法，步骤如下：

步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；

步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；

步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)以及启发规则，实现启发搜索；

步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步。

调用机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)、关节空间距离检测函数D2_F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)函数，将函数计算结果与区域触发阈值K1_F、K2_F及K_O进行比较，根据判断条件选择执行以下步骤：

条件1：若D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)>K_O，则顺序执行步骤五；

条件2：若D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)<K_O，则跳转至步骤六；

条件3：若D1_F(q)<A_F1或D2_F(q)<A_F2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；

条件4：其他情况跳转至步骤七；

步骤五：在降维稀疏的关节空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果。

步骤六：在非降维稀疏关节空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；

步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。

进一步，步骤一中，采用DH参数法分别建立机械臂完整运动学模型T₀和简化运动学模型T₁分；根据国家标准和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型；

进一步，建立机械臂完整运动学模型T₀时，以末端手爪的末端中心为原点建立工具坐标系，以基座中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型T₁时，以末端手爪的末端中心为原点建立工具坐标系，以基座中心为原点建立基座坐标，将第四关节、第五关节、第六关节、双目相机以及末端手爪视为第三关节的机械结构延伸，除了随动于第三关节外没有相对运动。

进一步，步骤二中，运动基元库包括单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规划库CP_LIB；单关节空间轨迹规划库SP_LIB以单关节空间轨迹规划结果作为库的运动基元，用于构建离散C-空间；当机械臂接近目标位姿时，使用自适应逆运动学规划库AP_LIB自适应地建立精确到达指定位姿的轨迹；笛卡尔空间轨迹规划库CP_LIB用于快速脱离障碍物区域。

进一步，步骤三中，初始化机械臂工作环境参数时，首先初始化世界坐标系W₀，建立机械臂末端与世界坐标系的变换矩阵T_W初始化机械臂当前关节空间状态S₀；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w₀位姿坐标O_w，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建；距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)和关节空间距离检测函数D2_F(q)；危险区域阈值包括笛卡尔空间距离末端区域触发阈值K1_F、关节空间距离末端区域触发阈值K2_F、笛卡尔空间距离障碍物区域触发阈值K_O以及笛卡尔空间距离紧急避障区域阈值K_M；目标点精度范围阈值包括笛卡尔空间阈值A_F1和关节空间阈值A_F2。

进一步，步骤五中，根据当前机械臂关节状态参数，采用简化机械臂运动学模型T₁，在降维离散C-空间中使用启发式搜索算法，遍历步骤二中定义的单关节空间轨迹规划库SP_LIB库中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，更新机械臂状态，跳转至步骤四。

进一步，步骤六中，判断与障碍物距离是否小于紧急值K_M，若不小于则进入非紧急状态，遍历步骤二中所有单关节空间轨迹规划库SP_LIB中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)；若小于则进入紧急避障状态，计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向，利用笛卡尔空间轨迹规划库CP_LIB中基元进行紧急避障轨迹规划，快速脱离障碍物区域；执行此步骤后更新机械臂状态，跳转至步骤四。

进一步，步骤七中，当机械臂在关节空间的距离不大于阈值K2_F时进行协同运动规划；当仅笛卡尔空间距离达到阈值K1_F时进行单关节独立规划；根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索，调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，执行后更新机械臂状态，跳转至步骤四。

本发明与现有技术相比，其显著优点在于，

(1)本发明可实现多自由度机械臂在高维C-空间中快速完成路径搜索，保证结果路径具有次优性且满足关节角速度、角加速度及角加加速度的运动学约束，实现机械臂平稳运动；

(2)本发明利用带电作业场景的标准化信息对机器人作业场景进行标准化模板建模，大幅提高作业环境三维实时重建的速度和精度；

(3)本发明提出基于运动基元库来实现机械臂路径规划，实质上对高维C-空间进行了稀疏离散，解决了已有启发式搜索算法因计算量庞大不适用于多关节机械臂路径规划的问题，提高了路径规划实时性；

(4)本发明提出对带电作业环境进行模块化建模，根据标准件参数和标准安装尺寸建立作业环境模板，关键部位进行视觉测量后对模板自适应进行调整即可获取实时3D重建环境，降低了障碍物重建计算量和视觉测量难度；

(5)本发明对运动基元按运动学约束进行运动学规划，以运动基元作为启发搜索的搜索单元，保证了搜索结果路径满足运动学约束条件和机械臂运动过程的平稳性；

(6)本发明设计了笛卡尔空间和关节空间两种启发函数，根据实际使用情况具体调用，提高了启发效率；

(7)本发明设计了多类运动运动基元，实质上对整个工作空间进行区域划分，路径规划任务分解为粗、细和精三个阶段，提高了路径规划效率；

附图说明

图1是本发明的自主避障路径规划机械臂系统硬件组成示意图。

图2是为实现机械臂自主避障路径规划设计的机械臂结构示意图。

图3是用简单几何体对杆塔器件进行包覆简化的示意图。

图4是创建的带电作业模板环境中的杆塔接线场景示意图。

图5是单关节运动基元轨迹规划流程示意图。

图6是本发明方法流程示意图。

图7是本发明实施例中机械臂运动学模型坐标系关系示意图。

具体实施方式

容易理解，依据本发明的技术方案，在不变更本发明的实质精神的情况下，本领域的一般技术人员可以想象出本发明基于运动基元库的带电作业机械臂自主避障路径规划方法的多种实施方式。因此，以下具体实施方式和附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限制或限定。

本发明所应用的带电作业机械臂的一个实施方式如图1所示。带电作业机械臂由主机、主机分屏显示器、深度相机(点云相机)、彩色相机、通信线缆以及机械臂子系统构成。主机用来处理深度相机和彩色相机采集的实时环境数据，用于作业场景信息的实时构建；分屏显示器用于实时显示视频信息和场景构建信息，兼具实时显示和安全监控的功能；机械臂子系统接收并执行来主机的运动指令，并向主机实时反馈机械臂运动学信息，机械臂上带有双目视觉系统，用于弥补固定相机在实际工作时存在的遮挡和盲区问题，可同时提供一维视角的监控功能。

机械臂具体结构如图2所示，机械臂由6个旋转关节串联而成，包括基座20、第一关节21、第二关节22、第三关节23、第四关节24、第五关节25、第六关节26、双目相机27以及末端手爪28。第一关节21、第二关节22、第三关节23分别为机械臂的腰、大臂及小臂；第四关节24、第五关节25、第六关节26构成机械臂的腕关节。

上述机械臂基于运动基元库的带电作业机械臂自主避障路径规划方法，其步骤如下：

步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境。

采用DH参数法分别建立机械臂完整运动学模型和简化运动学模型分别记为T₀和T₁(T₀、T₁为相应的机械臂坐标变换矩阵)。根据国标和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装设计，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型，简化计算。

建立机械臂完整运动学模型T₀时，以末端手爪28末端中心为原点建立工具坐标系，以基座20中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型T₁时，仍以末端手爪28末端中心为原点建立工具坐标系，以基座20中心为原点建立基座坐标，但将第四关节24、第五关节25、第六关节26、双目相机27以及末端手爪28视为第三关节23的机械结构延伸，除了随动于第三关节23外没有相对运动，此时相当于只需建第一关节21、第二关节22、第三关节23这三个独立关节坐标系。

以图3作为杆塔元器件包覆简化设计为例，图中31为耐张绝缘子及其包覆，32为横担及其包覆简化，33为杆塔及其包覆简化。

以杆塔连接线场景作为场景模型库封装设计说明实例，如图4所示，其中41为耐张线夹，42合成绝缘子，43并沟线夹，44导线，45为杆塔，46为悬垂绝缘串，47为紧固圈。

步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员。

根据具体需要，设计三类运动基元库：单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规划库CP_LIB。单关节空间轨迹规划库SP_LIB以单关节空间轨迹规划结果作为库的运动基元，用于构建离散C-空间，这样的C-空间是稀疏的且其中任何两点间运动过程均满足单关节运动学约束；C-空间中目标状态点是随机的，仅靠单关节空间轨迹规划库SP_LIB库中的基元不能保证任意状态点可达，因此需要建立自适应逆运动学规划库AP_LIB，当机械臂接近目标位姿时自适应的建立精确到达指定位姿的轨迹；考虑到避障规划过程中可能出现搜索陷入障碍物陷阱区域的情况，建立笛卡尔空间轨迹规划库CP_LIB用于快速脱离障碍物区域。

步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建启发函数和启发规则，实现启发搜索。

初始化机械臂工作环境参数时，首先初始化世界坐标系W₀(默认与机械臂基座中心重合)，建立机械臂末端与世界坐标系的变换矩阵T_W初始化机械臂当前关节空间状态S₀。然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w₀位姿坐标O_w，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建。

距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)和关节空间距离检测函数D2_F(q)。目标点约束包括位置和姿态，故需关节空间和笛卡尔空间两种距离检测函数，只有当两者都归零或在一定精度范围内时才可认为到达目标点。危险区域阈值包括设置末端区域触发阈值K1_F(笛卡尔空间距离)、K2_F(关节空间距离)和障碍物区域触发阈值K_O(笛卡尔空间距离)，设置紧急避障区域阈值K_M(笛卡尔空间距离)。目标点精度范围阈值包括笛卡尔空间阈值A_F1和关节空间阈值A_F2。

为实现启发搜索，构造并初始化笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)实现启发计算，设计启发搜索计算过程，实现对基元库中成员的启发计算，获取指定运动基元库中基元启发性能的降序排列。

步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步。

调用机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)、关节空间距离检测函数D2_F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)函数，将函数计算结果与区域触发阈值K1_F、K2_F及K_O进行比较，根据判断条件选择执行以下步骤：

条件1：若：D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)>K_O；顺序执行步骤五；

条件2：若：D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)<K_O，跳转步骤六；

条件3：若：D1_F(q)<A_F1或D2_F(q)<A_F2，判断机械臂末端到达期望位姿态，获得搜索结果跳转步骤八；

条件4：其他情况跳转步骤七；

步骤五：在降维稀疏的关节空间中使用单关节空间轨迹规划库SP_LIB库执行启发式搜索，获得当前库中最优运动基元作为规划结果。

根据当前机械臂关节状态参数，采用简化机械臂运动学模型T₁，在降维离散C-空间中使用启发式搜索算法，遍历步骤二中定义的单关节空间轨迹规划库SP_LIB库中的运动基元进行启发搜索(只遍历21,22,23前三关节运动基元)，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，更新机械臂状态，跳转步骤四。

步骤六：在非降维稀疏关节空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB库执行启发式搜索，获得当前库中最优运动基元作为规划结果。

首先判断与障碍物距离是否小于紧急值K_M。若不小于则进入非紧急状态，遍历步骤二中所有单关节空间轨迹规划库SP_LIB中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)；若小于则进入紧急避障状态，计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向，利用笛卡尔空间轨迹规划库CP_LIB中基元进行紧急避障轨迹规划，快速脱离障碍物区域。执行此步骤后更新机械臂状态，跳转步骤四。

步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。

机械臂位姿此时已经非常接近目标位置点，步骤2中的单关节空间轨迹规划库SP_LIB和笛卡尔空间轨迹规划库CP_LIB均不能保证精确达到指定C-空间状态S_F，故采用自适应逆运动学解(AP_LIB库)方法。

这里的自适应库有两种。第一种是多关节同时进行轨迹规划MAP_LIB，实现协同运动；第二种是单关节独立进行规划SAP_LIB，根据具体需求进行选择。当机械臂在关节空间的距离不大于阈值K2_F时进行协同运动规划；当仅笛卡尔空间距离达到阈值K1_F时进行单关节独立规划。根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索，调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，执行后更新机械臂状态，跳转步骤四。

步骤八：跳出计算过程，保存路径搜索结果，清理缓存和状态参数，等待指令执行下一次路径规划任务。

实施例

步骤1，建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境，具体按如下步骤进行：

步骤1-1根据厂家提供的D-H参数按式(1-2)建立机械臂关节坐标变换矩阵T₀，将T₀作为机械臂完整运动学模型，具体实施如下。其中，T₀以28末端中心为原点建立工具坐标系，以20中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系w_i，建立的机械臂完整运动学模型的机械臂关节几何关系如附图7所示；式(1-2)中为第i关节坐标系w_i到第i-1关节坐标系w_i-1的变换矩阵，具体形式如式(1-1)，其中α_i为两相邻关节坐标系z_i-1轴到z_i轴之间的夹角，a_i为两个关节轴间距离，d_i为w_i-1与w_i坐标系沿坐标轴方向的偏移距离，θ_i为第i关节旋转角度，

由上述方法可以获取机械臂末端到机械臂基座的变换矩阵T₀如式(1-2)，其中n_x、n_y、n_z,o_x、o_y、o_z，a_x、a_y、a_z以及P_x、P_y、P_z分别简记变换矩阵各项参数，需要注意的是最后一个连杆坐标系原点应该是机械臂抓手末端中心点。

步骤1-2根据机械臂执行路径规划时的初始位置，按式(1-3)建立机械臂坐标变换矩阵T₁，将T₁作为机械臂简化运动学模型，具体实施如下。当指定机械臂起始位姿后，T₁仍以28末端中心为原点建立工具坐标系，以20中心为原点建立基座坐标，但将24、25、26、27、28视为23的机械结构延伸，除了随动于23外没有相对运动，此时相当于只需建立21、22及23这三个独立关节的关节坐标系，即附图7中w₀、w₁、w₂、w₃和w₆。此时变换矩阵计算方法如式(1-3)，其中n1_x、n1_y、n1_z,o1_x、o1_y、o1_z，a1_x、a1_y、a1_z以及P1_x、P1_y、P1_z分别简记T₁变换矩阵各项参数

步骤1-3杆塔元器件为标准件，故根据标准几何特征参数将各元器件利用简单几何体进行包覆替代，简化碰撞检测计算，具体实施如下。实施中将绝缘子用圆柱体包覆简化；桁架用长方体包覆简化；电线用圆筒包覆替代；其他元器件运用类似方法，使用圆柱体、球体或者长方体进行包覆简化，不再赘述。以附图3作为杆塔元器件包覆简化的说明实例，如附图中31为耐张绝缘子及其包覆，32为横担及其包覆简化，33为杆塔及其包覆简化。

步骤1-4建立作业环境的场景模型库，加速机器人三维工作环境实时建模速度，具体实施如下。根据作业杆塔的标准安装参数，建立描述作业环境的场景模板库。包括：接搭线模板环境、绝缘子模板环境、隔离刀闸模板环境、变压器模板环境。以杆塔连接线场景作为场景模型库封装设计说明实例，如附图4所示，其中41为耐张线夹，42合成绝缘子，43并沟线夹，44导线，45为杆塔，46为悬垂绝缘串，47为紧固圈。

步骤2分类设计运动基元库，为各类库设计相应运动基元作为库成员，具体包括如下步骤：

步骤2-1设计第一类基元库：单关节空间轨迹规划库(SP_LIB)，对单关节独立运动过程按式(2-1)所示加速度变化规则进行轨迹规划，将规划结果作为库中成员，轨迹规划结果曲线规律如附图5所示，具体实施如下。各关节运动基元根据设备给定的参考运动学参数进行设计，此实施例加速度轨迹方程如式(2-1)所示，加速度-时间曲线由正弦曲线段与坐标轴段组成，加速度、加加速度曲线具有连续性，保证了轨迹的平稳性；速度-时间曲线光滑连续，速度分为加速度段、匀速段和减速段，这样的运动规律可降低功耗，减小抖动和冲击，提高运动平稳性。式(2-1)中acc_max为加速度峰值、T为加速度曲线正弦段三角函数周期、T₀是加速度为负值的起点时刻、t为规划过程中的时间变量，进而可以推导得到关节匀速值基元运动周期其中acc_max、T以及T₀均可以根据机械臂运动学参数范围和实际需求进行调整。

每个关节均需建立独立的轨迹规划过程，同时每个关节运动有顺时针和逆时针运动两种情况。故6关节机械臂SP_LIB库需要12个库成员。每一种特定情况只需按照式(2-1)调整三个参数即可完成规划过程。

步骤2-2设计第二类基元库：自适应逆运动学轨迹规划库(AP_LIB)，利用式(2-3)获取机械臂目标关节角和起始关节角，随后运用式(2-2)方程进行单关节轨迹规划，将规划结果作为库中成员，具体实施过程如下。C空间中两点之间的轨迹规划采用三阶四次多项式进行拟合，方程形式如式(2-2)所示。其中θ_i(t)(i＝1…n)为第i关角度，c₁、c₂、c₃及c₄分别为可求解系数，t为时间变量。

θ_i(t)＝c₁t³+c₂t²+c₃t+c₄ (2-2)

建立轨迹起始和末端的速度和加速度均为零的约束，起始和终止角度均已知或可由式(2-3)求解，故式中c₁、c₂、c₃及c₄均可确定。这里将规划结果分为两种情况：第一，6个关节同时进行轨迹规划，轨迹运动在时间上同步；第二，6个关节独立进行规划(两种情况共有7个库成员)。关节独立进行轨迹规划时，对各关节依次按式(2-2)进行独立轨迹规划即可；多关节协同规划时，按式(2-2)进行轨迹计算中，各个关节计算过程中的时间t的步长保持一致，则可得到多关节协同运动结果。

当给定的目标点为笛卡尔空间下位姿时，根据T₀、T₁变换矩阵可以得到机械臂的逆运动学解，若是机械臂简化模型，则θ₄、θ₅、θ₆无需求解。f_k(x,y,z,α,β,γ),k∈[1,6]为第k关节的逆运动学解方程，其中x、y、z为机械臂末端笛卡尔空间中位置坐标，α、β、γ为笛卡尔空间中姿态坐标。若机械臂结构不满足封闭逆运动学解条件，则进行数值迭代求解。求解的结果记为T^-1(x,y,z,α,β,γ)，逆解形式如式(2-3)所示。

已知当前关节角度，利用式(2-3)获取期望关节角度后联立式(2-2)即可实现AP_LIB库下的轨迹规划。

步骤2-3设计第三类基元库：笛卡尔空间轨迹规划库(CP_LIB)，按式(2-4)、(2-5)确定笛卡尔空间中路径基元方向，按式(2-6)获取笛卡尔空间中路径点序列，对两相邻路径点调用步骤2-2中关节空间轨迹规划过程可完成运动基元设计，将规划结果作为库的成员，具体实施如下。在笛卡尔空间中规划机械臂末端路径轨迹时，路径的运动基元为笛卡尔空间中直线段。首先计算机械臂末端与障碍物笛卡尔空间距离梯度方向作为直线段轨迹方向，计算按式(2-4)、(2-5)进行，其中与障碍物距离计算方程D_O ^c(x,y,z)定义如式(2-4)，基元直线段长度d_l根据场景环境具体设置，直线插值按式(2-6)进行。为避开奇异点，根据机械臂结构参数，预先求解出机械臂在笛卡尔空间中奇异点位置集S＝{s₁,s₂…s_n}，建立笛卡尔空间中机械臂末端与奇异点距离检测函数D_s(x,y,z)如式(2-5)。当D_s(x,y,z)计算的距离大于阈值D_Fs时，正常执行基元规划过程；当计算距离小于阈值时，以奇异点为圆心，机械臂当前位置为半径随机做圆与预期直线段的交于两点，圆形中的直线段以随机圆产生的圆弧代替，其它部分保持不变。这样的轨迹修正函数可以实现避免奇异点的目的。

上述式(2-4)、(2-7)中(x，y，z)分别笛卡尔空间中机械臂末端位置坐标，(x₀，y₀，z₀)为障碍物位置坐标，(x₁，y₁，z₁)为奇异点位置坐标；式(2-5)中(d_x,d_y,d_z)为距离增加的单位梯度方向；式(2-6)中N为设置的路径点序列数，d_l为基元路径直线长度，x(n)、y(n)及z(n)分别为第n个序列点在笛卡尔空间中的x、y、z坐标。

步骤3初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建启发函数和启发规则，实现启发搜索。具体按如下步骤进行：

步骤3-1完成机械臂工作环境的搭建和环境参数初始化，具体实施如下。首先初始化世界坐标系w₀(默认与机械臂基座中心重合，如附图7所示)；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1-4中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w₀位姿坐标O_w，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建。

步骤3-2建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度触发阈值，具体实施如下。建立机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)和关节空间距离检测函数D2_F(q)(注：目标点约束包括位置和姿态，故需关节空间和笛卡尔空间两种距离检测函数，只有当两者都归零或在一定精度范围内时才可认为到达目标点)。设置末端区域触发阈值K1_F(笛卡尔空间距离)、K2_F(关节空间距离)和障碍物区域触发阈值K_O(笛卡尔空间距离)，设置紧急避障区域阈值K_M(笛卡尔空间距离)，设置到达目标点精度范围阈值,A_F1为笛卡尔空间阈值，A_F2为关节空间阈值。

上述步骤3-2中D_o(q)的计算具体按如下步骤进行：

步骤S210根据步骤3-1建立的环境参数获取障碍物最新位姿[b_x,b_y,b_z,b_α,b_β,b_γ]，其中b_x、b_y、b_z、b_α、b_β及b_γ分别为障碍物在笛卡尔空间中的(x,y,z,α、β、γ)位姿坐标，获取机械臂当前关节空间状态参数q＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]，其中θ₁、θ₂、θ₃、θ₄、θ₅、θ₆分别为第1到6关节的关节角。

步骤S211利用步骤1-1建立的机械臂运动学模型，按式(3-1)计算获取当前机械臂末端在笛卡尔空间中位姿坐标[c_x,c_y,c_z,c_α,c_β,c_γ]^T，其中c_x、c_y、c_z、c_α、c_β及c_γ分别为机械臂在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ；

步骤S212计算机械臂末端到障碍物的笛卡尔空间距离为

上述步骤3-2中D1_F(q)的计算具体按如下步骤进行：

步骤S220根据步骤3-1建立的环境参数获取当前目标点在笛卡尔空间下最新位姿参数[o_x,o_y,o_z,o_α,o_β,o_γ]^T，其中o_x、o_y、o_z、o_α、o_β、o_γ分别目标点为在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ；获取机械臂当前关节空间状态参数q＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]；

步骤S221利用步骤1-1建立的机械臂运动学方程，根据式(3-1)获取当前机械臂末端在笛卡尔空间中位姿[c_x,c_y,c_z,c_α,c_β,c_γ]^T；

步骤S222计算机械臂末端到目标点的笛卡尔空间距离为

上述步骤3-2中D2_F(q)的计算具体按如下步骤进行：

步骤S230根据步骤3-1建立的环境参数获取当前目标点最新位姿[o_x,o_y,o_z,o_α,o_β,o_γ]^T；获取机械臂当前关节空间状态参数q＝[θ₁,θ₂,θ₃,θ₄,θ₅,θ₆]；

步骤S231根据式(2-3)逆运动学解方程T^-1(x,y,z,α,β,γ)求取目标点在机械臂关节空间中坐标为[θ₁′,θ₂′,θ₃′,θ₄′,θ₅′,θ₆′]，其中θ₁′、θ₂′、θ₃′、θ₄′、θ₅′、θ₆′分别对应机械臂第1到6关节的关节角度；

步骤S232计算关节空间距离可得：

步骤3-3创建自定义启发函数，设计启发搜索计算过程，实现对基元库中成员的启发计算，获取指定运动基元库中基元启发性能的降序排列，具体实施如下。更新机械臂当前关节空间状态S₀＝[θ₁₀,θ₂₀,θ₃₀,θ₄₀,θ₅₀,θ₆₀]，根据式(2-3)获取机械臂期望关节位置S_F＝[θ_1F,θ_2F,θ_3F,θ_4F,θ_5F,θ_6F](注：若机械臂给定的目标末端位置是笛卡尔空间中的位姿点，则机械臂逆解存在多解的情况，这里对最优解的筛选按不碰撞、关节空间距离最小两个指标进行)。构造并初始化笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)实现启发计算。本发明设计的启发过程是基于运动基元库的，在路径启发搜素时遍历当前运动库中有限的运动基元即可完成启发计算。

上述步骤3-3中启发函数构造具体按如下步骤：

步骤3-3-1构造笛卡尔空间中的启发函数h_d(x)和启发计算规则，获取指定运动基元库中基元启发性能的降序排列。

确定当前选中运动基元库，定义以笛卡儿空间距离为基础的启发函数h_c(x)＝f(x₀x,ε),此处x₀为机械臂当前关节空间状态，x为运动基元，ε∈[0,1]为比例系数，这里的启发函数计算采用遍历当前库中运动基元的方式实现。根据库中选定的运动基元获取关节空间新状态x₀′，调用式(3-2)D_O(q)函数计算新状态下末端点与障碍物的笛卡儿空间距离do(k)(K为运动基元的编号)，按距离由大到小给选定库中运动基元赋值do(k)∈{1,2,…,n}(即max(do(k))＝1,min(do(k))＝n),其中n为运动基元总数；调用D1_F(q)函数，计算新状态下末端点与期望位置的笛卡儿空间距离da(k)，按距离由小到大给运动基元赋值da(k)∈{1,2,…,n}(即max(do(k))＝n，min(do(k))＝1)。启发性能指标为P＝ε*da(k)+(1-ε)*do(k)，基元性能按P值升序排列，相同P值取da(k)小者性能更佳。

其中计算新状态下do(k)与da(k)包括如下步骤：

步骤S310根据定h_d(x)＝f(x₀,x,ε),首先更新获取机械臂当前关节空间状态x₀＝s₀＝[θ₁₀,θ₂₀,θ₃₀,θ₄₀,θ₅₀,θ₆₀],计算加入运动基元后机械臂笛卡尔空间坐标，包括如下两个判断计算步骤：

步骤S310-1若当前选定库为步骤2中AP_LAB库或SP_LIB库，则基元形式为：

x＝[Δθ₁,Δθ₂,Δθ₃,Δθ₄,Δθ₅,Δθ₆]

新状态x₀′为：

x₀′＝[θ₁+Δθ₁,θ₂+Δθ₂,θ₃+Δθ₃,θ₄+Δθ₄,θ₅+Δθ₅,θ₆+Δθ₆]

新状态下，机械臂末端笛卡尔空间位姿坐标c′为：

步骤S310-2当前选定库为步骤2中CP_LIB库，则基元形式为：

x＝[Δc_x,Δc_y,Δc_z,Δc_α,Δc_β,Δc_γ]^T

根据x₀＝s₀,获取当前机械臂笛卡尔空间位姿坐标为：

计算加入运动基元后的新坐标为：

c′＝[c_x+Δc_x,c_y+Δc_y,c_z+Δc_z,c_α+Δc_α,c_β+Δc_β,c_γ+Δc_γ]^T (3-7)

步骤S311计算机械臂末端与障碍物距离：

获取当前障碍物最新位姿[b_x,b_y,b_z,b_α,b_β,b_γ]^T，调用式(3-1)距离计算函数如下

步骤S312计算机械臂末端与目标点距离：

获取当前目标点最新位姿[o_x,o_y,o_z,o_α,o_β,o_γ]^T,调用式(3-1)距离计算函数如下

步骤S313返回针对当前库中第k个运动基元的计算结果do(k)＝d_co和da(k)＝d_cs。

步骤3-3-2构造关节空间中的启发函数h_c(x)和启发计算规则，获取指定运动基元库中基元启发性能的降序排列。

确定当前选中运动基元库，定义以关节空间距离为基础的启发函数h_c(x)＝f(x₀x,ε),此处x₀为机械臂当前关节空间状态，x为运动基元，ε∈[0,1]为比例系数。这里的启发函数计算采用遍历当前库中运动基元的方式实现，根据库中选定的运动基元获取关节空间新状态x₀′，判断各关节角度当前值与期望值的差值ea(k)(k表示库中第k个基元)，按照|ea(k)|从小到大给运动基元赋值ea(k)∈{1,2,…,n}(n为关节数)，即max(|ea(k)|)＝n，min(|ea(k)|)＝1；；调用D2(q)函数，计算新状态与障碍物笛卡尔空间距离，按距离由大到小给运动基元赋值do(k)∈{1,2,…,n}。启发性能指标为P＝ε*ea(k)+(1-ε)*do(k)，基元性能按P值升序排列，相同P值取ea(k)小者性能更佳。

其中，计算新状态下ea(k)及do(k)包括如下步骤：

步骤S320根据定h_d(x)＝f(x₀,x,ε),首先更新获取机械臂当前关节空间状态x₀＝s₀＝[θ₁₀,θ₂₀,θ₃₀,θ₄₀,θ₅₀,θ₆₀],获取目标点关节空间状态S_F＝[θ_1F,θ_2F,θ_3F,θ_4F,θ_5F,θ_6F]，计算加入运动基元后机械臂关节空间坐标，具体包括如下两个判断执行步骤：

步骤S320-1若运动基元为AP_LAB或SP_LIB,则基元形式为

x＝[Δθ₁,Δθ₂,Δθ₃,Δθ₄,Δθ₅,Δθ₆]

新关节空间状态为：

x₀′＝[θ₁+Δθ₁,θ₂+Δθ₂,θ₃+Δθ₃,θ₄+Δθ₄,θ₅+Δθ₅,θ₆+Δθ₆]

＝[θ_c1,θ_c2,θ_c3,θ_c4,θ_c5,θ_c6]

新笛卡尔空间位姿坐标为：

步骤S320-2若运动基元为CP_LIB，则基元形式为x＝[Δc_x,Δc_y,Δc_z,Δc_α,Δc_β,Δc_γ]^T，根据x₀获取当前机械臂坐标笛卡尔空间位姿坐标为：

计算加入运动基元后的笛卡尔下新坐标为：

c′＝[c_x+Δc_x,c_y+Δc_y,c_z+Δc_z,c_α+Δc_α,c_β+Δc_β,c_γ+Δc_γ]^T

＝[c′_x,c′_y,c′_z,c′_α,c′_β,c′_γ]^T

此时，

x₀′＝T^-1(c′)＝[θ_c1,θ_c2,θ_c3,θ_c4,θ_c5,θ_c6] (3-11)

步骤S321计算机械臂末端与障碍物距离：

获取当前障碍物最新位姿[b_x,b_y,b_z,b_α,b_β,b_γ]^T，调用距离计算函数如下

步骤S322计算机械臂新关节状态下各关节与目标关节状态差值：

目标点关节空间状态为S_F＝[θ_1F,θ_2F,θ_3F,θ_4F,θ_5F,θ_6F]，根据下式计算关节空间距离为：

步骤S321返回针对当前库中第k个运动基元的计算结果do(k)＝d_co和ea(k)＝d_ea。

步骤4计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步，具体按如下步骤进行：

步骤4-1获取机械臂当前关节空间状态x₀，按式(3-6)利用步骤1-1建立的机械臂运动学模型获取当前机械臂在笛卡尔空间下位姿c，调用式(3-1)、式(3-3)及式(3-4)计算D_O(q)、D1_F(q)和D2_F(q)的值，将函数计算结果与区域触发阈值K_O、K1_F及K2_F进行比较。

步骤4-2对获得计算结果进行判断，根据判断条件选择执行以下步骤：

条件1：若：D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)>K_O；顺序执行步骤5；

条件2：若：D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)<K_O，跳转步骤6；

条件3：若：D1_F(q)<A_F1或D2_F(q)<A_F2，判断机械臂末端到达期望位姿点，获得搜索结果跳转步骤8；

条件4：其他情况跳转步骤7；

步骤5在降维稀疏的C空间中使用SP_LIB库执行启发式搜索，获得当前库中最优运动基元作为规划结果。具体包括如下步骤：

步骤5-1采用机械臂简化运动学模型T₁，调用步骤3-3-1中定义的h_d(x)启发函数遍历步骤2-1中定义的SP_LIB库中的运动基元进行启发搜索(只需遍历机械臂前三个关节的运动基元，即如图2中21、22、23这三个关节)，获得基元启发性能降序排列。调用步骤3-2中的碰撞检测函数D_O(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果；

步骤5-2更新当前机械臂关节空间状态；

步骤5-3跳转步骤4；

步骤6在非降维稀疏C空间中使用SP_LIB库或CP_LIB库执行启发式搜索，获得当前库中最优运动基元作为规划结果。具体包括如下步骤：

步骤6-1判断与障碍物距离是否触发阈值K_M，若D_O(q)≥K_M，则进入非紧急状态，顺序执行步骤6-2；反之跳转执行步骤6-3；

步骤6-2采用完整机械臂运动学模型T₀，调用步骤3-3-1中定义的h_d(x)启发函数，遍历步骤2-1中定义的单关节空间轨迹规划库(SP_LIB)中的12种库成员中进行启发搜索，获得基元启发性能降序排列。调用步骤3-2中的碰撞检测函数D_O(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果。跳转步骤6-4；

步骤6-3进入紧急避障状态，指定基元直线段长度d_l，调用执行步骤2-3中定义的笛卡尔空间轨迹规划库(记为CP_LIB)，获得紧急避障路径规划结果作为当前单元运动规划结果；

步骤6-4更新当前机械C-空间状态；

步骤6-5跳转步骤4；

步骤7规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。具体按如下步骤进行：

步骤7-1检测是否出发阈值K1_F，若D_O(q)≤K1_F，则顺序执行；反之跳转步骤7-3；

步骤7-2利用步骤2-2中的自适应逆运动学轨迹规划库(记为AP_LIB)，在关节空间完成轨迹规划。首先调用步骤3-3-2定义的h_c(x)启发函数，遍历AP_LIB库中单关节独立运动基元，获得基元启发性能降序排列。然后调用步骤3-2中的碰撞检测函数D_O(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果，跳转步骤7-4；

步骤7-3调用步骤2-2中的自适应逆运动学轨迹规划库(记为AP_LIB)，在关节空间完成轨迹规划，运动基元为6个关节协同运动轨迹规划基元；

步骤7-4更新当前C-空间状态；

步骤7-5跳转步骤4；

步骤8跳出计算过程，保存路径搜索结果，清理缓存和状态参数，等待指令执行下一次路径规划任务。

Claims (8)

1.一种基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤如下：

步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；

步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；

步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数h_d(x)和关节空间中的启发函数h_c(x)以及启发规则，实现启发搜索；

步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步；

调用机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)、关节空间距离检测函数D2_F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)函数，将函数计算结果与区域触发阈值K1_F、K2_F及K_O进行比较，根据判断条件选择执行以下步骤：

条件1：若D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)>K_O，则顺序执行步骤五；

条件2：若D1_F(q)>K1_F或D2_F(q)>K2_F，且D_O(q)<K_O，则跳转至步骤六；

条件3：若D1_F(q)<A_F1或D2_F(q)<A_F2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；

条件4：其他情况跳转至步骤七；

步骤五：在降维稀疏的C空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；

步骤六：在非降维稀疏C空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；

步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。

2.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤一中，

采用DH参数法分别建立机械臂完整运动学模型T₀和简化运动学模型T₁；根据国家标准和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型。

3.如权利要求2所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，建立机械臂完整运动学模型T₀时，以末端手爪(28)的末端中心为原点建立工具坐标系，以基座(20)中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型T₁时，以末端手爪(28)的末端中心为原点建立工具坐标系，以基座(20)中心为原点建立基座坐标，将第四关节(24)、第五关节(25)、第六关节(26)、双目相机(27)以及末端手爪(28)视为第三关节(23)的机械结构延伸，除了随动于第三关节(23)外没有相对运动。

4.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤二中，

运动基元库包括单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规划库CP_LIB；单关节空间轨迹规划库SP_LIB以单关节空间轨迹规划结果作为库的运动基元，用于构建离散C-空间；

当机械臂接近目标位姿时，使用自适应逆运动学规划库AP_LIB自适应地建立精确到达指定位姿的轨迹；

笛卡尔空间轨迹规划库CP_LIB用于快速脱离障碍物区域。

5.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤三中，

初始化机械臂工作环境参数时，首先初始化世界坐标系W₀，建立机械臂末端与世界坐标系的变换矩阵T_W初始化机械臂当前关节空间状态S₀；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤一中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w₀位姿坐标O_w，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建；

距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数D_O(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1_F(q)和关节空间距离检测函数D2_F(q)；

危险区域阈值包括笛卡尔空间距离末端区域触发阈值K1_F、关节空间距离末端区域触发阈值K2_F、笛卡尔空间距离障碍物区域触发阈值K_O以及笛卡尔空间距离紧急避障区域阈值K_M；目标点精度范围阈值包括笛卡尔空间阈值A_F1和关节空间阈值A_F2。

6.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤五中，

根据当前机械臂关节状态参数，采用简化机械臂运动学模型T₁，在降维离散C-空间中使用启发式搜索算法，遍历步骤二中定义的单关节空间轨迹规划库SP_LIB库中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，更新机械臂状态，跳转至步骤四。

7.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤六中，

判断与障碍物距离是否小于紧急值K_M，若不小于则进入非紧急状态，遍历步骤二中所有单关节空间轨迹规划库SP_LIB中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)；若小于则进入紧急避障状态，计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向，利用笛卡尔空间轨迹规划库CP_LIB中基元进行紧急避障轨迹规划，快速脱离障碍物区域；执行此步骤后更新机械臂状态，跳转至步骤四。

8.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤七中，当机械臂在关节空间的距离不大于阈值K2_F时进行协同运动规划；当仅笛卡尔空间距离达到阈值K1_F时进行单关节独立规划；根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索，调用步骤三中定义的笛卡尔空间启发函数h_d(x)和笛卡尔空间距离检测函数D_O(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，执行后更新机械臂状态，跳转至步骤四。