快速终端滑模阻抗控制算法.pdf

《快速终端滑模阻抗控制算法.pdf》由会员分享，可在线阅读，更多相关《快速终端滑模阻抗控制算法.pdf（18页完成版）》请在专利查询网上搜索。

1、(19)中华人民共和国国家知识产权局 (12)发明专利申请 (10)申请公布号 (43)申请公布日 (21)申请号 202010019659.7 (22)申请日 2020.01.08 (71)申请人 南昌大学 地址 330000 江西省南昌市红谷滩新区学 府大道999号 (72)发明人 李春泉何永华杨峰 (74)专利代理机构 北京众合诚成知识产权代理 有限公司 11246 代理人 袁红梅 (51)Int.Cl. G05B 13/04(2006.01) (54)发明名称 一种快速终端滑模阻抗控制算法 (57)摘要 本发明提供了一种快速终端滑模阻抗控制 算法。 主要包括以下步骤： 步骤1， 将力反。

2、馈设备 简化为双连杆模型； 步骤2， 求解简化模型的运动 学； 步骤3， 求解简化模型的动力学； 步骤4， 建立 阻抗模型； 步骤5， 设计快速终端滑模控制器。 本 发明的优点在于： 一、 引入了终端吸引子， 保证了 系统状态能够在有限时间内收敛到平衡点； 二， 保留了线性滑模控制部分， 因此具有在平衡点附 近快速收敛的特点。 快速终端滑模阻抗控制使系 统状态能够快速、 精准的收敛到平衡点。 与滑模 阻抗控制算法相比， 快速终端滑模阻抗控制具有 更小的位置跟踪误差。 权利要求书5页 说明书11页 附图1页 CN 111208730 A 2020.05.29 CN 111208730 A 1.一。

3、种快速终端滑模阻抗控制算法， 其特征在于： 按以下步骤： 步骤1、 将力反馈设备简化为双连杆模型； 步骤2、 求解力反馈设备模型的运动学； 步骤3、 求解力反馈设备模型的动力学； 步骤4、 建立阻抗模型； 步骤5、 设计快速终端滑模控制器。 2.根据权利要求1所述的一种快速终端滑模阻抗控制算法， 其特征在于： 所述步骤1中 将力反馈设备简化为双连杆模型具体如下： 力反馈设备具有N个可旋转的关节J1、 J2JN， N7， 关节J1控制的是设备末端在水平 面的运动， 控制算法所需要补偿的重力以及末端在工作空间中的运动主要来自关节J2JN， 将力反馈设备简化为双连杆模型， 并设定连杆为均匀质杆， 简。

4、化后的双连杆模型关节1记作 J1， 关节2记作J2； 其中， J1由原力反馈设备的关节J2JN-1合并而来， J2是原力反馈设备 的关节JN， 并将原力反馈设备的的关节J1省去。 3.根据权利要求2所述的一种快速终端滑模阻抗控制算法， 其特征在于： 所述步骤2中 具体求解力反馈设备模型运动学的算式如下： (a)正运动学求解算式： 式中， l1和l2分别是简化后模型关节J1的连杆长和关节J2的连杆长， 1是l1与水平面 间的夹角， 2是l2和竖直平面之间的夹角， (x1,x2)是设备末端在二维平面上的位置坐标； (b)逆运动学求解算式： 式中， r是设备末端到关节J1之间的距离； 式中， 是r与。

5、水平面的夹角； 再通过余弦定理得到： 式中， 是l1与r之间的夹角； 考虑力反馈设备的实际工作空间， 0， 因此 1的计算公式为： 1+ 通过余弦定理， 得到角 的计算公式： 为杆长l1和l2之间的夹角 得到 2的计算公式： 权利要求书 1/5 页 2 CN 111208730 A 2 定义xx1 x2， 12， 得雅可比矩阵公式： 将正运动学公式带入上式， 可得： 雅可比矩阵的微分为： 4.根据权利要求3所述的一种快速终端滑模阻抗控制算法， 其特征在于： 所述步骤3中 具体求解力反馈设备模型动力学的算式如下： 连杆l1动能为： 连杆l2质心C在x1上的线速度为： 连杆l2质心C在x2上的线速。

