CN108555911A - 基于虚拟推力的遥操作机械臂三维避障方法 - Google Patents

Abstract

本发明公开了基于虚拟推力的遥操作机械臂三维避障方法，遥操作机械臂的末端轨迹上存在障碍物时，根据改进任务优先级后的避障算法追踪末端轨迹。随着机械臂末端距离障碍物越来越近，启用虚拟推力参与障碍物的躲避的任务。在遥操作机械臂末端执行器任务轨迹追踪误差极小的情况下，完成机械臂对障碍物的回避动作。实现避障后，随着机械臂末端对障碍物的远离，逐渐削弱虚拟推力的力度，继续追踪末端轨迹，完成给定工作任务。该算法能实现遥操作机械臂末端在三维复杂环境中完成给定任务，同时保证杆件和各关节与障碍物的碰撞。本发明在虚拟推力的融入下，解决了任务转换算法的陷入局部避障死区的不足。

Description

基于虚拟推力的遥操作机械臂三维避障方法

技术领域

本发明公开了基于虚拟推力的遥操作机械臂三维避障方法，具体是对遥操作机械臂三维避障运动过程中运动规划方法，属于机械臂运动规划领域。

背景技术

机器人的运动规划问题的研究，在上世纪六十年代已被兴起。早期的运动规划只是涉及到路径规划。把机器人视为一个点，运动规划即为在位姿空间中寻找一条从起始位姿点到目标位姿点的连续路径，它的研究已经很成熟。与机器人避障相比较，遥操作机械臂的避障则复杂很多，它是在完成某个特定任务时，机械臂的所有组成杆件和末端都不与障碍物发生碰撞。

针对机械臂的避障规划，目前已采用的方法有全局规划和局部规划。全局规划的基础是精确的全局环境的建模，这就需要大量的计算，对于动态环境建模计算更复杂，难度增大，很难适应动态环境。局部规划是将避障问题看做一个局部控制问题，通过传感器和视觉检测器获取障碍物信息实现实时避障，与全局规划比较有着计算量小，动态环境适应力强，便于实现等优点。目前其主流的算法有神经网络方法、人工势场法、梯度投影法等。

其中神经网络算法计算量大，人工势力场算法末端轨迹追踪误差大易陷入局部极小值，梯度投影法在复杂非结构环境中容易避障失败。考虑以上算法存在的优缺点，本发明提出一种将虚拟推力和任务转换避障算法相结合的方法实现遥操作机械臂的三维避障方法。解决了上述避障算法中普遍存在的计算量大，末端轨迹追踪误差大，非结构化复杂三维环境中难以实现避障的问题。

发明内容

本发明针对现有避障方法的不足，提出一种基于虚拟推力的遥操作机械臂三维避障方法。

为实现上述目的，在本发明方法中，遥操作机械臂在三维立体环境空间中进行路径规划。遥操作机械臂的末端轨迹上存在障碍物时，根据改进任务优先级后的避障算法追踪末端轨迹。随着机械臂末端距离障碍物越来越近，启用虚拟推力参与障碍物的躲避的任务。在遥操作机械臂末端执行器任务轨迹追踪误差极小的情况下，完成机械臂对障碍物的回避动作。实现避障后，随着机械臂末端对障碍物的远离，逐渐削弱虚拟推力的力度，继续追踪末端轨迹，完成给定工作任务。下文将分别讲述虚拟推力与任务转换算法的原理和内容。最后通过实验证明本发明算法的有效性。

对于一个有N个关节的遥操作机械臂，该遥操作机械臂的动力学方程表示如下：

式中τ表示关节的驱动力矩。M(θ)表示机械臂的质量惯性矩阵，M(θ)是n×n的正定对称矩阵，为关节变量θ的函数，为关节变量θ的一阶偏导，为关节变量θ的二阶偏导；n为矩阵的行数或列数。表示离心力和哥式力向量，是n×1的向量。G(q)表示重力矢量，是一个n×1的向量，与关节变量θ有关。对关节的驱动力矩进行简化，表示如下：

τ＝τ₀+τ_力 (2)

其中τ₀表示离心力、哥式力和重力矢量，求解表达式为，τ_力表示环境对机械臂的作用力矩，求解表达式为，

虚拟推力是一个在遥操作机械臂完成给定工作任务过程中需要避障时假想出来的力。它的主要作用是在障碍阻碍机械臂运动时给机械臂提供的一个虚拟力。这个虚拟力作用在机械臂上，将机械臂拉离障碍物，实现避障。将遥操作机械臂上与障碍物距离最近的点定义为关键点，记为A₀。当A₀与障碍物的最小距离d₀达到一定值时候，假想的虚拟推力开启。此时，虚拟推力f_虚沿着障碍物指向关键点的方向(Bo指向Ao)作用于遥操作机械臂杆件，避障运动开始。遥操作机械臂受到的等效虚拟推力力矩为τ_虚，根据力雅克比关系求解式子表示为：

