基于动量控制的人型机器人全身运动自平衡控制方法

摘要

本发明涉及一种基于动量控制的人型机器人全身运动自平衡控制方法，包括以下步骤：1)以机器人的支撑脚为坐标原点，对身体基坐标系引入浮体运动学，建立一个基于全身关节的关节链和运动学模型，从而进行机器人的全身运动控制；2)在浮体运动学的基础上，建立关节速度与机器人动量的关系，通过控制关节速度来控制机器人动量，从而进行动量控制；3)结合动量控制和全身运动控制，在进行末端轨迹规划时保持自平衡。与现有技术相比，本发明引入浮体运动学，充分利用机器人全身关节来完成末端执行器跟踪，有助于扩大执行器的可达到区域，并分析运动对机器人质心产生的影响，通过控制质心运动恒定，产生自平衡的末端运动跟踪。

说明书

技术领域

本发明涉及一种机器人控制方法，尤其是涉及一种基于动量控制的人型机器 人全身运动自平衡控制方法。

背景技术

随着机器人技术的发展，机器人从用于工业开始转入家庭，社会，生活等各个 方面，于是更类似于人类的人型机器人引起了更多的研究，通过观察、训练、学习 一些类人技巧，如行走，抓取物体，甚至摔倒来加强机器人的行为能力，越来越多 的更高层次的行为引入了机器人之中，这就需要机器人能进行复杂的全身性运动， 如蹲下抓取物体等。在日常生活中，机器人和人类都是从行为库中选取最优的行为 动作，通常同一个行为只是规划目标的不同，一般规划目标为末端轨迹，如抓取物 体这个行为，不同的是规划抓取手的末端轨迹，所以在实际应用中，如何在规划末 端轨迹的同时，保持机器人自平衡成为其中的固有约束。

为了使得机器人在完成全身运动的同时保持平衡，以免机器人摔倒损坏机器人 本身或者外部环境，自平衡稳定器需要引入机器人的固有规划中。近些年来，很多 学者和机构考虑研究了一些自平衡的方法，比如通过离线学习、规划机器人的运动 满足一定的要求，如零力矩点(ZMP)，采用快速拓展随机树(RRTs)，动力学过滤 器等，实现了通过学习，离线的稳定器方法。这类方法可以对规划的目标轨迹进行 学习，使得机器人在进行轨迹规划的同时，避免了摔倒。但是目前对于平衡的技巧 研究大多集中在采用学习的方法，这类方法对于人型机器人由于关节多而产生的高 维度配置空间(C-Space)需要花费大量的计算时间，不适用于实时的运动规划。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于动量控 制的人型机器人全身运动自平衡控制方法。

本发明的目的可以通过以下技术方案来实现：

一种基于动量控制的人型机器人全身运动自平衡控制方法，包括以下步骤：

1)以机器人的支撑脚为坐标原点，对身体基坐标系引入浮体运动学，建立一 个基于全身关节的关节链和运动学模型，从而进行机器人的全身运动控制；

2)在浮体运动学的基础上，建立关节速度与机器人动量的关系，通过控制关 节速度来控制机器人动量，从而进行动量控制；

3)结合动量控制和全身运动控制，在进行末端轨迹规划时保持自平衡。

与现有技术相比，本发明引入浮体运动学，充分利用机器人全身关节来完成末 端执行器跟踪，有助于扩大执行器的可达到区域，并分析运动对机器人质心产生的 影响，通过控制质心运动恒定，产生自平衡的末端运动跟踪。

附图说明

图1为本发明的主要控制流程；

图2为机器人浮体坐标系以及关节链的结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

由于机器人运动规划是对机器人的关节配置空间依据末端轨迹进行控制，并不 考虑因此而引起的动量变化，以及对于自身平衡性的影响，因此具有自平衡的路径 规划是必不可收的约束，本发明通过结合动量控制和全身运动控制，达到机器人的 全身运动自平衡控制，具体的控制流程如图1所示：

首先，对于机器人的关节空间到任务空间的映射为非线性方程t＝h(q)，对该方 程进行逆运动计算，该方程可能有无数解，所以运用微分方程解即关节空间的速度 来代替关节空间本身，并且如图2建立支撑脚坐标系和身体基座标系 的映射，建立浮体运动学，身体基座标的速度对执行器末端位置产生影响 其中表示身体基坐标系的速度，表示末端执行器的 速度，表示从身体基座标到末端执行器的关节链，J_E、J_B，E分别是雅可比矩阵， 是关于当前关节状态空间和机器人自身模型的函数，同样求得支撑脚运动对身体基 座标系其中为支撑脚的速度，综合两个公式得到 可以看出建立在浮体运动学上的末端执行器 的空间速度不同于传统的只和也就是身体基座标到末端关节链相关，也和即支撑脚到身体基座标的关节链相关。

