(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210643516.2 (22)申请日 2022.06.09 (71)申请人 安徽工业大学 地址 243002 安徽省马鞍山市花 山区湖东 路59号 (72)发明人 徐向荣　杨浩　游天涯　李琦琦　 (74)专利代理 机构 安徽知问律师事务所 34134 专利代理师 侯晔 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于虚拟力的机 械臂末端 避障方法 (57)摘要 本发明公开了一种基于虚拟力的机械臂末 端避障方法， 属于机械臂运动规划技术领域。 本 发明的基于虚拟力的机械臂末端避障方法， 利用 伪距离的概念来构造防碰撞约束的方法， 每个障 碍都包含一个超二次曲面的解析表达式， 更定性 地表达了障碍物与机械手 之间的约束条件， 简化 了仿真中繁琐的计算， 基于虚拟力无碰撞约束的 公式不仅简单而且容易实时更新机械臂避障碍 运动。 在末端避障问题求解中， 改变传统方法只 考虑距离的缺点， 通过障碍物和机械臂末端之间 不同的速度、 距离和移动方向， 动态排斥场可 以 产生平滑的虚拟力斥力， 给末端执行器增加了一 个避障速度， 解决障碍物出现在末端期望轨迹上 的问题。 权利要求书2页 说明书8页 附图4页 CN 115026816 A 2022.09.09 CN 115026816 A 1.一种基于虚拟力的机 械臂末端 避障方法， 其特 征在于， 步骤为： S1、 建立机械臂运动学模型： 采用标准的D ‑H法建立机械臂运动学模型， 通过机械臂关 节坐标系相对于基坐标系的齐次变换矩阵进行坐标变换： 式中， i(i＝1,2, …,n)为机械臂关节序号， c osθi， sinθi， cosαi‑1， sinαi‑1缩写为cθi， sθi， cαi‑1， sαi‑1； Rot(xi‑1, αi‑1)为绕xi‑1轴转αi‑1角度变换矩阵， Trans(xi‑1,ai‑1)为沿xi‑1轴平移 αi‑1的变换矩阵， Rot(zi, θi)为绕zi轴转θi角度变换矩阵， Trans(zi,di)为绕zi轴平移di的变 换矩阵； 机械臂末端相对于基座标系的变换矩阵通过 上式各杆件的变换矩阵 得到： S2、 建立机械臂 ‑障碍物模型： 计算机械臂工作空间中任 意一点qc(x,y,z)在障碍物坐标 系oobs‑xobsyobszobs下的位置， 用于判别机 械臂连杆与障碍物伪距离大小； S3、 计算伪距离最小值： 采用向量方法来表达 机械臂和障碍物之间的最小伪距离值； S4、 改进虚拟斥力函数： 考虑到末端执行器的速度和运动方向， 改进虚拟斥力函数， 并 求出负梯度函数； 将步骤S3中求出 的最小伪距离值带入到改进虚拟斥力函数， 求出机械臂 末端和障碍物之间虚拟斥力， 对机 械臂末端轨 迹进行规划； S5、 利用算法实现末端避障任务： 将步骤S4中的求出的虚拟斥力带入到机械臂末端避 障方法中， 实现机 械臂末端 避障。 2.如权利要求1所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S2中 建立机械臂 ‑障碍物模型， 利用伪距离的概念来构 造防碰撞约束的方法， 每个障碍都包含一 个超二次曲面的解析表达式， 更定性地表达了障碍物与机械手之间的约束条件， 以障碍物 几何中心为原点建立 坐标系， 空间点到障碍物球 体的超曲面伪距离解析 方程为: 式中, 为障碍物坐 标空间中伪距离， (x0,y0,z0)为超二次曲面中心点； Rs＝robs+ri， 为 预设障碍物安全距离， robs为障碍物包络体半径， ri为模拟机 械臂厚度半径。 3.如权利要求2所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S2中 判别机械臂连杆与障碍物伪距离大小， 任务空间中任意一点 qc(x,y,z)， 计算出相应的伪距 离， 为了满足避障过程安全的进行， 避障过程中最小伪距离Sp,min(qc)始终大于障碍物包络 物的预设阈值dpm， 机械臂处于安全状态。 4.如权利要求3所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S3 中 向量方法表示 为： Sp(x,y,z)＝XTQX+BTX+C 其中， X＝[x,y,z]T， Q＝diag[1/Rs2,1/Rs2,1/Rs2]， B＝[‑2x0/Rs2,‑2y0/Rs2,‑2z0/Rs2]T， Q、 B、 C均为常量。 5.如权利要求4所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S4中权　利　要　求　书 1/2 页 2 CN 115026816 A 2采用虚拟引力斥力对机械臂末端轨迹进行规划， 即障碍物包络体与末端 执行器之 间的产生 一种新颖的虚拟排斥力， 对末端执行器轨迹进行修正， 使其在完成障碍物出现在期望轨迹 上的情境。 6.如权利要求5所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S4中 斥力函数为: 其中,Fdyn(x,v)为由速度和位置产生的虚拟力， Sp(x)为机械臂末端和障碍物之间的伪 距离， β 是一个常数， 以调整速度 矢量角 θ 的影响， θ表示当前速度 矢量ν和末端执行器位置qc (相对于障碍物的位置)之间的角度。 7.如权利要求6所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S4 中， 基于公 式 计算， 当θ在 之间时， 表 示机器人正在接近障碍物， 动态斥 力场开始工作； 当θ 值在 以内时， 机器人远离障碍物， 动态 斥力场不 起作用。 8.如权利要求6所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S5 中， 基于步骤S4中产生的虚拟斥力对机械臂末端速度 进行修正， 对传统的避障算法修正 后的运动学 方程： 其中， 为末端速度矢量， 雅可比矩阵J为机械臂关节角速度向量 在 的线性 映射( J∈Rn×m， n， m为关节空间和操作空间的维数)， J0为障碍物在连杆上最 小欧氏距离投影点对应的雅可比矩 阵， N＝I‑J+J， I∈Rn×n为单位矩阵， J+表示对矩阵求伪 逆， J+＝JT(JJT)‑1； 为上述虚拟斥力Fdyn(x,v)产生的速度调整量。 9.如权利要求8所述的基于虚拟力的机械臂末端避障方法， 其特征在于， 所述步骤S5当 障碍物出现在末端执行器期望轨迹上时， 即最近距离点为机械臂末端执行器位置， 改变末 端运动速度轨迹， 末端执行器不 断向障碍物靠近， 最小伪距离的减小， 同时θ在 时处于 虚拟斥力不断增大， 产生较大 的速度进行修正， 作用在末端执行器使其避开期望轨迹上 的 障碍物。权　利　要　求　书 2/2 页 3 CN 115026816 A 3