τ_虚＝J^T·f_虚 (3)

式中，d为通过传感器或视觉摄像机获得的障碍物与机械臂的距离，d_m为安全距离值，K为比例系数，J(θ)是m×n的机械臂末端的雅克比矩阵，M(θ)表示机械臂的质量惯性矩阵。在不受其他外力作用的情况下有：虚拟推力于关节加速度的关系表示为：

在m维的操作空间中，用r表示机械臂末端执行器的位姿坐标向量，θ表示机械臂关节位姿坐标向量，遥操作机械臂的运动学方程表示如下：

r(t)＝g(θ(t)) (1)

式中，g是向量函数，将关节空间变量映射为任务空间变量。

对式(1)两边同时求导，得到机械臂末端和关节的速度关系方程：

式中，表示机械臂末端执行器的速度矢量；

表示机械臂关节的速度矢量；

J(θ)是m×n的机械臂末端的雅克比矩阵。

将遥操作机械臂的避障问题中的机械臂避障作为主任务记为T₀，末端轨迹追踪运动运动作为子任务记为T，则对应的雅克比矩阵分别为J₀和J,运动学方程为:

r₀(t)＝g₀(θ(t)) (5)

r(t)＝g(θ(t)) (6)

遥操作机械臂关节速度的逆运动学近似求解方程表示如下：

式中，表示避障运动，表示机械臂末端的轨迹追踪运动。

其中，

避障运动是在一维平面中完成，将从A₀指向B₀的向量记为d₀，则单位向量n₀表示如下：

则关键点在笛卡尔空间的运动雅克比矩阵J₀简化为

关键点避障运动速度简化为定义如下：

式中σ表示障碍物的活跃程度，当σ＝0时，障碍物离机械臂杆件较远，障碍物不活跃；当σ≠0时，障碍物变得活跃，其活跃程度由下式求解：

式中，d_m为安全距离阀值，当||d₀||→∞时，σ＝0。根据式(6)，运动学逆解公式重新表示如下：

通过在避障环节加入虚拟推力，对任务避障优先级避障方法进行了优化。

本发明算法使得遥操作机械臂在非结构三维环境中进行避障时的避障效果更好，末端轨迹追踪误差最小化。该发明算法具有重要的研究意义。

附图说明

图1为发明的遥操作机械臂三维避障算法流程图。

具体实施方式

如图1所示为遥操作机械臂三维避障算法流程图。

将遥操作机器人UR5从臂末端执行器位置的起点定为出发点，给定终点定为目标点，末端执行器运动轨迹为给定螺旋曲线。遥操作机械臂的三维立体运动环境定为任务工作区。三维工作环境中存在的圆柱体障碍物定为障碍点。本发明主要关注遥操作机械臂在三维立体工作环境中从起点开始追踪给定末端轨迹过程中，躲避空间障碍、到达规定目标点的运动情况。研究无碰撞路径过程中遥操作机器人从臂躲避障碍物的运动方式。在三维立体环境中，遥操作机械臂以一种最快速度、最小末端轨迹追踪误差、最安全的原则沿着给定工作轨迹躲避空间工作区的障碍到达目标点。具体实施步骤如下：

步骤一：建立遥操作机械臂UR5及环境障碍的三维物理模型。在MATLAB软件的Robotic toolbox中建立坐标系，搭建遥操作机械臂UR5，搭建圆柱体障碍物的物理模型。

步骤二：机械臂运动学进行分析。对遥操作机械臂UR5进行D-H运动学分析，求出其运动学逆解。

步骤三：获取当前遥操作机械臂各个杆件的关节角度和末端运动。根据正运动学计算出出末端执行器的初始位置坐标，以及机械臂各个杆件两端的位置坐标。

步骤四：根据步骤三中的坐标，计算每节连杆与障碍物的距离，进行碰撞检测。用圆柱模拟代替遥操作机械臂杆件，用包围盒模拟代替障碍物。碰撞检测问题就转化为计算机械臂的简化模型线段与障碍物的简化模型包围盒之间的距离。假定遥操作机械臂杆件的两端为ai和bi，连杆圆柱体底面半径为ri。包围盒中心为q(x,y,z)，半径为r，碰撞检测可以简化为包围盒中心到杆件ab的距离di。为了减少计算，将机械臂的横向半径叠加到圆柱上面。障碍物与机械臂的距离可以表示为ri+r。如果d＜ri+r，则表示机械臂杆件与障碍物发生碰撞；如果d≥ri+r，则表示机械臂杆件与障碍物未发生碰撞。通过碰撞检测，对各个杆件进行障碍物的相交测试，剔除掉不会发生碰撞的安全杆件。计算出相交杆件与障碍物的实时最小距离dmin。