然后，机器人的线性动量P(P_x，P_y，P_z)和角动量L(L_x，L_y，L_z)代表了机器人的质心 (CoM)的变化，并和机器人的稳定性，平衡相关，对于一般的关节链有

$P=m\stackrel{·}{r}=\underset{j=1}{\overset{N}{\Sigma }}{m}_j{\stackrel{·}{c}}_j=P\left(\theta \right)\stackrel{·}{\theta }$

$L=\underset{j=1}{\overset{N}{\Sigma }}{c}_j\times P_j+R_j{I}_j{R}_j^T{w}_j=L\left(\theta \right)\stackrel{\text{·}}{\theta }$

可以看出动量是关于关节速度空间的线性方程，而基于浮体运动学的全身运 动，质点的速度和基坐标系相关，

$\left(\begin{array}{c}P\\ L\end{array}\right)=\left(\begin{array}{ccc}m& {\mathrm{mr}}_{b\to c}& P_{\theta }\\ 0& I\left(\theta \right)& L_{\theta }\end{array}\right)\left(\begin{array}{c}{v}_B\\ w_B\\ \stackrel{·}{\theta }\end{array}\right)$

其中m是机器人的质量总和，I是一个3*3矩阵，表示其转动惯量，r_b→c是从身体 基坐标系原点到质心的距离，简化方程可以得到带入基坐 标系速度和支撑坐标系之间映射关系得到关节速度空间对动量的线性雅可比关系。

$M=M_B{J}_{B,F}^{\#}({\stackrel{·}{X}}_F-J_F{\stackrel{·}{\theta }}_F)+M_{\theta }\stackrel{·}{\theta }=M_F{\stackrel{·}{\theta }}_F+{M_{\theta }}_{\mathrm{free}}{\stackrel{·}{\theta }}_{\mathrm{free}}$

其中θ_F是从身体基坐标到支撑脚的关节链，而θ_free是除此之外的剩余全身关节空 间，最后，从上面两节可以得到机器人的末端执行器的运动可以建立在全身运动的 系统下，并且可以分析出由此产生的运动会对动量产生的影响，本发明在利用全身 关节满足末端执行器的运动，求出该运动对于动量的影响。

$M^{\mathrm{ref}}=M_F{\stackrel{·}{\theta }}_F+{M_{\theta }}_E{J}_E^{\#}({\stackrel{·}{X}}_E^{\mathrm{ref}}-J_{F,E}{\stackrel{·}{\theta }}_F)+{M_{\theta }}_{\mathrm{passive}}{\stackrel{·}{\theta }}_{\mathrm{passive}}$

其中θ_passive是不影响末端轨迹运动X_E的关节空间，如当末端执行器为右手时，左臂 关节并不会对其轨迹产生影响。为了在运动的同时得到自平衡，控制动量M^ref恒为 零，也就是使得在满足运动的同时不影响质心运动，产生自平衡的末端轨迹跟踪规 划。

$M^{\mathrm{ref}}-{M_{\theta }}_E{J}_E^{\#}{\stackrel{·}{X}}_E^{\mathrm{ref}}=(M_F-{M_{\theta }}_E{J}_E^{\#}{J}_{F,E}){\stackrel{·}{\theta }}_F+{M_{\theta }}_{\mathrm{passive}}{\stackrel{·}{\theta }}_{\mathrm{passive}}=M_F^{\prime }{\stackrel{·}{\theta }}_F+{M_{\theta }}_{\mathrm{passove}}{\stackrel{·}{\theta }}_{\mathrm{passive}}$

在本发明中，简化为零，可以简化计算，同时用于可以完成其他任务，于是 得到

支撑腿的关节速度配置空间为并由此同时再带回 其对末端执行器的速度影响得到末端执行端关节速度配置空间 ${\stackrel{·}{\theta }}_E=J_E^{\#}({\stackrel{·}{X}}_E-J_{F,E}{\stackrel{·}{\theta }}_F).$

本发明通过引入浮体运动学，充分利用机器人全身关节来完成末端执行器跟 踪，有助于扩大执行器的可达到区域，并分析运动对机器人质心产生的影响，通过 控制质心运动恒定，产生自平衡的末端运动跟踪。