(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210533708.8 (22)申请日 2022.05.17 (71)申请人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 黄强　黄则临　余张国　陈学超　 李庆庆　董宸呈　孟祥　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 张明明 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种仿人机器人高动态运动质心柔顺控制 方法 (57)摘要 本发明提供了一种仿人机器人高动态运动 质心柔顺控制方法， 基于线性二次型调节器设计 反馈控制器， 计算质心动量二阶导的补偿量； 对 质心动量二阶导的补偿量积分， 得到质心位置及 动量的补偿量， 将质心位置及动量的补偿量叠加 到期望质心位置及动量上， 对期望轨迹进行更 新， 求解得到的关节轨迹发送给机器人进行执 行。 本发明计算质 心动量二阶导的补偿量， 得到 柔顺控制所需的反馈系数， 有效减少对反馈系数 进行调节的时间； 本发明同时考虑质心线动量和 角动量的变化量， 实现对质心在移动方向及转动 方向均进行柔顺控制， 提高机器人高动态运动的 稳定性。 权利要求书3页 说明书7页 附图2页 CN 114750162 A 2022.07.15 CN 114750162 A 1.一种仿人机器人高动态运动质心柔 顺控制方法， 其特 征在于： 获取机器人浮动基位姿， 计算已接触末端的实际位姿； 根据已接触机器人末端的实际位姿及受力， 计算机器人 所受实际接触合力/力矩； 基于线性 二次型调节器设计反馈控制器， 计算质心动量 二阶导的补偿量； 对质心动量二阶导的补偿量积分， 得到质心位置及动量的补偿量， 利用所述质心位置 及动量的补偿量对期望质心位置及动量轨 迹进行更新； 求解得到的关节角度发送给机器人进行 执行； 所述质心动量 二阶导的补偿量 为： 其中： 为质心动量二阶导的补偿量， μ为控制量， x为状态量， K为反馈系数矩阵， 由系 数矩阵 及权重矩阵Q、 R确定； 所述系数矩阵满足： 其中： 接触合力/力矩误差量Δλ＝[Δf  Δτ]T， 接触合力/力矩变化量的误差量 机器人动量的补偿量Δh＝[Δhl Δha]T， 机器人动量变化量的补偿量 机器人动量二阶导的补偿量 接触合力的误差量 接触合力变化量的误差量 接触合力矩的误差量 接触合力矩变化量的误差量 和 分别为质心线动量、 角动量的二阶导的 补偿量， 和 分别为质心线动量、 角动量的一阶导的补偿量， Δhl和Δha分别为质心线 动量、 角动量的补偿量， 和 分别为机器人期望接触合力、 合力矩的测量值， 和 分 别为机器人接触合力、 合力矩的期望值， 为接触合力变化量的测量值， 为接触合力变 化量的期 望值， 为接触合力矩变化量的测量值， 为接触合力矩变化量的期 望值， T为滞 后时间， I 为单位矩阵； 所述权重矩阵满足： 其中： J为目标函数。权　利　要　求　书 1/3 页 2 CN 114750162 A 22.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 所述 机器人浮动基位姿是通过读取IMU位置及姿态测量 值获取的。 3.根据权利要求2所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 所述 已接触末端的实际位姿为： 其中， 为末端i在世界坐标系下的实际姿态旋转矩阵， 为末端i在世界坐标系下的 实际位置， 为机器人在实际坐标系下的实际位姿， 为机器人实际关节角 度， GK_i(·) 为一种由关节角度求任意连 杆位姿的正 运动学算法， C为已接触的末端序号 集合。 4.根据权利要求3所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 所述 已接触的末端序号 集合C为： 其中， 为第i个末端力传感器的竖直方向力测量 值， fsat为接触阈值。 5.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 所述 机器人所受实际接触合力/力矩为： 其中， 为末端i在世界坐标系下的实际位置， 为世界坐标系下机器人实际质心位 置，wfim和 分别为末端i所受力、 力矩在世界坐标系下的表示。 6.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 利用 所述质心位置及动量的补偿量对期 望质心位置及动量轨迹进 行更新， 是将质心 位置及动量 的补偿量叠加到期望质心位置及动量上， 对期望 轨迹进行更新。 7.根据权利要求6所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 更新 后的轨迹为： hd*＝Δh+hd 其中， hd和 分别为原始期望质心动量、 质心位置， hd*和 分别为更新后的期望质心 动量、 质心位置 。 8.根据权利要求7所述的仿人机器人高动态运动质心柔顺控制方法， 其特征在于， 所述 关节轨迹角度通过逆运动学求 解关节角度得到： 其中： 是期望下发的关节角加速度及关节角度， FIK(·)为一种基于二次规划 的仿人机器人逆运动学求 解方法， 为期望的末端位置及姿态。权　利　要　求　书 2/3 页 3 CN 114750162 A 3