步骤五：开启避障规划并追踪末端轨迹。根据碰撞检测计算得到相交杆件与障碍物的最小距离dmin，将其与给定的安全距离阈值相比较。当dmin变小，末端执行器逐渐靠近障碍物，此时避障任务开启，从而保证机械臂杆件的安全。

步骤六：随着障碍物与遥操作机械臂末端执行器的距离越来越小，末端执行器进入危险区域，虚拟推力开始起作用并逐渐增强。在虚拟推力f_虚的作用下，遥操作机械臂末端执行器沿着躲避障碍物的方向运动，在机械臂自运动和虚拟推力的双重作用下完成障碍物的躲避动作。

步骤七：遥操作机械臂完成障碍物躲避任务后，末端执行器离开危险区域，虚拟推力逐渐减弱。最后，末端执行器回到给定的末端轨迹上来，继续完成给定任务。

步骤八：虚拟推力关闭后，机械臂在任务优先级避障算法的作用下，根据上一时刻的关节角度和运动情况，对角速度值进行积分，得到下一步关节角。

步骤九：判断遥操作机械臂末端执行是否到达给定运动轨迹终点，如果未到达继续步骤三，否则程序终止。

下面通过实验对本发明提出的基于虚拟推力的遥操作机械臂三维避障算法进行验证。实验所用平台是在MATLAB软件中的Toolbox工具箱搭建的UR5遥操作机械臂模型。实验环境是三维空间中给定末端轨迹上存在一个圆柱体的障碍物。实验是从给定螺旋曲线轨迹从起点到终点，且遥操作机械臂所有杆件和末端执行器都没有碰到障碍物。通过使用本发明提出的基于虚拟推力的遥操作机械臂三维避障算法做的实验结果可以知道，算法效果明显，实现了避障功能，证明了本方法在三维空间避障的可行性和有效性。

Claims (2)

1.基于虚拟推力的遥操作机械臂三维避障方法，其特征在于：遥操作机械臂在三维立体环境空间中进行路径规划；遥操作机械臂的末端轨迹上存在障碍物时，根据改进任务优先级后的避障方法追踪末端轨迹；随着机械臂末端距离障碍物越来越近，启用虚拟推力参与障碍物的躲避的任务；在遥操作机械臂末端执行器任务轨迹追踪误差极小的情况下，完成机械臂对障碍物的回避动作；实现避障后，随着机械臂末端对障碍物的远离，逐渐削弱虚拟推力的力度，继续追踪末端轨迹，完成给定工作任务；

对于一个有N个关节的遥操作机械臂，该遥操作机械臂的动力学方程表示如下：

式中τ表示关节的驱动力矩；M(θ)表示机械臂的质量惯性矩阵，M(θ)是n×n的正定对称矩阵，为关节变量θ的函数，为关节变量θ的一阶偏导，为关节变量θ的二阶偏导；n为矩阵的行数或列数；表示离心力和哥式力向量，是n×1的向量；G(q)表示重力矢量，是一个n×1的向量，与关节变量θ有关；对关节的驱动力矩进行简化，表示如下：

τ＝τ₀+τ_力 (2)

其中τ₀表示离心力、哥式力和重力矢量，求解表达式为，τ_力表示环境对机械臂的作用力矩，求解表达式为，

虚拟推力是一个在遥操作机械臂完成给定工作任务过程中需要避障时假想出来的力；它的主要作用是在障碍阻碍机械臂运动时给机械臂提供的一个虚拟力；这个虚拟力作用在机械臂上，将机械臂拉离障碍物，实现避障；将遥操作机械臂上与障碍物距离最近的点定义为关键点，记为A₀；当A₀与障碍物的最小距离d₀达到一定值时候，假想的虚拟推力开启；此时，虚拟推力f_虚沿着障碍物指向关键点的方向(Bo指向Ao)作用于遥操作机械臂杆件，避障运动开始；遥操作机械臂受到的等效虚拟推力力矩为τ_虚，根据力雅克比关系求解式子表示为：

τ_虚＝J^T·f_虚 (3)

式中，d为通过传感器或视觉摄像机获得的障碍物与机械臂的距离，d_m为安全距离值，K为比例系数，J(θ)是m×n的机械臂末端的雅克比矩阵，M(θ)表示机械臂的质量惯性矩阵；在不受其他外力作用的情况下有：虚拟推力于关节加速度的关系表示为：

