(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210832890.7 (22)申请日 2022.07.15 (71)申请人 海南大学 地址 海南省海口市人民大道58号 (72)发明人 郭东生　张卫东　潘阳　熊明磊　 朱晓光　王涛　 (74)专利代理 机构 佛山知正知识产权代理事务 所(特殊普通 合伙) 44483 专利代理师 胡晗绯 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种关节受限的冗余机械臂伪逆型重复运 动规划方法 (57)摘要 本发明提供一种关节受限的冗余机械臂伪 逆型重复运动规划方法， 如下： 考虑机械臂关节 的角度极限和速度极限， 引入非负向量， 推导可 用于关节极限处理的动态方程； 根据机械臂当前 状态与初始 状态之间的误差最小化思想， 推导在 加速度层上描述的重复运动判据； 在动态方程以 及重复运动判据基础上， 结合冗余机械臂的运动 学方程， 并引入末端误差的反馈， 设计加速度层 重复运动规划方案； 根据重复运动规划方案的计 算结果， 冗余机械臂的下位机控制器驱动各个关 节来完成给定的末端任务。 本发 明设计的伪逆型 重复运动规划方法， 能够使冗余机械臂在关节受 限的情况下仍可有效地完成给定的末端任务； 并 在任务完成后， 冗余机械臂的各个 关节回到其初 始状态。 权利要求书2页 说明书4页 附图1页 CN 115042186 A 2022.09.13 CN 115042186 A 1.一种关节受限的冗余机 械臂伪逆型重复运动规划方法， 其特 征在于， 包括如下步骤： S1:考虑机械臂关节的角度极限和速度极限， 引入一个非负向量， 并采用指数衰减公 式， 推导可用于关节极限处理的动态方程； 对于一个具有n自由度且关节受限的冗余机械 臂， 所推导的处 理关节极限的动态方程如下： 其中， 和 分别表示机械臂 的关节加速度和关节速度； v∈R2n表示动态方 程的决策向量且描述为v＝[v1； v2；…； v2n]， 表示v的时间导数， D∈R(2n)×(2n)表示对 角矩阵且定义 为D＝dia g{v1,v2,…,v2n}， v.2∈R2n表示非负向量且定义 为 C∈R(2n)×n表示增广系数矩阵且定义为C＝[ ‑I； I]， I∈Rn×n表 示单位矩阵； d∈R2n表示增广系数向量且定义 为 θ∈Rn表示机械臂的关节角度， θ±和 分别表示机械臂关节的角度极限和速度极限； η＞0∈R表示关节极 限处理的设计参数， γ＞0∈R表示动态方程的设计参数； S2:根据机械臂当前状态与初始状态之间的误差最小化思想， 再次采用指数衰减公式， 推导在加速度层上描述的重复运动判据； 所推导的加速度层重复运动判据如下： 其中， α ＞0∈R和β ＞0∈R均表示重复运动设计参数； θ0∈Rn表示机械臂关节角度的初始 值； S3:在所述动态方程以及重复运动判据的基础上， 结合冗余机械臂的运动学方程， 并引 入末端误差的反馈， 设计基于伪逆形式的加速度层重复运动规划方案； 所设计的伪逆型重 复运动规划方案如下： 其中， q∈ R3n表示联合决策向量且定义为 表示q的时间导数且定义 为 M∈R(m+2n)×(3n)和N∈R(m+2n)×(3n)均表示增广系数矩阵且定义如下： J∈Rm×n表示机械臂的雅 克比矩阵， 表示J的时间导数， λ＞0∈R表示设计参 数， M+∈R(3n)×(m+2n)表示M的伪逆矩阵； b∈Rm+2n表示增广系数向量且定义 为 e∈Rm表示机械臂的末端误差且定义为e＝f( θ ) ‑p， f(·)表示非 线性映射 函数， p∈Rm表示机械臂末端在m维空间中的位置， 表示p的时间导数， 表示 的时间导数， μ＞0∈R表示误差反馈系数； I3n∈R(3n)×(3n)表示单位矩阵， z∈权　利　要　求　书 1/2 页 2 CN 115042186 A 2R3n表示增广向量且 定义为 ρ ＞0∈R表示设计 参数， 1v∈R2n表示 由1组成的向量。 2.根据权利要求1所述的一种关节受限的冗余机械臂伪逆型重复运动规划方法， 其特 征在于， 根据重复运动规划方案的计算结果， 冗余机械臂的下位机控制 器驱动各个关节来 完成给定的末端任务， 并实现重复运动规划的目的。权　利　要　求　书 2/2 页 3 CN 115042186 A 3