6、度为： 可得： 连杆l2的动能： 系统的总动能为： 系统的总势能： 权利要求书 2/5 页 3 CN 111208730 A 3 将系统的总动能和系统的总势能代入拉格朗日函数中， 得到： 关节力矩的计算 简化模型的关节J1的驱动力矩计算公式如下： 式中： 得到力矩 1的计算公式： 简化模型的关节J2的驱动力矩计算公式如下： 式中： 得到力矩 2的计算公式： 权利要求书 3/5 页 4 CN 111208730 A 4 整理力矩 1和 2的计算公式， 最终得到力矩的表达式为： 5.根据权利要求4所述的一种快速终端滑模阻抗控制算法， 其特征在于： 所述步骤4阻 抗模型的建立具体为： 力反馈设备末端。

7、的接触力为Fe， 将Fe与理想位置误差xc-xd建立阻抗关系， 描述为： 其中， xc为末端位置的指令轨迹， xd为理想的阻抗轨迹， x(0)xc(0)，Md、 Bd、 Kd为阻抗参数， 分别是质量、 阻尼和刚度系数矩阵， 通过上述的阻抗关系公式计算出需要 跟踪的理想阻抗轨迹， 再根据动力学模型， 设计力反馈设备末端的控制律Fx。 6.根据权利要求5所述的一种快速终端滑模阻抗控制算法， 其特征在于： 所述步骤5快 速终端滑模控制器的设计具体为： 定义快速终端滑模切换面： 其中， xd(t)为理想轨迹，和分别为理想速度和加速度， 1和2为大于零的 正定常数矩阵， 和 ( )为正奇数； 定义中间参。

8、考变量： 对上式微分， 可得： 则有： 将中间参考变量的微分式代入上式中， 可得： 设计快速终端滑模控制器为： 权利要求书 4/5 页 5 CN 111208730 A 5 其中， K0， 0， tanh为双曲正切函数。 权利要求书 5/5 页 6 CN 111208730 A 6 一种快速终端滑模阻抗控制算法 技术领域 0001 本发明涉及力反馈设备与物体之间的交互， 用于提高阻抗控制的抗干扰能力、 动 态响应速度以及跟踪精度， 能够降低力反馈设备与物体接触产生的相互作用力， 从而保护 力反馈设备和接触物体。 背景技术 0002 众所周知， 人类的感觉主要由听觉、 嗅觉、 味觉、 视觉和力触。

9、觉组成。 力触觉又可分 为力觉和触觉， 其中力觉是对压力、 刚度等的感知， 而触觉是对纹理、 温度等的感知。 力触觉 是我们日常生活非常倚重的感觉， 例如： 完成开门、 书写、 打球等接触性的活动都需要依赖 于力触觉。 Marc O Ernst在 Nature 中指出人的力触觉反馈信息可以修正视觉上的偏差。 研究表明， 在人机交互的实验中， 完成插销入孔、 组装工件等任务， 与仅有图像显示相比， 将 视觉和力触觉反馈相结合， 能够极大地提高操作的精度和效率。 因此， 与其他感觉相比， 力 触觉更加微妙、 细腻和直接， 在人类的感知、 决策和判断中起着非常重要的作用。 0003 力触觉的实现依赖。

10、于力触觉反馈设备(或者力反馈设备)。 一个理想的力反馈设备 应当具有如下特点： 当操作者操作该设备与虚拟环境或者远端环境进行人机交互时， 操作 者在交互过程中所感觉到的力触觉与亲临现场进行真实操作所感觉到的一样。 因此， 如何 使力反馈设备能够构建真实、 准确、 可靠的临场感效果， 是力触觉反馈设备设计中最具有挑 战性的问题。 然而， 考虑到通常情况下， 力反馈设备未添加被动柔顺装置， 设备的机械阻抗 是固定且较高， 这会导致接触力过大， 损坏设备甚至威胁操作者的安全。 因此， 常使用阻抗 控制算法控制力反馈设备。 阻抗控制算法通过建立位置与接触力的关系， 使力反馈设备在 受限环境中实现位置控。

11、制的同时， 能够保持预先设定接触力， 从而有效地保护力反馈设备 和接触物。 0004 现有的阻抗控制算法有： Vu Minh Hung等人提出了一种新的自适应控制算法(详 见： Model Reference Adaptive Control of a Haptic Feedback Device for Improving Force PerformanceJ.Science&Technology Development,2014,17(K1):102114.)， 该 算法将人手施加到力反馈设备上的力模拟为扰动， 采用模型参考自适应控制算法改进力的 跟踪性能， 但该方法需要实时调整自适应率修。

