买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：安徽工业大学

摘要：本发明公开了一种基于虚拟力的机械臂末端避障方法，属于机械臂运动规划技术领域。本发明的基于虚拟力的机械臂末端避障方法，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，简化了仿真中繁琐的计算，基于虚拟力无碰撞约束的公式不仅简单而且容易实时更新机械臂避障碍运动。在末端避障问题求解中，改变传统方法只考虑距离的缺点，通过障碍物和机械臂末端之间不同的速度、距离和移动方向，动态排斥场可以产生平滑的虚拟力斥力，给末端执行器增加了一个避障速度，解决障碍物出现在末端期望轨迹上的问题。

主权项：1.一种基于虚拟力的机械臂末端避障方法，其特征在于，步骤为：S1、建立机械臂运动学模型：采用标准的D-H法建立机械臂运动学模型，通过机械臂关节坐标系相对于基坐标系的齐次变换矩阵进行坐标变换： 式中，i＝1,2,…,n，为机械臂关节序号，cosθi，sinθi，cosαi-1，sinαi-1缩写为cθi，sθi，cαi-1，sαi-1；Rotxi-1,αi-1为绕xi-1轴转αi-1角度变换矩阵，Transxi-1,ai-1为沿xi-1轴平移αi-1的变换矩阵，Rotzi,θi为绕zi轴转θi角度变换矩阵，Transzi,di为绕zi轴平移di的变换矩阵；机械臂末端相对于基座标系的变换矩阵通过上式各杆件的变换矩阵得到：S2、建立机械臂-障碍物模型：计算机械臂工作空间中任意一点qcx,y,z在障碍物坐标系oobs-xobsyobszobs下的位置，用于判别机械臂连杆与障碍物伪距离大小；S3、计算伪距离最小值：采用向量方法来表达机械臂和障碍物之间的最小伪距离值；S4、改进虚拟斥力函数：考虑到末端执行器的速度和运动方向，改进虚拟斥力函数，并求出负梯度函数；将步骤S3中求出的最小伪距离值带入到改进虚拟斥力函数，求出机械臂末端和障碍物之间虚拟斥力，对机械臂末端轨迹进行规划；S5、利用算法实现末端避障任务：将步骤S4中的求出的虚拟斥力带入到机械臂末端避障方法中，实现机械臂末端避障；所述步骤S2中建立机械臂-障碍物模型，利用伪距离的概念来构造防碰撞约束的方法，每个障碍都包含一个超二次曲面的解析表达式，更定性地表达了障碍物与机械手之间的约束条件，以障碍物几何中心为原点建立坐标系，空间点到障碍物球体的超曲面伪距离解析方程为: 式中，Spx，y，z为障碍物坐标空间中伪距离，x0，y0，z0为超二次曲面中心点；Rs＝robs+ri，为预设障碍物安全距离，robs为障碍物包络体半径，ri为模拟机械臂厚度半径；所述步骤S3中向量方法表示为：Spx，y，z＝XTQX+BTX+C；其中，X＝[x，y，z]T，Q＝diag[1Rs2，1Rs2，1Rs2]，B＝[-2x0Rs2，-2y0Rs2，-2z0Rs2]T，Q、B、C均为常量；所述步骤S4中斥力函数为： 其中，Fdynx，v为由速度和位置产生的虚拟力，Spx为机械臂末端和障碍物之间的伪距离，β是一个常数，以调整速度矢量角θ的影响，θ表示当前速度矢量v和末端执行器位置qc之间的角度，qc为相对于障碍物的位置；为梯度，表示对引力场函数求负梯度；表示对伪距离求梯度；所述步骤S4中，基于公式计算，当θ在之间时，表示机器人正在接近障碍物，动态斥力场开始工作；当θ值在以内时，机器人远离障碍物，动态斥力场不起作用；所述步骤S5中，基于步骤S4中产生的虚拟斥力对机械臂末端速度进行修正，对传统的避障算法修正后的运动学方程：其中，为末端速度矢量；雅可比矩阵J为机械臂关节角速度向量在的线性映射；J∈Rn×m，n，m为关节空间和操作空间的维数；J0为障碍物在连杆上最小欧氏距离投影点对应的雅可比矩阵，N＝I-J+J，I∈Rn×n为单位矩阵，J+表示对矩阵求伪逆，J+＝JTJJT-1；为上述虚拟斥力Fdynx,v产生的速度调整量；xd为期望轨迹。

全文数据：

权利要求：

百度查询： 安徽工业大学 一种基于虚拟力的机械臂末端避障方法