一种基于运动基元库的带电作业机械臂自主避障路径规划方法与流程
本发明属于带电作业机器人领域，具体涉及多自由度机械臂工作环境三维重建及自主动态避障路径规划，尤其是面向自主带电作业机械臂路径规划的方法。
背景技术：
目前我国高压带电作业方式仍停留在人工作业和机械臂遥操作作业阶段，人工作业时技术人员暴露在高压环境中，危险性极大；遥操作需要确保操作过程中机械臂不与杆塔元器件碰撞且最多只与一根电线接触，这对技术人员在现场观察、预判及精细操作方面的能力提出了很高的要求。
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)和笛卡尔空间距离检测函数do(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参数法分别建立机械臂完整运动学模型和简化运动学模型分别记为t0和t1(t0、t1为相应的机械臂坐标变换矩阵)。根据国标和行业标准，获取带电作业中杆塔元器件的标准尺寸信息及标准安装信息，将各标准场景作为场景模型库进行封装设计，得到机械臂作业环境模型库；将杆塔元器件用圆柱、球、长方体进行包覆简化，作为碰撞检测时的几何模型，简化计算。
建立机械臂完整运动学模型t0时，以末端手爪28末端中心为原点建立工具坐标系，以基座20中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系；建立机械臂简化运动学模型t1时，仍以末端手爪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用于快速脱离障碍物区域。
步骤三：初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建启发函数和启发规则，实现启发搜索。
初始化机械臂工作环境参数时，首先初始化世界坐标系w0(默认与机械臂基座中心重合)，建立机械臂末端与世界坐标系的变换矩阵tw初始化机械臂当前关节空间状态s0。然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w0位姿坐标ow，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建。
距离计算函数包括建立机械臂末端与障碍物的笛卡尔空间距离检测函数do(q)、机械臂末端与目标点笛卡尔空间距离检测函数d1f(q)和关节空间距离检测函数d2f(q)。目标点约束包括位置和姿态，故需关节空间和笛卡尔空间两种距离检测函数，只有当两者都归零或在一定精度范围内时才可认为到达目标点。危险区域阈值包括设置末端区域触发阈值k1f(笛卡尔空间距离)、k2f(关节空间距离)和障碍物区域触发阈值ko(笛卡尔空间距离)，设置紧急避障区域阈值km(笛卡尔空间距离)。目标点精度范围阈值包括笛卡尔空间阈值af1和关节空间阈值af2。
为实现启发搜索，构造并初始化笛卡尔空间中的启发函数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库执行启发式搜索，获得当前库中最优运动基元作为规划结果。
根据当前机械臂关节状态参数，采用简化机械臂运动学模型t1，在降维离散c-空间中使用启发式搜索算法，遍历步骤二中定义的单关节空间轨迹规划库sp_lib库中的运动基元进行启发搜索(只遍历21,22,23前三关节运动基元)，即调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数do(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，更新机械臂状态，跳转步骤四。
步骤六：在非降维稀疏关节空间中使用单关节空间轨迹规划库sp_lib库或笛卡尔空间轨迹规划库cp_lib库执行启发式搜索，获得当前库中最优运动基元作为规划结果。
首先判断与障碍物距离是否小于紧急值km。若不小于则进入非紧急状态，遍历步骤二中所有单关节空间轨迹规划库sp_lib中的运动基元进行启发搜索，即调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数do(q)；若小于则进入紧急避障状态，计算机械臂末端与障碍物笛卡尔空间距离梯度下降方向作为轨迹方向，利用笛卡尔空间轨迹规划库cp_lib中基元进行紧急避障轨迹规划，快速脱离障碍物区域。执行此步骤后更新机械臂状态，跳转步骤四。
步骤七：规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。
机械臂位姿此时已经非常接近目标位置点，步骤2中的单关节空间轨迹规划库sp_lib和笛卡尔空间轨迹规划库cp_lib均不能保证精确达到指定c-空间状态sf，故采用自适应逆运动学解(ap_lib库)方法。
这里的自适应库有两种。第一种是多关节同时进行轨迹规划map_lib，实现协同运动；第二种是单关节独立进行规划sap_lib，根据具体需求进行选择。当机械臂在关节空间的距离不大于阈值k2f时进行协同运动规划；当仅笛卡尔空间距离达到阈值k1f时进行单关节独立规划。根据情况选取自适应逆运动学轨迹规划库中的运动基元进行启发搜索，调用步骤三中定义的笛卡尔空间启发函数hd(x)和笛卡尔空间距离检测函数do(q)，综合启发函数和碰撞函数结果对运动基元库成员择优作为启发结果，执行后更新机械臂状态，跳转步骤四。
步骤八：跳出计算过程，保存路径搜索结果，清理缓存和状态参数，等待指令执行下一次路径规划任务。
实施例
步骤1，建立机械臂完整运动学模型、机械臂简化运动学模型并构建机械臂工作环境，具体按如下步骤进行：
步骤1-1根据厂家提供的d-h参数按式(1-2)建立机械臂关节坐标变换矩阵t0，将t0作为机械臂完整运动学模型，具体实施如下。其中，t0以28末端中心为原点建立工具坐标系，以20中心为原点建立基座坐标，6个关节分别独立建立相应的关节坐标系wi，建立的机械臂完整运动学模型的机械臂关节几何关系如附图7所示；式(1-2)中
由上述方法可以获取机械臂末端到机械臂基座的变换矩阵t0如式(1-2)，其中nx、ny、nz,ox、oy、oz，ax、ay、az以及px、py、pz分别简记变换矩阵各项参数，需要注意的是最后一个连杆坐标系原点应该是机械臂抓手末端中心点。
步骤1-2根据机械臂执行路径规划时的初始位置，按式(1-3)建立机械臂坐标变换矩阵t1，将t1作为机械臂简化运动学模型，具体实施如下。当指定机械臂起始位姿后，t1仍以28末端中心为原点建立工具坐标系，以20中心为原点建立基座坐标，但将24、25、26、27、28视为23的机械结构延伸，除了随动于23外没有相对运动，此时相当于只需建立21、22及23这三个独立关节的关节坐标系，即附图7中w0、w1、w2、w3和w6。此时变换矩阵计算方法如式(1-3)，其中n1x、n1y、n1z,o1x、o1y、o1z，a1x、a1y、a1z以及p1x、p1y、p1z分别简记t1变换矩阵各项参数
步骤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)中accmax为加速度峰值、t为加速度曲线正弦段三角函数周期、t0是加速度为负值的起点时刻、t为规划过程中的时间变量，进而可以推导得到关节匀速值
每个关节均需建立独立的轨迹规划过程，同时每个关节运动有顺时针和逆时针运动两种情况。故6关节机械臂sp_lib库需要12个库成员。每一种特定情况只需按照式(2-1)调整三个参数即可完成规划过程。
步骤2-2设计第二类基元库：自适应逆运动学轨迹规划库(ap_lib)，利用式(2-3)获取机械臂目标关节角和起始关节角，随后运用式(2-2)方程进行单关节轨迹规划，将规划结果作为库中成员，具体实施过程如下。c空间中两点之间的轨迹规划采用三阶四次多项式进行拟合，方程形式如式(2-2)所示。其中θi(t)(i＝1…n)为第i关角度，c1、c2、c3及c4分别为可求解系数，t为时间变量。
θi(t)＝c1t3+c2t2+c3t+c4(2-2)
建立轨迹起始和末端的速度和加速度均为零的约束，起始和终止角度均已知或可由式(2-3)求解，故式中c1、c2、c3及c4均可确定。这里将规划结果分为两种情况：第一，6个关节同时进行轨迹规划，轨迹运动在时间上同步；第二，6个关节独立进行规划(两种情况共有7个库成员)。关节独立进行轨迹规划时，对各关节依次按式(2-2)进行独立轨迹规划即可；多关节协同规划时，按式(2-2)进行轨迹计算中，各个关节计算过程中的时间t的步长保持一致，则可得到多关节协同运动结果。
当给定的目标点为笛卡尔空间下位姿时，根据t0、t1变换矩阵可以得到机械臂的逆运动学解，若是机械臂简化模型，则θ4、θ5、θ6无需求解。fk(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)进行，其中与障碍物距离计算方程doc(x,y,z)定义如式(2-4)，基元直线段长度dl根据场景环境具体设置，直线插值按式(2-6)进行。为避开奇异点，根据机械臂结构参数，预先求解出机械臂在笛卡尔空间中奇异点位置集s＝{s1,s2…sn}，建立笛卡尔空间中机械臂末端与奇异点距离检测函数ds(x,y,z)如式(2-5)。当ds(x,y,z)计算的距离大于阈值dfs时，正常执行基元规划过程；当计算距离小于阈值时，以奇异点为圆心，机械臂当前位置为半径随机做圆与预期直线段的交于两点，圆形中的直线段以随机圆产生的圆弧代替，其它部分保持不变。这样的轨迹修正函数可以实现避免奇异点的目的。
上述式(2-4)、(2-7)中(x，y，z)分别笛卡尔空间中机械臂末端位置坐标，(x0，y0，z0)为障碍物位置坐标，(x1，y1，z1)为奇异点位置坐标；式(2-5)中(dx,dy,dz)为距离增加的单位梯度方向；式(2-6)中n为设置的路径点序列数，dl为基元路径直线长度，x(n)、y(n)及z(n)分别为第n个序列点在笛卡尔空间中的x、y、z坐标。
步骤3初始化机械臂工作环境参数；分类建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度阈值；创建启发函数和启发规则，实现启发搜索。具体按如下步骤进行：
步骤3-1完成机械臂工作环境的搭建和环境参数初始化，具体实施如下。首先初始化世界坐标系w0(默认与机械臂基座中心重合，如附图7所示)；然后根据三维点云功能模块和图像处理模块的计算结果，选择步骤1-4中建立的相匹配的机器人作业场景模型进行匹配操作，匹配成功后利用双目相机重复采样模型与环境匹配特征点的位置坐标，利用最小二乘法拟合变换矩阵，获取各障碍物基准点相对w0位姿坐标ow，若匹配失败则利用点云相机和双目相机数据进行实时三维场景重建。
步骤3-2建立机械臂与目标点、障碍物的距离计算函数，设置危险区域阈值和到达目标点精度触发阈值，具体实施如下。建立机械臂末端与障碍物的笛卡尔空间距离检测函数do(q)、机械臂末端与目标点笛卡尔空间距离检测函数d1f(q)和关节空间距离检测函数d2f(q)(注：目标点约束包括位置和姿态，故需关节空间和笛卡尔空间两种距离检测函数，只有当两者都归零或在一定精度范围内时才可认为到达目标点)。设置末端区域触发阈值k1f(笛卡尔空间距离)、k2f(关节空间距离)和障碍物区域触发阈值ko(笛卡尔空间距离)，设置紧急避障区域阈值km(笛卡尔空间距离)，设置到达目标点精度范围阈值,af1为笛卡尔空间阈值，af2为关节空间阈值。
上述步骤3-2中do(q)的计算具体按如下步骤进行：
步骤s210根据步骤3-1建立的环境参数获取障碍物最新位姿[bx,by,bz,bα,bβ,bγ]，其中bx、by、bz、bα、bβ及bγ分别为障碍物在笛卡尔空间中的(x,y,z,α、β、γ)位姿坐标，获取机械臂当前关节空间状态参数q＝[θ1,θ2,θ3,θ4,θ5,θ6]，其中θ1、θ2、θ3、θ4、θ5、θ6分别为第1到6关节的关节角。
步骤s211利用步骤1-1建立的机械臂运动学模型，按式(3-1)计算获取当前机械臂末端在笛卡尔空间中位姿坐标[cx,cy,cz,cα,cβ,cγ]t，其中cx、cy、cz、cα、cβ及cγ分别为机械臂在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ；
步骤s212计算机械臂末端到障碍物的笛卡尔空间距离为
上述步骤3-2中d1f(q)的计算具体按如下步骤进行：
步骤s220根据步骤3-1建立的环境参数获取当前目标点在笛卡尔空间下最新位姿参数[ox,oy,oz,oα,oβ,oγ]t，其中ox、oy、oz、oα、oβ、oγ分别目标点为在笛卡尔空间中的位置坐标x、y、z和姿态角α、β、γ；获取机械臂当前关节空间状态参数q＝[θ1,θ2,θ3,θ4,θ5,θ6]；
步骤s221利用步骤1-1建立的机械臂运动学方程，根据式(3-1)获取当前机械臂末端在笛卡尔空间中位姿[cx,cy,cz,cα,cβ,cγ]t；
步骤s222计算机械臂末端到目标点的笛卡尔空间距离为
上述步骤3-2中d2f(q)的计算具体按如下步骤进行：
步骤s230根据步骤3-1建立的环境参数获取当前目标点最新位姿[ox,oy,oz,oα,oβ,oγ]t；获取机械臂当前关节空间状态参数q＝[θ1,θ2,θ3,θ4,θ5,θ6]；
步骤s231根据式(2-3)逆运动学解方程t-1(x,y,z,α,β,γ)求取目标点在机械臂关节空间中坐标为[θ1′,θ2′,θ3′,θ4′,θ5′,θ6′]，其中θ1′、θ2′、θ3′、θ4′、θ5′、θ6′分别对应机械臂第1到6关节的关节角度；
步骤s232计算关节空间距离可得：
步骤3-3创建自定义启发函数，设计启发搜索计算过程，实现对基元库中成员的启发计算，获取指定运动基元库中基元启发性能的降序排列，具体实施如下。更新机械臂当前关节空间状态s0＝[θ10,θ20,θ30,θ40,θ50,θ60]，根据式(2-3)获取机械臂期望关节位置sf＝[θ1f,θ2f,θ3f,θ4f,θ5f,θ6f](注：若机械臂给定的目标末端位置是笛卡尔空间中的位姿点，则机械臂逆解存在多解的情况，这里对最优解的筛选按不碰撞、关节空间距离最小两个指标进行)。构造并初始化笛卡尔空间中的启发函数hd(x)和关节空间中的启发函数hc(x)实现启发计算。本发明设计的启发过程是基于运动基元库的，在路径启发搜素时遍历当前运动库中有限的运动基元即可完成启发计算。
上述步骤3-3中启发函数构造具体按如下步骤：
步骤3-3-1构造笛卡尔空间中的启发函数hd(x)和启发计算规则，获取指定运动基元库中基元启发性能的降序排列。
确定当前选中运动基元库，定义以笛卡儿空间距离为基础的启发函数hc(x)＝f(x0x,ε),此处x0为机械臂当前关节空间状态，x为运动基元，ε∈[0,1]为比例系数，这里的启发函数计算采用遍历当前库中运动基元的方式实现。根据库中选定的运动基元获取关节空间新状态x0′，调用式(3-2)do(q)函数计算新状态下末端点与障碍物的笛卡儿空间距离do(k)(k为运动基元的编号)，按距离由大到小给选定库中运动基元赋值do(k)∈{1,2,…,n}(即max(do(k))＝1,min(do(k))＝n),其中n为运动基元总数；调用d1f(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根据定hd(x)＝f(x0,x,ε),首先更新获取机械臂当前关节空间状态x0＝s0＝[θ10,θ20,θ30,θ40,θ50,θ60],计算加入运动基元后机械臂笛卡尔空间坐标，包括如下两个判断计算步骤：
步骤s310-1若当前选定库为步骤2中ap_lab库或sp_lib库，则基元形式为：
x＝[δθ1,δθ2,δθ3,δθ4,δθ5,δθ6]
新状态x0′为：
x0′＝[θ1+δθ1,θ2+δθ2,θ3+δθ3,θ4+δθ4,θ5+δθ5,θ6+δθ6]
新状态下，机械臂末端笛卡尔空间位姿坐标c′为：
步骤s310-2当前选定库为步骤2中cp_lib库，则基元形式为：
x＝[δcx,δcy,δcz,δcα,δcβ,δcγ]t
根据x0＝s0,获取当前机械臂笛卡尔空间位姿坐标为：
计算加入运动基元后的新坐标为：
c′＝[cx+δcx,cy+δcy,cz+δcz,cα+δcα,cβ+δcβ,cγ+δcγ]t(3-7)
步骤s311计算机械臂末端与障碍物距离：
获取当前障碍物最新位姿[bx,by,bz,bα,bβ,bγ]t，调用式(3-1)距离计算函数如下
步骤s312计算机械臂末端与目标点距离：
获取当前目标点最新位姿[ox,oy,oz,oα,oβ,oγ]t,调用式(3-1)距离计算函数如下
步骤s313返回针对当前库中第k个运动基元的计算结果do(k)＝dco和da(k)＝dcs。
步骤3-3-2构造关节空间中的启发函数hc(x)和启发计算规则，获取指定运动基元库中基元启发性能的降序排列。
确定当前选中运动基元库，定义以关节空间距离为基础的启发函数hc(x)＝f(x0x,ε),此处x0为机械臂当前关节空间状态，x为运动基元，ε∈[0,1]为比例系数。这里的启发函数计算采用遍历当前库中运动基元的方式实现，根据库中选定的运动基元获取关节空间新状态x0′，判断各关节角度当前值与期望值的差值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根据定hd(x)＝f(x0,x,ε),首先更新获取机械臂当前关节空间状态x0＝s0＝[θ10,θ20,θ30,θ40,θ50,θ60],获取目标点关节空间状态sf＝[θ1f,θ2f,θ3f,θ4f,θ5f,θ6f]，计算加入运动基元后机械臂关节空间坐标，具体包括如下两个判断执行步骤：
步骤s320-1若运动基元为ap_lab或sp_lib,则基元形式为
x＝[δθ1,δθ2,δθ3,δθ4,δθ5,δθ6]
新关节空间状态为：
x0′＝[θ1+δθ1,θ2+δθ2,θ3+δθ3,θ4+δθ4,θ5+δθ5,θ6+δθ6]
＝[θc1,θc2,θc3,θc4,θc5,θc6]
新笛卡尔空间位姿坐标为：
步骤s320-2若运动基元为cp_lib，则基元形式为x＝[δcx,δcy,δcz,δcα,δcβ,δcγ]t，根据x0获取当前机械臂坐标笛卡尔空间位姿坐标为：
计算加入运动基元后的笛卡尔下新坐标为：
c′＝[cx+δcx,cy+δcy,cz+δcz,cα+δcα,cβ+δcβ,cγ+δcγ]t
＝[c′x,c′y,c′z,c′α,c′β,c′γ]t
此时，
x0′＝t-1(c′)＝[θc1,θc2,θc3,θc4,θc5,θc6](3-11)
步骤s321计算机械臂末端与障碍物距离：
获取当前障碍物最新位姿[bx,by,bz,bα,bβ,bγ]t，调用距离计算函数如下
步骤s322计算机械臂新关节状态下各关节与目标关节状态差值：
目标点关节空间状态为sf＝[θ1f,θ2f,θ3f,θ4f,θ5f,θ6f]，根据下式计算关节空间距离为：
步骤s321返回针对当前库中第k个运动基元的计算结果do(k)＝dco和ea(k)＝dea。
步骤4计算机械臂当前位姿与期望位姿及障碍物的笛卡尔空间距离，根据设定的判断条件选择执行下一步，具体按如下步骤进行：
步骤4-1获取机械臂当前关节空间状态x0，按式(3-6)利用步骤1-1建立的机械臂运动学模型获取当前机械臂在笛卡尔空间下位姿c，调用式(3-1)、式(3-3)及式(3-4)计算do(q)、d1f(q)和d2f(q)的值，将函数计算结果与区域触发阈值ko、k1f及k2f进行比较。
步骤4-2对获得计算结果进行判断，根据判断条件选择执行以下步骤：
条件1：若：d1f(q)>k1f或d2f(q)>k2f，且do(q)>ko；顺序执行步骤5；
条件2：若：d1f(q)>k1f或d2f(q)>k2f，且do(q)<ko，跳转步骤6；
条件3：若：d1f(q)<af1或d2f(q)<af2，判断机械臂末端到达期望位姿点，获得搜索结果跳转步骤8；
条件4：其他情况跳转步骤7；
步骤5在降维稀疏的c空间中使用sp_lib库执行启发式搜索，获得当前库中最优运动基元作为规划结果。具体包括如下步骤：
步骤5-1采用机械臂简化运动学模型t1，调用步骤3-3-1中定义的hd(x)启发函数遍历步骤2-1中定义的sp_lib库中的运动基元进行启发搜索(只需遍历机械臂前三个关节的运动基元，即如图2中21、22、23这三个关节)，获得基元启发性能降序排列。调用步骤3-2中的碰撞检测函数do(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果；
步骤5-2更新当前机械臂关节空间状态；
步骤5-3跳转步骤4；
步骤6在非降维稀疏c空间中使用sp_lib库或cp_lib库执行启发式搜索，获得当前库中最优运动基元作为规划结果。具体包括如下步骤：
步骤6-1判断与障碍物距离是否触发阈值km，若do(q)≥km，则进入非紧急状态，顺序执行步骤6-2；反之跳转执行步骤6-3；
步骤6-2采用完整机械臂运动学模型t0，调用步骤3-3-1中定义的hd(x)启发函数，遍历步骤2-1中定义的单关节空间轨迹规划库(sp_lib)中的12种库成员中进行启发搜索，获得基元启发性能降序排列。调用步骤3-2中的碰撞检测函数do(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果。跳转步骤6-4；
步骤6-3进入紧急避障状态，指定基元直线段长度dl，调用执行步骤2-3中定义的笛卡尔空间轨迹规划库(记为cp_lib)，获得紧急避障路径规划结果作为当前单元运动规划结果；
步骤6-4更新当前机械c-空间状态；
步骤6-5跳转步骤4；
步骤7规划精确到达目标点轨迹，获得各个关节精确达到目标关节角的单元运动规划结果。具体按如下步骤进行：
步骤7-1检测是否出发阈值k1f，若do(q)≤k1f，则顺序执行；反之跳转步骤7-3；
步骤7-2利用步骤2-2中的自适应逆运动学轨迹规划库(记为ap_lib)，在关节空间完成轨迹规划。首先调用步骤3-3-2定义的hc(x)启发函数，遍历ap_lib库中单关节独立运动基元，获得基元启发性能降序排列。然后调用步骤3-2中的碰撞检测函数do(q)按顺序对启发性能序列进行碰撞检测计算，第一个不碰撞基元作为启发搜索结果，也即当前单元运动规划结果，跳转步骤7-4；
步骤7-3调用步骤2-2中的自适应逆运动学轨迹规划库(记为ap_lib)，在关节空间完成轨迹规划，运动基元为6个关节协同运动轨迹规划基元；
步骤7-4更新当前c-空间状态；
步骤7-5跳转步骤4；
步骤8跳出计算过程，保存路径搜索结果，清理缓存和状态参数，等待指令执行下一次路径规划任务。
- 还没有人留言评论。精彩留言会获得点赞！