一种仿人机器人摆动腿迈步的能效优化控制方法

摘要

本发明提供了一种仿人机器人摆动腿迈步的能效优化控制方法，其基于给定的初始、末位时刻的状态和单脚支撑起时间获得对应于评价函数的运动轨迹；通过建立仿人机器人摆动腿的模型的动力学方程，建立非线性状态方程，建立评价函数，运用遗传算法获得对应于评价函数的运动轨迹；通过设置所述评价函数中的权重系数，获得具有不同运动效果的运动轨迹。通过自主选择使评价函数最小化，获得最优运动轨迹，实现对仿人机器人行走时摆动腿的运动优化，减少能耗，提高仿人机器人的续航能力。

说明书

技术领域

本发明涉及一种仿人机器人，具体涉及一种仿人机器人摆动腿迈步的能效优化控制方法。

背景技术

仿人机器人是与人类最接近的一种机器人，和其他构形的机器人相比，仿人机器人在外形上更加友好，同时能方便地使用为人类发明设计的各种工具，更容易适应人类生活环境。随着科技的发展，人们对智能机器人的需求日益增长。仿人机器人具有广阔的发展前景。

仿人机器人已经能够实现稳定的行走。然而，为了满足仿人机器人实用化需求，必须减小仿人机器人的能耗。一种有效可行的方法是通过调整仿人机器人的行走步态来减小行走过程中摆动腿的力矩和速度，达到减小能耗的目的。

仿人机器人作为一个多自由度、非线性、强耦合的多刚体系统，其行走步态是腿部所有关节之间的耦合作用和身体结构比例关系的集中体现，步态优化需要综合考虑运动学参数、动力学参数、运动约束和初始条件。有必要寻求一种耗能小、拟人化程度高的运动轨迹以提高仿人机器人的实用性。

在公开号为CN101847009A的中国专利中公开了一种双足机器人步态能效优化方法，该方法不仅需要建立一整套的能效评估模型，而且求解过程繁琐，收敛速度较慢。

在论文《Sagittalgaitofabipedrobotduringthesinglesupportphase.Part2:optimalmotion》中提出了一种仿人机器人摆动腿运动的优化方法，但该方法只考虑了运动过程中关节力矩的最小化，并没有考虑到摆动腿在运动过程中的能量消耗。

本专利针对上述现有技术的缺陷，提出一种高效便捷的能效评估方式，充分考虑了摆动腿运动过程中的力矩和能量消耗，并将其应用到仿人机器人摆动腿的步态规划中。该方法不仅能有效减小仿人机器人行走过程中的能耗，而且效率高、收敛速度快，并具有较好的精度。

发明内容

本发明提供了一种提出一种优化仿人机器人摆动腿运动轨迹的方案，通过优化算法得到满足评价函数的摆动腿运动轨迹。通过自主选择改变评价函数的权重系数可实现不同的优化目标。

本发明的仿人机器人摆动腿迈步的能效优化控制方法，基于给定的初始、末位时刻的状态和单脚支撑起时间获得对应于评价函数的运动轨迹；其包括如下步骤：

(1)建立仿人机器人摆动腿的模型的动力学方程；

(2)依据所述动力学方程建立非线性状态方程；

(3)建立评价函数；

(4)运用遗传算法获得对应于评价函数的运动轨迹；

其特征在于：通过设置所述步骤(3)中的评价函数中的权重系数，获得具有不同运动效果的运动轨迹。

根据上述的能效优化控制方法，其中，所述动力学方程为：

$M\left(\theta \right)\stackrel{··}{\theta }+C(\theta ,\stackrel{·}{\theta })=u$

其中M(θ)∈R^6×6是关节空间的惯性矩阵，是科氏力、离心力与重力的合力矢量，

$\theta =\left(\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\theta }_3\\ {\theta }_4\\ {\theta }_5\\ {\theta }_6\end{array}\right),u=\left(\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\\ u_5\\ u_6\end{array}\right),$

u₁～u₆为摆动腿关节1～6的驱动力矩，M(θ)是关节角度的函数，只与关节角度相关；是关节角度、角速度的函数，坐标系建立法则如下:

平行于旋转轴的方向为杆件坐标系的Z轴。

根据Z轴方向确定杆件坐标系的X轴的方向：若Z_i-1轴的方向不平行于Z_i轴的方向，则X轴的方向将垂直于Z_i-1轴和Z_i轴；若Z_i-1轴的方向平行于Z_i轴的方向，则X_i-1轴的方向为Z_i-1轴与Z_i轴的公垂线方向。

由右手定则决定Y轴正方向。

各角度的定义如说明书附图2所示：

