的角度；
[0088] Θ5:表示踝关节绕X轴正方向所旋转的角度；
[0089] Θ6:表示踝关节绕Y轴正方向所旋转的角度。
[0090] 再由摆动腿的动力学方程建立非线性状态方程：
[0091]
[0092] 其中u为该系统的控制量，
[0093] 所述评价函数为：
[0094]
[0095] 其中，是末位时刻的状态，S、Q、R为12*12的权重矩阵；
[0096] τe匕。，切3。为开始时间，、为结束时间。
[0097] 根据机器人的模型对脚部末端位置进行约束，使得机器人的脚始终位于地面以 上。由于脚平面所在平面上的两点即可确定脚平面所在，因而只需给定脚尖和脚跟两个点 的位置约束。
[0098] 根据正运动学，可以求出脚尖与脚跟z轴方向的位置分别为：
[0099] Zt= (c2*cl_s2*sl)*L2+cl*Ll+
[0100] ((s2*cl+c2*sl) *c3+ (c2*cl_s2*sl) *s3)
[0101] +(-(s2*cl+c2*sl)*s3+(c2*cl_s2*sl)*c3)*height
[0102] Zh= (c2*cl_s2*sl)*L2+cl*Ll
[0103] + ((s2*cl+c2*sl) *c3+ (c2*cl_s2*sl) *s3) *rfoot
[0104] +(-(s2*cl+c2*sl)*s3+(c2*cl_s2*sl)*c3)*height
[0105] 其中cl=cosΘ3、c2 =cosΘ4、c3 =cosΘ5,si=sinΘ3、s2 =sinΘ4、s3 = sinΘ5,height为脚部高度，ffC]Clt为脚尖与前脚掌长（脚尖到脚心），rfC]Clt为后脚跟长（脚 后跟到脚心），ZtS脚尖与z轴方向的位置，ZA脚跟与z轴方向的位置；由此可以设定脚 跟与脚尖在z轴方向的运动范围。
[0106] 把所述动力学方程在高斯点上进行离散，将时间区间τe[t。，tf]转换到 τe[-1，1]，这个转化可以通过下式完成：
[0107]
[0108] 转换后的τ取代t成为独立变量，τ= -1时对应t。，τ= 1时对应tf;t。为开 始时间，tf为结束时间；
[0109] 用N个高斯点τ^τ2…，τN和初始端点t。= -1上的离散状态构造Lagrange插 值多项式去近似状态的时间历程：
[0110]
[0111] 式中：Χ(τ)为真实的状态时间历程，χ(τ)为由Lagrange插值多项式近似得到的 状态时间历程山;（τ)为Lagrange插值基函数，i= 0, 1，…，N
[0112]
[0113] 对时间求导，得
[0114]
[0115] 由此对仿人机器人的脚部末位时刻的位置进行约束的微分方程动态转化为一系 列代数约束：
[0116]
[0117] 式中：Xk=X(Tk)，Uk=U(Tk)，k= 1，2，..·Ν;
[0118] 性能指标、边界条件和不等式约束分别转化为：
[0119]
[0120] Φ(X〇,t〇,Xf,tf) = 0
[0121] C(Xk，Uk，Tk;t0,tf)彡 0〇
[0122] 式中Wk为高斯积分权重。
[0123] 运用遗传算法优化求解该非线性规划问题，步骤如下：
[0124] 第一步：令j= 0,随机产生N个初始个体构成初始种群P(0);
[0125] 第二步：评价P(j)中各个体的适应值；
[0126] 第三步：判断是否满足算法收敛准则，若满足则输出搜索结果否则执行下一步 骤；
[0127] 第四步：令m=0;
[0128] 第五步：根据适应值大小以一定方式执行复制操作从P(k)中选出两个个体作为 父代；
[0129] 第六步：若交叉概率F。〉$e[0, 1]，则对选中个体执行交叉操作生成两个临时 子代，否则将选中的父代个体直接作为临时子代；
[0130] 第七步：按变异概率对临时个体执行变异操作产生两个新个体放入P(j+1)同 时令m=m+2 ;
[0131] 第八步：若m〈N则返回第五步，否则令j=j+1并返回第二步；
[0132] 通过以上的一整套算法可以获得一个使评价函数最优的运动轨迹，该运动轨迹即 是所期望获得的减小仿人机器人能耗的摆动腿运动轨迹。通过设置评价函数中的权重系数 S、Q、R可以获得不同的运动效果。
[0133] 以上所述的实施例，只是本发明较优选的【具体实施方式】的一种，本领域的技术人 员在本发明技术方案范围内进行的通常变化和替换都应包含在本发明的保护范围内。
【主权项】
1. 一种仿人机器人摆动腿迈步的能效优化控制方法，基于给定的初始、末位时刻的状 态和单脚支撑起时间获得对应于评价函数的运动轨迹；其包括如下步骤： (1) 建立仿人机器人摆动腿的模型的动力学方程； (2) 依据所述动力学方程建立非线性状态方程； (3) 建立评价函数； (4) 运用遗传算法获得对应于评价函数的运动轨迹； 其特征在于：通过设置所述步骤（3)中的评价函数中的权重系数，获得具有不同运动 效果的运动轨迹。2. 根据权利要求1所述的能效优化控制方法，其中，所述动力学方程为：其中Μ(Θ)eR6X6是关节空间的惯性矩阵，Γ(Υ人勿e/T1是科氏力、离心力与重力的合 力矢量，Ui~u6为摆动腿关节1~6的驱动力矩，Μ(Θ)是关节角度的函数，只与关节角度相 关；C0/))足关节角度、角速度的函数，坐标系建立法则如下： 平行于旋转轴的方向为杆件坐标系的Z轴。 根据Z轴方向确定杆件坐标系的X轴的方向：若Zii轴的方向不平行于Zji的方向，则X轴的方向将垂直于Zii轴和Zji;若Zii轴的方向平行于Zji的方向，则Xii轴的方向为 Zii轴与Zi轴的公垂线方向。 由右手定则决定Y轴正方向。 各角度的定义如下： Θ1:表示髋关节绕Z轴正方向所旋转的角度；θ2:表示髋关节绕Y轴正方向所旋转的角度； θ3:表示髋关节绕X轴正方向所旋转的角度； Θ4:表示膝关节绕X轴正方向所旋转的角度； Θ5:表示踝关节绕X轴正方向所旋转的角度； Θ6:表示踝关节绕Y轴正方向所旋转的角度。3. 根据权利要求2所述的能效优化控制方法，其中，所述非线性状态方程为：其中，是末位时刻的状态，S、Q、R为12*12的权重矩阵；τe[t。，tf]，t。为开始时间，tf为结束时间。4. 根据权利要求3所述的能效优化控制方法，其中，所述步骤⑶中还包括如下步骤： 把所述动力学方程在高斯点上进行离散，将时间区间τe[t。，tf]转换到τe[-1， 1]，这个转化可以通过下式完成：转换后的τ取代t成为独立变量，τ= -1时对应t。，τ= 1时对应tf;t。为开始时 间，tf为结束时间； 用N个高斯点τρτ2…，τN和初始端点t。= -1上的离散状态构造Lagrange插值多 项式去近似状态的时间历程：式中：Χ(τ)为真实的状态时间历程，Χ(τ)为由Lagrange插值多项式近似得到的状态 时间历程；Q(τ)为Lagrange插值基函数，i= 0, 1，· ··，N对仿人机器人的脚部末位时刻的位置进行约束的微分方程动态转化为一系列代数约 束：式中：Xk=Χ(τk),Uk=U(τk),k= 1，2，·，·Ν; 性能指标、边界条件和不等式约束分别转化为：Φ(x〇,t〇,xf,tf) =ο C(Xk，Uk，τk;t0,tf) <Ο; 式中Wk为高斯积分权重。5. 根据权利要求1所述的能效优化控制方法，其中，所述遗传算法用于优化求解评价 函数的非线性规划问题，所述步骤（4)又包括如下步骤： (4. 1)令j= 0,随机产生N个初始个体构成初始种群P(0); (4. 2)评价P(j)中各个体的适应值； (4. 3)判断是否满足算法收敛准则，若满足，则输出搜索结果，否则执行下一步骤； (4. 4)令m= 0 ; (4.5) 根据适应值大小以一定方式执行复制操作从P(k)中选出两个个体作为父代； (4.6) 若交叉概率F。〉$e[〇, 1]，则对选中个体执行交叉操作生成两个临时子代，否 则将选中的父代个体直接作为临时子代； (4. 7)按变异概率对临时个体执行变异操作产生两个新个体放入P(j+1)同时令m=m+2； (4. 8)若m〈N，则返回第五步；否则令j=j+1，并返回步骤（4. 2)。6.根据权利要求1-5所述的能效优化控制方法，其中，仿人机器人的脚尖与脚跟z轴方 向的位置分别为： Zt= (c2*cl_s2*sl) *L2+cl*Ll+ ((s2*cl+c2*sl) *c3+ (c2*cl_s2*sl) *s3) (- (s2* cl+c2*sl) *s3+(c2*cl_s2*sl)*c3)*height Zh= (c2*cl_s2*sl) *L2+cl*Ll+ ((s2*cl+c2*sl) *c3+ (c2*cl_s2*sl) *s3)*rfcmt+ (- (s2*cl+c2*sl) *s3+(c2*cl_s2*sl)*c3)*height 其中cl=cosΘ3、c2 =cosΘ4、c3 =cosΘ5，si=sinΘ3、s2 =sinΘ4、s3 =sinΘ5,height为脚部高度，ffC]Clt与前脚掌长（脚尖到脚心），rfC]Clt为后脚跟长（脚后跟到 脚心），L1为大腿长度，L2为小腿长度，脚尖与z轴方向的位置，ZA脚跟与z轴方向 的位置；由此可以设定脚跟与脚尖在z轴方向的运动范围。
【专利摘要】本发明提供了一种仿人机器人摆动腿迈步的能效优化控制方法，其基于给定的初始、末位时刻的状态和单脚支撑起时间获得对应于评价函数的运动轨迹；通过建立仿人机器人摆动腿的模型的动力学方程，建立非线性状态方程，建立评价函数，运用遗传算法获得对应于评价函数的运动轨迹；通过设置所述评价函数中的权重系数，获得具有不同运动效果的运动轨迹。通过自主选择使评价函数最小化，获得最优运动轨迹，实现对仿人机器人行走时摆动腿的运动优化，减少能耗，提高仿人机器人的续航能力。
【IPC分类】G05B13/04
【公开号】CN105388755
【申请号】CN201510239589
【发明人】陈学超, 黄强, 余张国, 罗舟, 张伟民, 孟立波, 孙文涛
【申请人】北京理工大学
【公开日】2016年3月9日
【申请日】2015年5月12日