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

权利要求书

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_虚的作用下，遥操作机械臂末端执行器沿着躲避障碍物的方向运动，在机械臂自运动和虚拟推力的双重作用下完成障碍物的躲避动作；

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

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

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