θ₁：表示髋关节绕Z轴正方向所旋转的角度；

θ₂：表示髋关节绕Y轴正方向所旋转的角度；

θ₃：表示髋关节绕X轴正方向所旋转的角度；

θ₄：表示膝关节绕X轴正方向所旋转的角度；

θ₅：表示踝关节绕X轴正方向所旋转的角度；

θ₆：表示踝关节绕Y轴正方向所旋转的角度。

根据上述的任一能效优化控制方法，其中，所述非线性状态方程为：

$\stackrel{·}{x}=f(x,u)=\left(\begin{array}{c}\stackrel{·}{\theta }\\ M^{-1}(u-C)\end{array}\right)$

其中u为该系统的控制量，$x=\left(\begin{array}{c}\theta \\ \stackrel{·}{\theta }\end{array}\right),\stackrel{·}{x}=\left(\begin{array}{c}\stackrel{·}{\theta }\\ \stackrel{··}{\theta }\end{array}\right);$

所述评价函数为：

$J=x_e^T{\mathrm{Sx}}_e+\underset{0}{\overset{t}{\int }}(x^T\mathrm{Qx}+u^T\mathrm{Ru})\mathrm{dt}$

其中，x_e是末位时刻的状态，S、Q、R为12*12的权重矩阵；

τ∈[t₀，t_f]，t₀为开始时间，t_f为结束时间。

根据上述的任一能效优化控制方法，其中，所述步骤(3)中还包括如下步骤：

把所述动力学方程在高斯点上进行离散，将时间区间τ∈[t₀，t_f]转换到τ∈[-1，1]，这个转化可以通过下式完成：

$t=\frac{(t_f-t_0)\tau +(t_f+t_0)}{2}$

转换后的τ取代t成为独立变量，τ＝-1时对应t₀，τ＝1时对应t_f；t₀为开始时间，t_f为结束时间；

用N个高斯点τ₁，τ₂…，τ_N和初始端点t₀＝-1上的离散状态构造Lagrange插值多项式去近似状态的时间历程：

$x\left(\tau \right)\approx X\left(\tau \right)=\underset{i=0}{\overset{N}{\Sigma }}X\left({\tau }_i\right)L_i\left(\tau \right)$

式中：x(τ)为真实的状态时间历程，X(τ)为由Lagrange插值多项式近似得到的状态时间历程；L_i(τ)为Lagrange插值基函数，i＝0,1,...，N

$L_i\left(\tau \right)={\Pi }_{j=0,j\ne i}^N\frac{\tau -{\tau }_i}{{\tau }_i-{\tau }_j}$

对时间求导，得

$\stackrel{·}{x}\left(\tau \right)\approx \stackrel{·}{X}\left(\tau \right)=\underset{i=0}{\overset{N}{\Sigma }}X\left({\tau }_i\right){\stackrel{·}{L}}_i\left(\tau \right)$

由此对仿人机器人的脚部末位时刻的位置进行约束的微分方程动态转化为一系列代数约束：

$\underset{i=0}{\overset{N}{\Sigma }}{\stackrel{·}{L}}_i{X}_i-\frac{t_f-t_0}{2}f(X_k,U_k,{\tau }_k;t_0,t_f)=0$

式中：X_k＝X(τ_k),U_k＝U(τ_k),k＝1,2，…N；

性能指标、边界条件和不等式约束分别转化为：

$J=\phi (X_0,t_0,X_f,t_f)+\frac{t_f-t_0}{2}\underset{k=1}{\overset{N}{\Sigma }}{w}_{k}g(X_k,U_k,{\tau }_k;t_0,t_f)$

Φ(X₀，t₀，X_f，t_f)＝0

C(X_k，U_k，τ_k；t₀，t_f)≤0。

式中W_k为高斯积分权重。

根据上述的任一能效优化控制方法，其中，所述遗传算法用于优化求解评价函数的非线性规划问题，所述步骤(4)又包括如下步骤：

(4.1)令j＝0，随机产生N个初始个体构成初始种群P(0)；

(4.2)评价P(j)中各个体的适应值；

(4.3)判断是否满足算法收敛准则，若满足，则输出搜索结果，否则执行下一步骤；

(4.4)令m＝0；

(4.5)根据适应值大小以一定方式执行复制操作从P(k)中选出两个个体作为父代；

(4.6)若交叉概率Ρ_c>＄∈[0,1]，则对选中个体执行交叉操作生成两个临时子代，否则将选中的父代个体直接作为临时子代；

(4.7)按变异概率Ρ_m对临时个体执行变异操作产生两个新个体放入P(j+1)同时令m＝m+2；