12、正控制器的参数， 将会降低控制算法实时性。 李二超等人提出了一种模糊滑模阻抗控制算法(详见： 未知环境下机器人模糊滑模阻抗控 制J.电气自动化,2010,32(4):12-13.)， 利用模糊控制来克服滑模控制的抖振问题， 但在 模糊阻抗控制中， 若模糊控制器将信息简单地模糊处理， 会降低控制精度， 若处理较为复 杂， 则失去模糊控制的优势。 Kim等人提出了一种基于径向基函数(RBF)神经网络算法的滑 模阻抗控制方法(详见： Impedance control of robot manipulator using artificial intelligenceC.ICCAS 2010.IEE。

13、E,2010:1891-1894.)， 用于实时估计所需阻抗模型的惯 性、 阻尼、 刚度等设计参数， 而在神经网络阻抗控制中， 若使用的网络学习能力不够， 将导致 对系统不确定项的逼近精确不够。 Garcia等人提出了一种无抖振、 二阶滑模控制算法， 并且 与阻抗控制相结合(详见： Observer-based higher-order sliding mode impedance 说明书 1/11 页 7 CN 111208730 A 7 control of bilateral teleoperation under constant unknown time delayC.2006 IE。

14、EE/RSJ International Conference on Intelligent Robots and Systems.IEEE, 2006:1692-1699.)， 以保证在未知常数时延下的鲁棒跟踪。 0005 综上所述可知， 自适应阻抗控制算法、 模糊阻抗控制算法以及神经网络阻抗控制 算法分别在算法的实时性、 处理信息的复杂程度以及算法精确度方面存在一定的问题。 相 较而言， 滑模阻抗控制算法具有抗扰动能力强， 结构简单， 对系统模型精确度要求不高等优 点， 更适用于力反馈设备与物体之间的交互。 发明内容 0006 基于上述背景， 本发明提出了一种快速终端滑模阻抗控制算法。 将。

15、快速终端滑模 控制引入阻抗控制中， 保证了系统状态能够在有限时间内收敛到平衡点， 提高阻抗控制的 抗干扰能力、 动态响应速度以及跟踪精度， 使系统具备抗干扰能力强和快速、 精准收敛的特 点。 与滑模阻抗控制算法相比， 快速终端滑模阻抗控制具有更小的位置跟踪误差。 本发明是 通过以下技术方案实现的。 0007 本发明所述的一种快速终端滑模阻抗控制算法， 按以下步骤： 0008 步骤1、 将力反馈设备简化为双连杆模型； 0009 步骤2、 求解力反馈设备模型的运动学； 0010 步骤3、 求解力反馈设备模型的动力学； 0011 步骤4、 建立阻抗模型； 0012 步骤5、 设计快速终端滑模控制器。。

16、 0013 进一步的， 所述步骤1中将力反馈设备简化为双连杆模型具体如下： 0014 力反馈设备具有N个可旋转的关节J1、 J2JN， (N7)， 关节J1控制的是设备末端 在水平面的运动， 即设备的左右转动。 控制算法所需要补偿的重力以及末端在工作空间中 的运动主要来自关节J2JN。 为提高工作效率， 将力反馈设备简化为双连杆模型， 并设定连 杆为均匀质杆。 简化后的双连杆模型关节1记作J1， 关节2记作J2。 其中， J1由原力反馈设 备的关节J2JN-1合并而来， J2是原力反馈设备的关节JN， 并将原力反馈设备的的关节J1省 去。 0015 进一步的， 所述步骤2中具体求解力反馈设备模。

17、型运动学的算式如下： 0016 (a)正运动学求解算式： 0017 0018 式中， l1和l2分别是简化后模型关节J1的连杆长和关节J2的连杆长， 1是l1与水 平面间的夹角， 2是l2和竖直平面之间的夹角， (x1,x2)是设备末端在二维平面上的位置坐 标。 0019 (b)逆运动学求解算式： 0020 0021 式中， r是设备末端到关节J1之间的距离 说明书 2/11 页 8 CN 111208730 A 8 0022 0023 式中， 是r与水平面的夹角 0024 再通过余弦定理得到： 0025 0026 式中， 是l1与r之间的夹角 0027 考虑力反馈设备的实际工作空间， 0， 。

