[发明专利]一种仿人机器人高动态运动质心柔顺控制方法

权利要求书

1.一种仿人机器人高动态运动质心柔顺控制方法，其特征在于：

获取机器人浮动基位姿，计算已接触末端的实际位姿；

根据已接触机器人末端的实际位姿及受力，计算机器人所受实际接触合力/力矩；

基于线性二次型调节器设计反馈控制器，计算质心动量二阶导的补偿量；

对质心动量二阶导的补偿量积分，得到质心位置及动量的补偿量，利用所述质心位置及动量的补偿量对期望质心位置及动量轨迹进行更新；

求解得到的关节角度发送给机器人进行执行；

所述质心动量二阶导的补偿量为：

其中：为质心动量二阶导的补偿量，μ为控制量，x为状态量，K为反馈系数矩阵，由系数矩阵及权重矩阵Q、R确定；

所述系数矩阵满足：

其中：接触合力/力矩误差量Δλ＝[Δf Δτ]^T，接触合力/力矩变化量的误差量机器人动量的补偿量Δh＝[Δh_l Δh_a]^T，机器人动量变化量的补偿量机器人动量二阶导的补偿量接触合力的误差量接触合力变化量的误差量接触合力矩的误差量接触合力矩变化量的误差量和分别为质心线动量、角动量的二阶导的补偿量，和分别为质心线动量、角动量的一阶导的补偿量，Δh_l和Δh_a分别为质心线动量、角动量的补偿量，和分别为机器人期望接触合力、合力矩的测量值，和分别为机器人接触合力、合力矩的期望值，为接触合力变化量的测量值，为接触合力变化量的期望值，为接触合力矩变化量的测量值，为接触合力矩变化量的期望值，T为滞后时间，I为单位矩阵；

所述权重矩阵满足：

其中：J为目标函数。

2.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，所述机器人浮动基位姿是通过读取IMU位置及姿态测量值获取的。

3.根据权利要求2所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，所述已接触末端的实际位姿为：

其中，为末端i在世界坐标系下的实际姿态旋转矩阵，为末端i在世界坐标系下的实际位置，为机器人在实际坐标系下的实际位姿，为机器人实际关节角度，G_{K_i}(·)为一种由关节角度求任意连杆位姿的正运动学算法，C为已接触的末端序号集合。

4.根据权利要求3所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，所述已接触的末端序号集合C为：

其中，为第i个末端力传感器的竖直方向力测量值，f_sat为接触阈值。

5.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，所述机器人所受实际接触合力/力矩为：

其中，为末端i在世界坐标系下的实际位置，为世界坐标系下机器人实际质心位置，^wf_i^m和分别为末端i所受力、力矩在世界坐标系下的表示。

6.根据权利要求1所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，利用所述质心位置及动量的补偿量对期望质心位置及动量轨迹进行更新，是将质心位置及动量的补偿量叠加到期望质心位置及动量上，对期望轨迹进行更新。

7.根据权利要求6所述的仿人机器人高动态运动质心柔顺控制方法，其特征在于，更新后的轨迹为：

h^d*＝Δh+h^d

其中，h^d和分别为原始期望质心动量、质心位置，h^d*和分别为更新后的期望质心动量、质心位置。