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

Abstract

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

Description

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

技术领域

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

背景技术

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

为了使得机器人在完成全身运动的同时保持平衡，以免机器人摔倒损坏机器人本身或者外部环境，自平衡稳定器需要引入机器人的固有规划中。近些年来，很多学者和机构考虑研究了一些自平衡的方法，比如通过离线学习、规划机器人的运动满足一定的要求，如零力矩点(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}{\mathrm{\Σ}}}{m}_j{\stackrel{\·}{c}}_j=P\left(\mathrm{\θ}\right)\stackrel{\·}{\mathrm{\θ}}$

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

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

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

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

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

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

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

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

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

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

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

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

Claims (1)

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

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

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

3)结合动量控制和全身运动控制，在进行末端轨迹规划时保持自平衡，末端执行端关节速度配置空间表示为其中，表示从身体基座标到末端执行器的关节链，J_E、J_F,E均为雅可比矩阵，为J_E的逆矩阵，表示末端执行器的速度，表示从支撑脚到身体基座标的关节链。