18、因此 1的计算公式为： 0028 1+ 0029 通过余弦定理， 得到角 的计算公式： 0030 0031 为杆长l1和l2之间的夹角 0032 得到 2的计算公式： 0033 0034 定义xx1 x2， 12， 得雅可比矩阵公式： 0035 0036 将正运动学公式带入上式， 可得： 0037 0038 雅可比矩阵的微分为： 0039 0040 进一步的， 所述步骤3中具体求解力反馈设备模型动力学的算式如下： 0041 连杆l1动能为： 0042 0043 连杆l2质心C在x1上的线速度为： 0044 0045 连杆l2质心C在x2上的线速度为： 0046 0047 可得： 说明书 3/1。

19、1 页 9 CN 111208730 A 9 0048 0049 连杆l2的动能： 0050 0051 0052 系统的总动能为： 0053 0054 系统的总势能： 0055 0056 将系统的总动能和系统的总势能代入拉格朗日函数中， 得到： 0057 0058 关节力矩的计算 0059 简化模型的关节J1的驱动力矩计算公式如下： 0060 0061 式中： 0062 0063 0064 0065 得到力矩 1的计算公式： 说明书 4/11 页 10 CN 111208730 A 10 0066 0067 简化模型的关节J2的驱动力矩计算公式如下： 0068 0069 式中： 0070 00。

20、71 0072 0073 得到力矩 2的计算公式： 0074 0075 整理力矩 1和 2的计算公式， 最终得到力矩的表达式为： 0076 0077 进一步的， 所述步骤4阻抗模型的建立具体为： 0078 力反馈设备末端的接触力为Fe， 将Fe与理想位置误差xc-xd建立阻抗关系， 描述为： 0079 0080 其中 ， xc为末端位置的指令轨迹 ， xd为理想的阻抗轨迹 ， x(0)xc(0) ， Md、 Bd、 Kd为阻抗参数， 分别是质量、 阻尼和刚度系数矩阵。 通过上述的阻抗关 系公式计算出需要跟踪的理想阻抗轨迹， 再根据动力学模型， 设计力反馈设备末端的控制 律Fx。 0081 进一。

21、步的， 所述步骤5快速终端滑模控制器的设计具体为： 0082 定义快速终端滑模切换面： 说明书 5/11 页 11 CN 111208730 A 11 0083 0084其中， xd(t)为理想轨迹，和分别为理想速度和加速度。 1和2为大于 零的正定常数矩阵， 和 ( )为正奇数。 0085 定义中间参考变量： 0086 0087 对上式微分， 可得： 0088 0089 则有： 0090 0091 将中间参考变量的微分式代入上式中， 可得： 0092 0093 设计快速终端滑模控制器为： 0094 0095 其中， K0， 0， tanh为双曲正切函数。 0096 与现有技术相比， 本发明的。

22、有益效果是： 0097 (1)引入了终端吸引子， 保证了系统状态能够在有限时间内收敛到平衡点； 0098 (2)保留了线性滑模控制部分， 因此具有在平衡点附近快速收敛的特点。 快速终端 滑模阻抗控制使系统状态能够快速、 精准的收敛到平衡点。 与滑模阻抗控制算法相比， 快速 终端滑模阻抗控制具有更小的位置跟踪误差。 附图说明 0099 图1为力反馈设备两连杆简化模型分析图。 图中： x1和x2分别表示二维平面中的横 轴和纵轴， (x1,x2)是设备末端在二维平面上的位置坐标， l1和l2分别是关节J1的连杆长和 关节J2的连杆长， 1为l1与横轴间的夹角， 2为l2与纵轴间的夹角， 为杆长l1和。

23、l2之间的夹 角， r是设备末端到关节J1之间的距离， 是r与水平面的夹角， 是l1与r之间的夹角。 具体实施方式 0100 本发明将通过以下实例作进一步说明。 0101 步骤1， 将力反馈设备简化为连杆模型具体为： 0102 力反馈设备具有3个可旋转的关节J1、 J2、 J3， 关节J1控制的是设备末端在水平面的 运动， 即设备的左右转动。 控制算法所需要补偿的重力以及末端在工作空间中的运动主要 来自关节J2、 J3。 为提高工作效率， 将力反馈设备简化为双连杆模型， 并设定连杆为均匀质 杆。 简化后的双连杆模型关节1记作J1， 关节2记作J2。 其中， J1为原力反馈设备的关节J2， 说明。

