[发明专利]基于虚拟推力的遥操作机械臂三维避障方法

摘要

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

主权项

1.基于虚拟推力的遥操作机械臂三维避障方法，其特征在于：遥操作机械臂在三维立体环境空间中进行路径规划；遥操作机械臂的末端轨迹上存在障碍物时，根据改进任务优先级后的避障方法追踪末端轨迹；随着机械臂末端距离障碍物越来越近，启用虚拟推力参与障碍物的躲避的任务；在遥操作机械臂末端执行器任务轨迹追踪误差极小的情况下，完成机械臂对障碍物的回避动作；实现避障后，随着机械臂末端对障碍物的远离，逐渐削弱虚拟推力的力度，继续追踪末端轨迹，完成给定工作任务；对于一个有N个关节的遥操作机械臂，该遥操作机械臂的动力学方程表示如下：式中τ表示关节的驱动力矩；M(θ)表示机械臂的质量惯性矩阵，M(θ)是n×n的正定对称矩阵，为关节变量θ的函数，为关节变量θ的一阶偏导，为关节变量θ的二阶偏导；n为矩阵的行数或列数；表示离心力和哥式力向量，是n×1的向量；G(q)表示重力矢量，是一个n×1的向量，与关节变量θ有关；对关节的驱动力矩进行简化，表示如下：τ＝τ0+τ力   (2)其中τ₀表示离心力、哥式力和重力矢量，求解表达式为，τ_力表示环境对机械臂的作用力矩，求解表达式为，虚拟推力是一个在遥操作机械臂完成给定工作任务过程中需要避障时假想出来的力；它的主要作用是在障碍阻碍机械臂运动时给机械臂提供的一个虚拟力；这个虚拟力作用在机械臂上，将机械臂拉离障碍物，实现避障；将遥操作机械臂上与障碍物距离最近的点定义为关键点，记为A0；当A0与障碍物的最小距离d0达到一定值时候，假想的虚拟推力开启；此时，虚拟推力f虚沿着障碍物指向关键点的方向(Bo指向Ao)作用于遥操作机械臂杆件，避障运动开始；遥操作机械臂受到的等效虚拟推力力矩为τ虚，根据力雅克比关系求解式子表示为：τ虚＝JT·f虚   (3)式中，d为通过传感器或视觉摄像机获得的障碍物与机械臂的距离，d_m为安全距离值，K为比例系数，J(θ)是m×n的机械臂末端的雅克比矩阵，M(θ)表示机械臂的质量惯性矩阵；在不受其他外力作用的情况下有：虚拟推力于关节加速度的关系表示为：在m维的操作空间中，用r表示机械臂末端执行器的位姿坐标向量，θ表示机械臂关节位姿坐标向量，遥操作机械臂的运动学方程表示如下：r(t)＝g(θ(t))   (1)式中，g是向量函数，将关节空间变量映射为任务空间变量；对式(1)两边同时求导，得到机械臂末端和关节的速度关系方程：式中，表示机械臂末端执行器的速度矢量；表示机械臂关节的速度矢量；J(θ)是m×n的机械臂末端的雅克比矩阵；将遥操作机械臂的避障问题中的机械臂避障作为主任务记为T0，末端轨迹追踪运动运动作为子任务记为T，则对应的雅克比矩阵分别为J0和J,运动学方程为:r0(t)＝g0(θ(t))   (5)r(t)＝g(θ(t))   (6)遥操作机械臂关节速度的逆运动学近似求解方程表示如下：式中，表示避障运动，表示机械臂末端的轨迹追踪运动；其中，避障运动是在一维平面中完成，将从A0指向B0的向量记为d0，则单位向量n0表示如下：则关键点在笛卡尔空间的运动雅克比矩阵J₀简化为关键点避障运动速度简化为定义如下：式中σ表示障碍物的活跃程度，当σ＝0时，障碍物离机械臂杆件较远，障碍物不活跃；当σ≠0时，障碍物变得活跃，其活跃程度由下式求解：式中，dm为安全距离阀值，当||d0||→∞时，σ＝0；根据式(6)，运动学逆解公式重新表示如下：通过在避障环节加入虚拟推力，对任务避障优先级避障方法进行了优化。