(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210899576.0 (22)申请日 2022.07.28 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 刘安东　金哲豪　吕方正　朱华中　 俞立　张文安　 (74)专利代理 机构 杭州浙科专利事务所(普通 合伙) 33213 专利代理师 汤明 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于二次规划的约束变阻抗控制方法 (57)摘要 本发明公开了一种基于二次规划的约束变 阻抗控制方法， 该方法提出了一种可以限制机械 臂极限柔顺特性的变阻抗约束， 并将考虑该阻抗 约束的变阻抗控制算法设计问题转化为一个QP 问题； 另外提出在变阻抗模型中引入补偿力来处 理位置和速度约束， 并且将补偿力的设计问题也 转化成一个QP问题。 结合这两个QP问题 的解， 提 出了一种同时具有阻抗、 位置和速度约束的变阻 抗控制算法。 权利要求书5页 说明书10页 附图1页 CN 115107038 A 2022.09.27 CN 115107038 A 1.一种基于二次规划的约束变阻抗控制方法， 其特 征在于， 包括以下步骤： 1)设计状态反馈控制律： 一个n自由度的机器人动力学模型， 如式(1 ‑1)所示： 其中， q、 分别表示关节角度、 关节角速度、 关节角加速度； H(q)、 g(q)分别 表示n×n的惯性矩阵、 n ×n的科里奥利力矩阵、 n 维的重力矢量； J表 示的是m×n的雅可比矩 阵； f表示m维的相互作用力； τ表示关节力矩； 笛卡尔空间的加速度， 如式(1 ‑2)所示： 其中， 分别表示的是笛卡尔空间x方向的实际加速度、 笛卡尔空间y方向的实 际加速度、 笛卡尔空间z方向的实际加速度； 分别表示笛卡尔空间绕x方向的欧 拉角角加速度、 笛卡尔空间绕y方向的欧拉角角加速度、 笛卡尔空间绕z方向的欧拉角角加 速度； 笛卡尔空间速度与关节角速度之间的关系， 如式(1 ‑3)所示： 将上式求 导可得笛卡尔空间加速度与关节角速度之间的关系， 如式(1 ‑4)所示： 其中， J+表示雅克比矩阵的伪逆； 联立(1 ‑1)和(1‑4)得到一个新的控制器， 如式(1 ‑5)所 示： 其中， v表示笛卡尔空间的期望加速度； 2)设计阻抗控制模型： 笛卡尔空间的阻抗控制模型， 如式(1 ‑6)所示： 其中， M、 D、 K分别代表惯性矩阵、 阻尼矩阵、 刚度矩阵； ξ、 分别表示笛卡尔空间中的实 际位置和参考速度； 假设M、 D、 K是正定的对角矩阵， 将式(1 ‑6)模型解耦到几个一维模 型中， 解耦到x维的一维模型， 如式(1 ‑7)所示： 其中， 下标x表示所考虑的维度， mx、 dx、 kx、 fx分别表示解耦到x维的惯性矩阵、 阻尼矩阵、 刚度矩阵和相互作用力； x、 xr、 分别表示笛卡尔空间x维的实际位置、 期望位置、 实际 速度和期望速度； 由于状态的每个维都具有相同的控制策略， 都以x维的一维模型为例， 为 了方便后面都省去下 标x并将式(1 ‑7)重写为式(1 ‑8)：权　利　要　求　书 1/5 页 2 CN 115107038 A 2通过确定阻抗参数m、 d和k， 将v作为阻抗模型(1 ‑8)的状态反馈控制律； sr表示参考状 态； 3)基于二次规划的阻抗约束变阻抗控制律设计： 由于阻抗参数隐藏在控制输入中， 因 此使用步骤2)中的模型(1 ‑8)很难约束阻抗参数， 为了处理这个问题， 定义了一个新的输 入， 如式(1 ‑9)所示： 通过这个新的输入， 重写步骤2)中的模型(1 ‑8)， 如式(1 ‑10)所示： u表示模型输入； 使用如(1 ‑11)所示的匀加速模型离散化式(1 ‑10)， 可得离散化后的状 态空间方程(1 ‑12)： 其中， T表示采样周期； I表示单位矩阵； 设计ut用于解决式(1 ‑13)的优 化问题： 其中Q和R分别为正半定矩阵和正定矩阵； cm表示为一个常数； kmin、 kmax分别表示刚度的 最小值和最大值； 式(1 ‑13)的目标函数是为了在跟踪精度和接触柔顺性之前实现一个权 衡， 约束条件使系统成为一个恒定惯 性、 过阻尼和刚度可变的系统； 优化问题式(1 ‑13)转换 成一个二次规划QP问题， 如式(1 ‑14)所示：权　利　要　求　书 2/5 页 3 CN 115107038 A 3