一种仿人机器人高动态运动质心柔顺控制方法

1.本发明属于仿人机器人技术领域，具体涉及一种仿人机器人高动态运动质心柔顺控制方法。


背景技术：

2.仿人机器人在受到外界扰动时，需要通过调节末端接触力来维持自身稳定，由于接触力与机器人的整体质心及动量密切相关，因此在对机器人进行稳定控制时往往需要对机器人末端及质心同时进行柔顺控制，从而使机器人能承受较大的持续外界扰动。目前，对于仿人机器人质心柔顺控制大多集中于研究对质心移动自由度相关(即质心位置及质心线动量)运动的柔顺控制，未涉及对质心转动自由度相关(即绕质心的角动量)运动的柔顺控制。虽然，机器人在进行低速或静态运动(如慢速行走、站立等)时，机器人绕其质心的角动量可忽略不计，但当机器人进行高动态运动(如快速奔跑、跳跃等)时其绕质心的角动量不可忽视且对运动的完成至关重要。因此，在机器人进行高动态运动时需要同时对机器人质心移动及转动自由度相关运动进行柔顺控制，从而实现使机器人实现稳定的高动态运动。


技术实现要素：

3.针对现有技术中存在不足，本发明提供了一种仿人机器人高动态运动质心柔顺控制方法，实现对机器人质心状态在移动及转动方向同时进行柔顺控制。
4.本发明是通过以下技术手段实现上述技术目的的。
5.一种仿人机器人高动态运动质心柔顺控制方法，具体为：
6.获取机器人浮动基位姿，计算已接触末端的实际位姿；
7.根据已接触机器人末端的实际位姿及受力，计算机器人所受实际接触合力/力矩；
8.基于线性二次型调节器设计反馈控制器，计算质心动量二阶导的补偿量；
9.对质心动量二阶导的补偿量积分，得到质心位置及动量的补偿量，利用所述质心位置及动量的补偿量对期望质心位置及动量轨迹进行更新；
10.求解得到的关节轨迹发送给机器人进行执行；
11.所述质心动量二阶导的补偿量为：
[0012][0013]
其中：为质心动量二阶导的补偿量，μ为控制量，x为状态量，k为反馈系数矩阵，由系数矩阵及权重矩阵q、r确定；
[0014]
所述系数矩阵满足：
[0015][0016]
其中：接触合力/力矩误差量δλ＝[δf δτ]
t
，接触合力/力矩变化量的误差量机器人动量的补偿量δh＝[δh
l δha]
t
，机器人动量变化量的补偿量机器人动量二阶导的补偿量接触合力的误差量接触合力变化量的误差量接触合力矩的误差量接触合力矩变化量的误差量接触合力矩变化量的误差量和分别为质心线动量、角动量的二阶导的补偿量，和分别为质心线动量、角动量的一阶导的补偿量，δh
l
和δha分别为质心线动量、角动量的补偿量，和分别为机器人期望接触合力、合力矩的测量值，和分别为机器人接触合力、合力矩的期望值，为接触合力变化量的测量值，为接触合力变化量的期望值，为接触合力矩变化量的测量值，为接触合力矩变化量的期望值，t为滞后时间，i为单位矩阵；
[0017]
所述权重矩阵满足：
[0018][0019]
其中：j为目标函数。
[0020]
进一步的技术方案，所述机器人浮动基位姿是通过读取imu位置及姿态测量值获取的。
[0021]
更进一步的技术方案，所述已接触末端的实际位姿为：
[0022][0023]
其中，为末端i在世界坐标系下的实际姿态旋转矩阵，为末端i在世界坐标系下的实际位置，为机器人在实际坐标系下的实际位姿，为机器人实际关节角度，f
k_i
(
·
)为一种由关节角度求任意连杆位姿的正运动学算法，c为已接触的末端序号集合。
[0024]
更进一步的技术方案，所述已接触的末端序号集合c为：
[0025][0026]
其中，为第i个末端力传感器的竖直方向力测量值，f
sat
为接触阈值。
[0027]
进一步的技术方案，所述机器人所受实际接触合力/力矩为：
[0028]
[0029]
其中，为末端i在世界坐标系下的实际位置，为世界坐标系下机器人实际质心位置，
wfim
和分别为末端i所受力、力矩在世界坐标系下的表示。
[0030]
进一步的技术方案，利用所述质心位置及动量的补偿量对期望质心位置及动量轨迹进行更新，是将质心位置及动量的补偿量叠加到期望质心位置及动量上，对期望轨迹进行更新。
[0031]
更进一步的技术方案，更新后的轨迹为：
[0032]hd*
＝δh+hd[0033][0034]
其中，hd和分别为原始期望质心动量、质心位置，h
d*
和分别为更新后的期望质心动量、质心位置。
[0035]
更进一步的技术方案，所述关节轨迹角度通过逆运动学求解关节角度得到：
[0036][0037][0038]
其中：是期望下发的关节角加速度及关节角度，f
ik
(
·
)为一种基于二次规划的仿人机器人逆运动学求解方法，为期望的末端位置及姿态。
[0039]
进一步的技术方案，所述机器人期望接触合力、合力矩满足：
[0040][0041]
其中：和分别为期望质心线动量变化量、角动量变化量，m为机器人总质量，g为重力加速度向量。
[0042]
本发明的有益效果为：
[0043]
(1)本发明构建了一种实际质心状态与测量接触力间关系的线性模型，并基于线性二次型调节器计算柔顺控制所需的反馈系数，有效减少对反馈系数进行调节的时间，且本发明提出的反馈控制器的实时计算速度远低于二次优化的实时计算速度；
[0044]
(2)本发明计算期望接触合力/力矩同时考虑质心线动量变化量、角动量变化量，实现对质心在移动方向及转动方向均进行柔顺控制，在仿人机器人在进行高动态运动(机器人角动量不可忽略)时对质心状态在移动及转动方向上的实现柔顺控制，从而提高机器人高动态运动的稳定性。
附图说明
[0045]
图1为本发明所述仿人机器人高动态运动质心柔顺控制流程图；
[0046]
图2为本发明所述仿人机器人质心受力示意图；
[0047]
图3为本发明所述末端局部坐标系与世界坐标系之间的关系图。
具体实施方式
[0048]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0049]
如图1所示，本发明一种仿人机器人高动态运动质心柔顺控制方法，包括如下步骤：
[0050]
步骤(1)，根据当前机器人的期望质心动量计算期望接触合力/力矩；
[0051]
本发明认为期望质心位置及动量轨迹已知，则机器人期望接触合力/力矩可由仿人机器人质心动力学(如图2)得到：
[0052][0053]
其中，和分别为机器人接触合力、合力矩的期望值；和分别为期望质心线动量变化量、角动量变化量，且由期望动量轨迹微分得到；m为机器人总质量，重力加速度向量g＝[0 0
ꢀ‑
g]
t
，g为重力加速度；图2中，f为接触合力，τ为接触合力矩。
[0054]
步骤(2)，读取机器人所有末端力传感器检测值，根据检测值判断机器人末端是否与环境接触，并获取已接触的末端序号；
[0055]
由于力传感器测量存在一定误差，且一般末端所受竖直方向力最大，因此，本发明以末端力传感器所受竖直方向力大小作为判断是否与环境接触标准，判断形式如下：
[0056][0057]
其中，为第i个末端力传感器的竖直方向力测量值；f
sat
为接触阈值，可根据力传感器实际应用情况而定；
[0058]
若满足式(2)，则认为第i个末端与环境产生接触，若不满足式(2)，则认为第i个末端未与环境产生接触；令满足接触的机器人末端序号集合c为：
[0059][0060]
步骤(3)，读取imu位置及姿态测量值作为机器人浮动基位姿，计算已接触末端的实际位姿；
[0061]
本发明认为结合imu测量值及状态估计算法(如扩展卡尔曼滤波等)可获取仿人机器人浮动基在世界坐标系的位置和姿态，则与环境存在接触的末端的实际位姿可由仿人机器人正运动学算法求解得到：
[0062][0063]
其中，为末端i在世界坐标系下的实际姿态旋转矩阵，为末端i在世界坐标系下的实际位置，为机器人在实际坐标系下的实际位姿，为机器人实际关节角度，f
k_i
(
·
)为一种由关节角度求任意连杆位姿的正运动学算法(梶田秀司.仿人机器人[m].管贻生.北京：清华大学出版社，2007：50-54)，可用于计算末端i位姿。
[0064]
步骤(4)，根据已接触末端的实际位姿及受力计算机器人所受实际接触合力/力矩；
[0065]
首先末端力传感器测量值是在末端局部坐标系下测量的，因此需要将测量值转到世界坐标系下，末端力传感器局部坐标与世界坐标系的关系如图3，则测量值转换如下式：
[0066][0067]
其中，
wfim
和分别为末端i所受力、力矩在世界坐标系下的表示，f
im
和分别为末端i对应的力传感器测得的力、力矩；图3中，r为局部坐标系相对世界坐标系的旋转矩阵，p为局部坐标系原点在世界坐标系下的位置；
[0068]
然后，将所有已接触末端所受力、力矩平移至质心处进行求和，即可得到实际接触合力/力矩：
[0069][0070]
其中，和分别为机器人期望接触合力、合力矩的测量值，为世界坐标系下机器人实际质心位置，可由成熟的商业运动学工具求得，[
·
]
×
表示将方括号内的向量转变为对角矩阵，i为单位矩阵。
[0071]
步骤(5)，计算机器人当前接触合力误差，基于线性二次型调节器设计反馈控制器，计算质心动量二阶导的补偿量；
[0072]
由于，实际机器人传感器测量值通常存在滞后，因此则有：
[0073][0074][0075]
其中，为当前实际接触合力的真值，为实际接触合力真值所对应的期望接触合力，为一阶惯性环节，t为滞后时间，由实际机器人具体运行情况确定；对当前实际接触合力的真值及其所对应的期望值根据质心动力学有：
[0076][0077][0078]
其中，为机器人质心线动量变化量，为机器人绕质心角动量变化量的真值，为机器人质心线动量变化量对应的期望值，为机器人绕质心角动量变化量的真值对应的期望值；
[0079]
联立(7)和(9)，(8)和(10)可得：
[0080][0081][0082]
将(11)与(12)作差并整理，可得实际质心状态与测量接触力间关系的线性模型：
[0083][0084]
其中，接触合力/力矩误差量δλ＝[δf δτ]
t
，接触合力/力矩变化量的误差量机器人动量的补偿量δh＝[δh
l δha]
t
，机器人动量变化量的补偿量机器人动量二阶导的补偿量接触合力的误差量接触合力变化量的误差量接触合力矩的误差量接触合力矩变化量的误差量接触合力矩变化量的误差量为质心线动量、角动量的二阶导的补偿量，为质心线动量、角动量的一阶导的补偿量，δh
l
、δha为质心线动量、角动量的补偿量，为接触合力变化量的测量值，为接触合力变化量的期望值，为接触合力矩变化量的测量值，为接触合力矩变化量的期望值；
[0085]
取如下目标函数j：
[0086][0087]
其中，状态量控制量q、r为权重矩阵；
[0088]
可基于线性二次型调节器设计反馈控制器，则质心动量二阶导的补偿量为：
[0089][0090]
其中，k为反馈系数矩阵，由公式(13)的系数矩阵(和)及公式(14)的权重矩阵确定。
[0091]
步骤(6)，对上述补偿量积分得到质心位置及动量的补偿量，将质心位置及动量的补偿量叠加到期望质心位置及动量上，对期望轨迹进行更新；
[0092]
通过积分求取质心位置及动量的补偿量：
[0093][0094]
其中，δh为质心动量的补偿量，δx
cog
为质心位置的补偿量；
[0095]
将(16)叠加在期望质心位置及动量轨迹，即得到更新后的轨迹：
[0096][0097]
其中，hd和分别为原始期望质心动量、质心位置，h
d*
和分别为更新后的期望质心动量、质心位置。
[0098]
步骤(7)，根据更新后的期望质心相关轨迹求解逆运动学，并将得到的关节轨迹发送给机器人，机器人执行上述关节轨迹；
[0099]
逆运动学求解关节角度：
[0100][0101]
其中，是期望下发的关节角加速度及关节角度，f
ik
(
·
)为一种基于二次规划的仿人机器人逆运动学求解方法(k.bouyarmane,et al.quadratic programming for multirobot and task-space force control[j].ieee transactions on robotics.2019,35(1):64-77.)，为期望的末端位置及姿态，由规划算法给出，本发明认为已知。
[0102]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。