24、书 6/11 页 12 CN 111208730 A 12 J2是原力反馈设备的关节J3， 并将原力反馈设备的的关节J1省去。 0103 步骤2， 求解力反馈设备模型的运动学算式如下： 0104 (a)正运动学求解算式： 0105 0106 式中， l1和l2分别是简化后模型关节J1的连杆长和关节J2的连杆长， 1是l1与水 平面间的夹角， 2是l2和竖直平面之间的夹角， (x1,x2)是设备末端在二维平面上的位置坐 标。 0107 (b)逆运动学求解算式： 0108 0109 式中， r是设备末端到关节J1之间的距离 0110 0111 式中， 是r与水平面的夹角 0112 再通过余弦定理得。

25、到： 0113 0114 式中， 是l1与r之间的夹角 0115 考虑力反馈设备的实际工作空间， 0， 因此 1的计算公式为： 0116 1+ 0117 通过余弦定理， 得到角 的计算公式： 0118 0119 为杆长l1和l2之间的夹角 0120 根据附图1， 得到 2的计算公式： 0121 0122 定义xx1 x2， 12， 得雅可比矩阵公式： 0123 0124 将正运动学公式带入上式， 可得： 0125 0126 雅可比矩阵的微分为： 说明书 7/11 页 13 CN 111208730 A 13 0127 0128 步骤3， 求解力反馈设备模型的动力学算式如下： 0129 连杆l1。

26、动能为： 0130 0131 连杆l2质心C在x1上的线速度为： 0132 0133 连杆l2质心C在x2上的线速度为： 0134 0135 可得： 0136 0137 连杆l2的动能： 0138 0139 系统的总动能为： 0140 0141 系统的总势能： 0142 0143 将系统的总动能和系统的总势能代入拉格朗日函数中， 得到： 0144 0145 关节力矩的计算 0146 简化模型的关节J1的驱动力矩计算公式如下： 说明书 8/11 页 14 CN 111208730 A 14 0147 0148 式中： 0149 0150 0151 0152 得到力矩 1的计算公式： 0153 0。

27、154 简化模型的关节J2的驱动力矩计算公式如下： 0155 0156 式中： 0157 0158 0159 0160 得到力矩 2的计算公式： 0161 0162 整理力矩 1和 2的计算公式， 最终得到力矩的表达式为： 说明书 9/11 页 15 CN 111208730 A 15 0163 0164 步骤4， 阻抗模型的建立具体为： 0165 力反馈设备末端的接触力为Fe， 将Fe与理想位置误差xc-xd建立阻抗关系， 描述为： 0166 0167 其中 ， xc为末端位置的指令轨迹 ， xd为理想的阻抗轨迹 ， x(0)xc(0) ， Md、 Bd、 Kd为阻抗参数， 分别是质量、 阻。

28、尼和刚度系数矩阵。 0168 通过上述的阻抗关系公式计算出需要跟踪的理想阻抗轨迹， 再根据动力学模型， 设计力反馈设备末端的控制律Fx。 0169 步骤5， 快速终端滑模控制器的设计具体为： 0170 定义快速终端滑模切换面： 0171 0172其中， xd(t)为理想轨迹，和分别为理想速度和加速度。 1和2为大于 零的正定常数矩阵， 和 ( )为正奇数。 0173 定义中间参考变量： 0174 0175 对上式微分， 可得： 0176 0177 则有： 0178 0179 将中间参考变量的微分式代入上式中， 可得： 0180 0181 设计快速终端滑模控制器为： 0182 0183 其中， K0， 0， tanh为双曲正切函数。 说明书 10/11 页 16 CN 111208730 A 16 0184 以上所述仅表达了本发明的优选实施方式， 其描述较为具体和详细， 但并不能因 此而理解为对本发明专利范围的限制。 应当指出的是， 对于本领域的普通技术人员来说， 在 不脱离本发明构思的前提下， 还可以做出若干变形、 改进及替代， 这些都属于本发明的保护 范围。 因此， 本发明专利的保护范围应以所附权利要求为准。 说明书 11/11 页 17 CN 111208730 A 17 图1 说明书附图 1/1 页 18 CN 111208730 A 18 。