(4.8)若m<N，则返回第五步；否则令j＝j+l，并返回步骤(4.2)。

根据上述的任一能效优化控制方法，其中，仿人机器人的脚尖与脚跟z轴方向的位置分别为：

Z_t＝(c2*c1-s2*s1)*L2+c1*L1+

((s2*c1+c2*s1)*c3+(c2*c1-s2*s1)*s3)*f_foot

+(-(s2*c1+c2*s1)*s3+(c2*c1-s2*s1)*c3)*height

Z_h＝(c2*c1-s2*s1)*L2+c1*L1

+((s2*c1+c2*s1)*c3+(c2*c1-s2*s1)*s3)*r_foot

+(-(s2*c1+c2*s1)*s3+(c2*c1-s2*s1)*c3)*height

其中c1＝cosθ3、c2＝cosθ4、c3＝cosθ5，s1＝sinθ3、s2＝sinθ4、s3＝sinθ5，height为脚部高度，f_foot为脚尖与前脚掌长(脚尖到脚心)，r_foot为后脚跟长(脚后跟到脚心),L1为大腿长度，L2为小腿长度,Z_t为脚尖与z轴方向的位置，Z_h为脚跟与z轴方向的位置；由此可以设定脚跟与脚尖在z轴方向的运动范围。

通过以上的一整套算法可以获得一个使评价函数最优的运动轨迹，该运动轨迹即是所期望获得的减小仿人机器人能耗的摆动腿运动轨迹。通过设置评价函数中的权重系数S、Q、R可以获得不同的运动效果。

附图说明

图1是本发明的机器人行走轨迹的控制方法流程图。

图2是本发明中使用的机器人的腿部模型示意图。

具体实施方式

本专利采用的优化方法主要是基于给定的初始、末位时刻的状态和单脚支撑起时间的情况下，通过自主选择使评价函数最小化的步态来实现。具体方法如下：

首先建立仿人机器人摆动腿模型的动力学方程

$M\left(\theta \right)\stackrel{··}{\theta }+C(\theta ,\stackrel{·}{\theta })=u$

其中M(θ)∈R^6×6是关节空间的惯性矩阵，是科氏力、离心力与重力的合力矢量，

$\theta =\left(\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\theta }_3\\ {\theta }_4\\ {\theta }_5\\ {\theta }_6\end{array}\right),u=\left(\begin{array}{c}{u}_1\\ u_2\\ u_3\\ u_4\\ u_5\\ u_6\end{array}\right),$

u₁～u₆为摆动腿关节1～6的驱动力矩，M(θ)是关节角度的函数，只与关节角度相关；是关节角度、角速度的函数，坐标系建立法则如下:

平行于旋转轴的方向为杆件坐标系的Z轴。

根据Z轴方向确定杆件坐标系的X轴的方向：若Z_i-1轴的方向不平行于Z_i轴的方向，则X轴的方向将垂直于Z_i-1轴和Z_i轴；若Z_i-1轴的方向平行于Z_i轴的方向，则X_i-1轴的方向为Z_i-1轴与Z_i轴的公垂线方向。

由右手定则决定Y轴正方向。

各角度的定义如下：

θ₁：表示髋关节绕Z轴正方向所旋转的角度；

θ₂：表示髋关节绕Y轴正方向所旋转的角度；

θ₃：表示髋关节绕X轴正方向所旋转的角度；

θ₄：表示膝关节绕X轴正方向所旋转的角度；

θ₅：表示踝关节绕X轴正方向所旋转的角度；

θ₆：表示踝关节绕Y轴正方向所旋转的角度。

再由摆动腿的动力学方程建立非线性状态方程：

$\stackrel{·}{x}=f(x,u)=\left(\begin{array}{c}\stackrel{·}{\theta }\\ M^{-1}(u-C)\end{array}\right)$

其中u为该系统的控制量，$x=\left(\begin{array}{c}\theta \\ \stackrel{·}{\theta }\end{array}\right),\stackrel{·}{x}=\left(\begin{array}{c}\stackrel{·}{\theta }\\ \stackrel{··}{\theta }\end{array}\right);$

所述评价函数为：

$J=x_e^T{\mathrm{Sx}}_e+\underset{0}{\overset{t}{\int }}(x^T\mathrm{Qx}+u^T\mathrm{Ru})\mathrm{dt}$

其中，x_e是末位时刻的状态，S、Q、R为12*12的权重矩阵；

τ∈[t₀，t_f]，t₀为开始时间，t_f为结束时间。

根据机器人的模型对脚部末端位置进行约束，使得机器人的脚始终位于地面以上。由于脚平面所在平面上的两点即可确定脚平面所在，因而只需给定脚尖和脚跟两个点的位置约束。

