(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210650939.7 (22)申请日 2022.06.09 (71)申请人 郑州大学 地址 450000 河南省郑州市高新 技术开发 区科学大道100号 (72)发明人 徐一村　王浩南　姬玉杰　程磊　 孙宏伟　刘超　张本顺　 (74)专利代理 机构 宁波海曙甬睿专利代理事务 所(普通合伙) 33330 专利代理师 王洋 (51)Int.Cl. B25J 9/16(2006.01) G06F 17/11(2006.01) G06F 30/20(2020.01) (54)发明名称 一种多自由度机器人动力学建模和轨迹跟 踪方法 (57)摘要 本发明公开了一种多自由度机器人动力学 建模和轨迹跟踪方法， 包括推导逆动力学方程和 设计基于计算力矩法的模糊自适应补偿控制器 的过程。 本发 明结合了旋量理论和李 群李代数的 数学工具对 牛顿欧拉的递推方程进行简化， 降低 了算法复杂度， 获得更加简洁清晰的动力学表达 式， 极大的提高了动力学建模计算的效率； 本发 明再对机器人轨迹跟踪控制时， 设计模糊自适应 补偿控制器， 将力矩分为具有精确动力学标称的 系统和具有未知参数的不确定系统， 计算力矩控 制器控制采用上述建立的动力学方程来对具有 精确动力学标称的系统进行力矩的求解， 模糊自 适应补偿控制器则对具有未知参数的不确定系 统进行补偿， 以保证机器人系统的轨迹跟踪精 度。 权利要求书3页 说明书10页 附图2页 CN 115157238 A 2022.10.11 CN 115157238 A 1.一种多自由度机器人动力学建模和轨 迹跟踪方法， 其特 征在于， 包括如下步骤： 步骤一、 推导逆动力学 方程： 机器人操作臂的每个连杆的位姿都可用欧氏变换来表示， 连杆i相对于连杆i ‑1的位姿 可表示为 则机器人操作臂末端相对于基坐标系的位姿用指数积公式 表示为： 由旋量理论定义连 杆i在体坐标系下{i}的广义速度： 符号Ad表示李群的子集在李代数上的线性映射， 由李群李代数的定义再结合伴随变 换， 连杆i在体坐标系下的广义速度为： 由上述广义速度可 得广义加速度的李 代数表达： 在体坐标{i}下， 作用在连 杆i上的合力为： 其中， Ad对偶的伴随算子 用Ad*表示， 定义为对偶空间的线性映射， 定义李代数的对偶相邻表达 式为ad*， 结合李代数 的伴随变换性质， 可以化简得到： 根据力的相互作用原理和李代数 伴随变换的性质， 连杆i+1作用在连杆i上的反作用力为： 因此连杆i承受的力 矩和力为： 整理可得： 总结上述推导的公式， 机器人逆动力学 方程的矩阵形式表示 为： 步骤二、 设计 基于计算力矩法的模糊自适应补偿控制器： 首先假设机器人不受结构不确定性和非结构不确定性的影响， 动力学表示 为：权　利　要　求　书 1/3 页 2 CN 115157238 A 2对这部分进行计算力矩法控制的设计， 令 e＝qd‑q， qd为期望的角度， Kd和Kp为正定矩阵， 因此该控制的闭环方程表示 为： 此时， 计算力矩法控制律 为： 考虑到机器人系统中的结构不确定性和非结构不确定性的影响， 动力学方程表达式 为： 定义M(q)＝Mo(q)‑ΔM(q)， G(q)＝Go(q)‑ΔG(q)， 其中结构 不确定项为ΔM(q)， ΔG(q)， 为未建模动态项包括摩擦项和外部扰动， 则 该控制的闭环方程 为： 定义τ＝τo+τc， τc为计算力矩的补偿， 接下来开始对结构不确性和非结构不确定性进行 自适应模糊补偿控制的设计； 控制系统中补偿力矩为 表示为ρ 的模糊逼近估计值， 为一个 权值矩阵， 为模糊基函数矢量， 模糊隶属度函数 采用高斯型隶 属度函数； ρ用模糊逻辑系统进行表示： ρ ＝W*ζ + ε 其中， W*为W的最优值， 并满足： 将系统的总控制律 τ ＝ τo+τc带入系统动力学 方程中， 可 得系统闭环方程： 其中， 表示为模糊系统的权值矩阵； 为了便于分析将系统的闭环方程转化为系统 的状态空间方程形式， 定义系统误差状态 矢量 定义系统的状态空间方程为： 模糊系统参数的自适应调节规 律为： 权　利　要　求　书 2/3 页 3 CN 115157238 A 3