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

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

【技术实现步骤摘要】
一种基于运动基元库的带电作业机械臂自主避障路径规划方法
本专利技术属于带电作业机器人领域，具体涉及多自由度机械臂工作环境三维重建及自主动态避障路径规划，尤其是面向自主带电作业机械臂路径规划的方法。
技术介绍
目前我国高压带电作业方式仍停留在人工作业和机械臂遥操作作业阶段，人工作业时技术人员暴露在高压环境中，危险性极大；遥操作需要确保操作过程中机械臂不与杆塔元器件碰撞且最多只与一根电线接触，这对技术人员在现场观察、预判及精细操作方面的能力提出了很高的要求。C-空间法在解决移动机器人路径规划问题方面是一种经典且有效的算法，但如何在高维C-空间中快速搜索出满足运动学和障碍物约束的路径并保证路径最优，是多自由度机械臂路径规划问题的关键。基于采样的路径规划算法解决了高维C-空间计算量庞大的问题，但其性能取决于采样方法和节点间连接方式且具有随机性；人工势场法在高维C-空间中使用时计算量庞大和局部最小值问题仍未得到很好的解决；启发式搜索算法通过建立代价方程保证路径的次优且能确保搜索的完备性，但对于多自由度机械臂问题，传统启发式搜索算法庞大的计算量使之难以实现工程应用。对于上述方法，还存在一个重要问题，它们无法在搜索过程中保证搜索结果满足关节速度、加速度的运动学约束以及机械臂运动过程的平稳性。机器人作业环境的三维实时重构
，基于点云和深度相机的现场重建技术得到广泛运用，基于双目相机的三维测量技术也取得了一定的成功，但如何提高复杂环境下三维重建的精度和实时性仍是一个有待突破的问题。
技术实现思路
本专利技术提出一种基于运动基元库的带电作业机械臂自主避障路径搜索方法，能快速、精确构建机械臂作业场景信息，保证避障路径搜索结果具有次优性和完备性，满足动力学约束前提下能快速完成路径规划，保证机械臂运动过程的平稳性。为了解决上述技术问题，本专利技术提供一种基于运动基元库的带电作业机械臂自主避障路径规划方法，步骤如下：步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数hd(x)和关节空间中的启发函数hc(x)以及启发规则，实现启发搜索；步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步。调用机械臂末端与目标点笛卡尔空间距离检测函数D1F(q)、关节空间距离检测函数D2F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数DO(q)函数，将函数计算结果与区域触发阈值K1F、K2F及KO进行比较，根据判断条件选择执行以下步骤：条件1：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)>KO，则顺序执行步骤五；条件2：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)<KO，则跳转至步骤六；条件3：若D1F(q)<AF1或D2F(q)<AF2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；条件4：其他情况跳转至步骤七；步骤五：在降维稀疏的关节空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果。步骤六：在非降维稀疏关节空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。进一步，步骤一中，采用DH参数法分别建立机械臂完整运动学模型T0和简化运动学模型T1分；根据国家标准和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型；进一步，建立机械臂完整运动学模型T0时，以末端手爪的末端中心为原点建立工具坐标系，以基座中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型T1时，以末端手爪的末端中心为原点建立工具坐标系，以基座中心为原点建立基座坐标，将第四关节、第五关节、第六关节、双目相机以及末端手爪视为第三关节的机械结构延伸，除了随动于第三关节外没有相对运动。进一步，步骤二中，运动基元库包括单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规划库CP_LIB；单关节空间轨迹规划库SP_LIB以单关节空间轨迹规划结果作为库的运动基元，用于构建离散C-空间；当机械臂接近目标位姿时，使用自适应逆运动学规划库AP_LIB自适应地建立精确到达指定位姿的轨迹；笛卡尔空间轨迹规划库CP_LIB用于快速脱离障碍物区域。进一步，步骤三中，初始化机械臂工作环境参数时，首先初始化世界坐标系W0，建立机械臂末端与世界坐标系的变换矩阵TW初始化机械臂当前关节空间状态S0；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w0位姿坐标Ow，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建；距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数DO(q)、机械臂末端与目标点笛卡尔空间距离检测函数D1F(q)和关节空间距离检测函数D2F(q)；危险区域阈值包括笛卡尔空间距离末端区域触发阈值K1F、关节空间距离末端区域触发阈值K2F、笛卡尔空间距离障碍物区域触发阈值KO以及笛卡尔空间距离紧急避障区域阈值KM；目标点精度范围阈值包括笛卡尔空间阈值AF1和关节空间阈值AF2。进一步，步骤五中，根据当前机械臂关节状态参数，采用简化机械臂运动学模型T1，在降维离散C-空间中使用启发式搜索算法，遍历步骤二中定义的单关节空间轨迹规划库SP_LIB库中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数DO(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，更新机械臂状态，跳转至步骤四。进一步，步骤六中，判断与障碍物距离是否小于紧急值KM，若不小于则进入非紧急状态，遍历步骤二中所有单关节空间轨迹规划库SP_LIB中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数DO(q)；若小于则进入紧急避障状态，计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向，利用笛卡尔空间轨迹规划库CP_LIB中基元进行紧急避障轨迹规划，快速脱离障碍物区域；执行此步骤后更新机械臂状态，跳转至步骤四。进一步，步骤七中，当机械臂在关节空间的距离不大于阈值K2F时进行协同运动规划；当仅笛卡尔空间距离达到阈值K1F时进行单关节独立规划；根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索，调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数D本文档来自技高网...

【技术保护点】
一种基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤如下：步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数hd(x)和关节空间中的启发函数hc(x)以及启发规则，实现启发搜索；步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步；调用机械臂末端与目标点笛卡尔空间距离检测函数D1F(q)、关节空间距离检测函数D2F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数DO(q)函数，将函数计算结果与区域触发阈值K1F、K2F及KO进行比较，根据判断条件选择执行以下步骤：条件1：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)>KO，则顺序执行步骤五；条件2：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)<KO，则跳转至步骤六；条件3：若D1F(q)<AF1或D2F(q)<AF2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；条件4：其他情况跳转至步骤七；步骤五：在降维稀疏的C空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤六：在非降维稀疏C空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。...