在m维的操作空间中，用r表示机械臂末端执行器的位姿坐标向量，θ表示机械臂关节位姿坐标向量，遥操作机械臂的运动学方程表示如下：

r(t)＝g(θ(t)) (1)

式中，g是向量函数，将关节空间变量映射为任务空间变量；

对式(1)两边同时求导，得到机械臂末端和关节的速度关系方程：

式中，表示机械臂末端执行器的速度矢量；

表示机械臂关节的速度矢量；

J(θ)是m×n的机械臂末端的雅克比矩阵；

将遥操作机械臂的避障问题中的机械臂避障作为主任务记为T₀，末端轨迹追踪运动运动作为子任务记为T，则对应的雅克比矩阵分别为J₀和J,运动学方程为:

r₀(t)＝g₀(θ(t)) (5)

r(t)＝g(θ(t)) (6)

遥操作机械臂关节速度的逆运动学近似求解方程表示如下：

式中，表示避障运动，表示机械臂末端的轨迹追踪运动；

其中，

避障运动是在一维平面中完成，将从A₀指向B₀的向量记为d₀，则单位向量n₀表示如下：

则关键点在笛卡尔空间的运动雅克比矩阵J₀简化为

关键点避障运动速度简化为定义如下：

式中σ表示障碍物的活跃程度，当σ＝0时，障碍物离机械臂杆件较远，障碍物不活跃；当σ≠0时，障碍物变得活跃，其活跃程度由下式求解：

式中，d_m为安全距离阀值，当||d₀||→∞时，σ＝0；根据式(6)，运动学逆解公式重新表示如下：

通过在避障环节加入虚拟推力，对任务避障优先级避障方法进行了优化。

2.根据权利要求1所述的基于虚拟推力的遥操作机械臂三维避障方法，其特征在于：

具体实施步骤如下，

步骤一：建立遥操作机械臂UR5及环境障碍的三维物理模型；在MATLAB软件的Robotictoolbox中建立坐标系，搭建遥操作机械臂UR5，搭建圆柱体障碍物的物理模型；

步骤二：机械臂运动学进行分析；对遥操作机械臂UR5进行D-H运动学分析，求出其运动学逆解；

步骤三：获取当前遥操作机械臂各个杆件的关节角度和末端运动；根据正运动学计算出出末端执行器的初始位置坐标，以及机械臂各个杆件两端的位置坐标；

步骤四：根据步骤三中的坐标，计算每节连杆与障碍物的距离，进行碰撞检测；用圆柱模拟代替遥操作机械臂杆件，用包围盒模拟代替障碍物；碰撞检测问题就转化为计算机械臂的简化模型线段与障碍物的简化模型包围盒之间的距离；假定遥操作机械臂杆件的两端为ai和bi，连杆圆柱体底面半径为ri；包围盒中心为q(x,y,z)，半径为r，碰撞检测可以简化为包围盒中心到杆件ab的距离di；为了减少计算，将机械臂的横向半径叠加到圆柱上面；障碍物与机械臂的距离可以表示为ri+r；如果d＜ri+r，则表示机械臂杆件与障碍物发生碰撞；如果d≥ri+r，则表示机械臂杆件与障碍物未发生碰撞；通过碰撞检测，对各个杆件进行障碍物的相交测试，剔除掉不会发生碰撞的安全杆件；计算出相交杆件与障碍物的实时最小距离d min；

步骤五：开启避障规划并追踪末端轨迹；根据碰撞检测计算得到相交杆件与障碍物的最小距离d min，将其与给定的安全距离阈值相比较；当d min变小，末端执行器逐渐靠近障碍物，此时避障任务开启，从而保证机械臂杆件的安全；

步骤六：随着障碍物与遥操作机械臂末端执行器的距离越来越小，末端执行器进入危险区域，虚拟推力开始起作用并逐渐增强；在虚拟推力f_虚的作用下，遥操作机械臂末端执行器沿着躲避障碍物的方向运动，在机械臂自运动和虚拟推力的双重作用下完成障碍物的躲避动作；

步骤七：遥操作机械臂完成障碍物躲避任务后，末端执行器离开危险区域，虚拟推力逐渐减弱；最后，末端执行器回到给定的末端轨迹上来，继续完成给定任务；

步骤八：虚拟推力关闭后，机械臂在任务优先级避障算法的作用下，根据上一时刻的关节角度和运动情况，对角速度值进行积分，得到下一步关节角；

步骤九：判断遥操作机械臂末端执行是否到达给定运动轨迹终点，如果未到达继续步骤三，否则程序终止。