根据正运动学，可以求出脚尖与脚跟z轴方向的位置分别为：

Z_t＝(c2*c1-s2*s1)*L2+c1*L1+

((s2*c1+c2*s1)*c3+(c2*c1-s2*s1)*s3)*f_foot

+(-(s2*c1+c2*s1)*s3+(c2*c1-s2*s1)*c3)*height

Z_h＝(c2*c1-s2*s1)*L2+c1*L1

+((s2*c1+c2*s1)*c3+(c2*c1-s2*s1)*s3)*r_foot

+(-(s2*c1+c2*s1)*s3+(c2*c1-s2*s1)*c3)*height

其中c1＝cosθ3、c2＝cosθ4、c3＝cosθ5，s1＝sinθ3、s2＝sinθ4、s3＝sinθ5，height为脚部高度，f_foot为脚尖与前脚掌长(脚尖到脚心)，r_foot为后脚跟长(脚后跟到脚心),Z_t为脚尖与z轴方向的位置，Z_h为脚跟与z轴方向的位置；由此可以设定脚跟与脚尖在z轴方向的运动范围。

把所述动力学方程在高斯点上进行离散，将时间区间τ∈[t₀，t_f]转换到τ∈[-1，1]，这个转化可以通过下式完成：

$t=\frac{(t_f-t_0)\tau +(t_f+t_0)}{2}$

转换后的τ取代t成为独立变量，τ＝-1时对应t₀，τ＝1时对应t_f；t₀为开始时间，t_f为结束时间；

用N个高斯点τ₁，τ₂…，τ_N和初始端点t₀＝-1上的离散状态构造Lagrange插值多项式去近似状态的时间历程：

$x\left(\tau \right)\approx X\left(\tau \right)=\underset{i=0}{\overset{N}{\Sigma }}X\left({\tau }_i\right)L_i\left(\tau \right)$

式中：x(τ)为真实的状态时间历程，X(τ)为由Lagrange插值多项式近似得到的状态时间历程；L_i(τ)为Lagrange插值基函数，i＝0,1,...，N

$L_i\left(\tau \right)={\Pi }_{j=0,j\ne i}^N\frac{\tau -{\tau }_i}{{\tau }_i-{\tau }_j}$

对时间求导，得

$\stackrel{·}{x}\left(\tau \right)\approx \stackrel{·}{X}\left(\tau \right)=\underset{i=0}{\overset{N}{\Sigma }}X\left({\tau }_i\right){\stackrel{·}{L}}_i\left(\tau \right)$

由此对仿人机器人的脚部末位时刻的位置进行约束的微分方程动态转化为一系列代数约束：

$\underset{i=0}{\overset{N}{\Sigma }}{\stackrel{·}{L}}_i{X}_i-\frac{t_f-t_0}{2}f(X_k,U_k,{\tau }_k;t_0,t_f)=0$

式中：X_k＝X(τ_k),U_k＝U(τ_k),k＝1,2，…N；

性能指标、边界条件和不等式约束分别转化为：

$J=\phi (X_0,t_0,X_f,t_f)+\frac{t_f-t_0}{2}\underset{k=1}{\overset{N}{\Sigma }}{w}_{k}g(X_k,U_k,{\tau }_k;t_0,t_f)$

Φ(X₀，t₀，X_f，t_f)＝0

C(X_k，U_k，τ_k；t₀，t_f)≤0。

式中W_k为高斯积分权重。

运用遗传算法优化求解该非线性规划问题，步骤如下：

第一步：令j＝0，随机产生N个初始个体构成初始种群P(0)；

第二步：评价P(j)中各个体的适应值；

第三步：判断是否满足算法收敛准则，若满足则输出搜索结果否则执行下一步骤；

第四步：令m＝0；

第五步：根据适应值大小以一定方式执行复制操作从P(k)中选出两个个体作为父代；

第六步：若交叉概率Ρ_c>＄∈[0,1]，则对选中个体执行交叉操作生成两个临时子代，否则将选中的父代个体直接作为临时子代；

第七步：按变异概率Ρ_m对临时个体执行变异操作产生两个新个体放入P(j+1)同时令m＝m+2；

第八步：若m<N则返回第五步，否则令j＝j+l并返回第二步；

通过以上的一整套算法可以获得一个使评价函数最优的运动轨迹，该运动轨迹即是所期望获得的减小仿人机器人能耗的摆动腿运动轨迹。通过设置评价函数中的权重系数S、Q、R可以获得不同的运动效果。

以上所述的实施例，只是本发明较优选的具体实施方式的一种，本领域的技术人员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。