【技术特征摘要】
1.一种基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤如下：步骤一：建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境；步骤二：分类设计运动基元库，为各类库设计相应运动基元作为库成员；步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建笛卡尔空间中的启发函数hd(x)和关节空间中的启发函数hc(x)以及启发规则，实现启发搜索；步骤四：计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步；调用机械臂末端与目标点笛卡尔空间距离检测函数D1F(q)、关节空间距离检测函数D2F(q)和机械臂末端与障碍物的笛卡尔空间距离检测函数DO(q)函数，将函数计算结果与区域触发阈值K1F、K2F及KO进行比较，根据判断条件选择执行以下步骤：条件1：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)>KO，则顺序执行步骤五；条件2：若D1F(q)>K1F或D2F(q)>K2F，且DO(q)<KO，则跳转至步骤六；条件3：若D1F(q)<AF1或D2F(q)<AF2，则判断机械臂末端到达期望位姿态，获得搜索结果跳转至步骤八；条件4：其他情况跳转至步骤七；步骤五：在降维稀疏的C空间中使用单关节空间轨迹规划库SP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤六：在非降维稀疏C空间中使用单关节空间轨迹规划库SP_LIB库或笛卡尔空间轨迹规划库CP_LIB执行启发式搜索，获得当前库中最优运动基元作为规划结果；步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。2.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤一中，采用DH参数法分别建立机械臂完整运动学模型T0和简化运动学模型T1；根据国家标准和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型。3.如权利要求2所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，建立机械臂完整运动学模型T0时，以末端手爪(28)的末端中心为原点建立工具坐标系，以基座(20)中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型T1时，以末端手爪(28)的末端中心为原点建立工具坐标系，以基座(20)中心为原点建立基座坐标，将第四关节(24)、第五关节(25)、第六关节(26)、双目相机(27)以及末端手爪(28)视为第三关节(23)的机械结构延伸，除了随动于第三关节(23)外没有相对运动。4.如权利要求1所述基于运动基元库的带电作业机械臂自主避障路径规划方法，其特征在于，步骤二中，运动基元库包括单关节空间轨迹规划库SP_LIB、自适应逆运动学轨迹规划库AP_LIB以及笛卡尔空间轨迹规...

【专利技术属性】
技术研发人员：郭毓，饶志强，郭健，韩昊一，黄颖，吴禹均，苏鹏飞，吴巍，陈宝存，肖潇，郭飞，
申请(专利权)人：南京理工大学，
类型：发明
国别省市：江苏,32