一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法，包括以下步骤：步骤一：基于曲线拟合算法对摆动腿足端所处局部环境进行建模；步骤二：根据所拟合出的局部环境，结合摆动腿足端工作空间，拟合出摆动腿足端可达区域；步骤三：根据步骤二中所拟合出的摆动腿足端可达区域，结合多重约束落足点评估算法，对摆动腿足端可达区域内的可规划落足点进行评估分析；步骤四：根据步骤一至步骤三，结合多重姿态转换算法，解析机器人关节输出位置，并对关节转角进行多项式插补运算，完成机器人在非结构环境下的稳定运动控制。本发明提出一种地形适应能力强、高效的稳定运动控制方法。
1.一种六足机器人稳定运动控制方法，其特征在于：包括以下步骤：步骤一：基于曲线拟合算法对摆动腿足端所处局部环境进行建模，包括测距传感器位置分布，局部环境网格化，网格化局部环境点插补及二次曲线拟合；过程如下：首先，完成测距传感器的位置分布，测距传感器分布在六足机器人的五个根关节处；其次，对局部环境进行网格化，用 来表示测距传感器在世界坐标系上的五个垂直投影点，再用 表示六足机器人前后四个足端点在世界坐标系上的四个垂直投影点，其位置坐标通过六足机器人正运动学求解获得；至此，一共获得九个已知坐标的点，其表达式如下：用Gij＝[xij yij zij]T来表示网格节点的坐标，其中，i代表的是网格的行，i＝1,2,3,4,
5，j代表网格的列，j＝1,2,3,4，即Gij代表的是网格第i行和第j列交点处的坐标，至此，一共获得20个网格节点，实现对局部环境的网格化；G11、G14、G51、G54分别表示六足机器人前后四个足端点在网格中的坐标，分别等于P4F、P1F、P6F、P3F的位置坐标；G22、G23、G33、G42、G43分别表示测距传感器在网格中的坐标，分别等于P4S、P1S、P2S、P6S、P3S的位置坐标；
然后，求解网格中未知点在世界坐标系下的位置坐标，首先求解G32的坐标位置：其次，根据G22、G32、G42拟合出一条二次曲线C1：x＝xsensor1
而根据G23、G33、G43也拟合出一条二次曲线C2：x＝xsensor3
然后根据二次曲线C1和C2，能够求出G12和G13的坐标位置：G13＝C1×Π1(G11,G14)G12＝C2×Π1(G11,G14)式中，∏1(G11，G14)为G11和G14所在的平面，运算符“×”表示求解曲线和平面的交点；
根据二次曲线C1和C2，求出G52和G53的坐标位置：G53＝C1×Π1(G51,G54)G52＝C2×Π1(G51,G54)式中，∏1(G51，G54)为G51和G54所在的平面；
最后，求解其余未知的六个网格节点的坐标位置：式中k＝2，3，4；
最后，根据G11、G12、G14得到一条二次曲线C3：x＝xsensor2
C3经过G11、G12、G13、G14，根据上述方法同样得到另外一条二次曲线C4：x＝xsensor4
根据上述曲线，对摆动腿足端所处的局部环境曲面进行拟合，所得局部环境拟合曲面为：S＝∑Ci(i＝1,2,3,4)；
步骤二：根据步骤一中所拟合出的局部环境拟合曲面，结合摆动腿足端工作空间，拟合出摆动腿足端可达区域；过程如下：首先，求解摆动腿足端在根关节坐标系的位置，令摆动腿连杆长为Li，关节转角为θsi，则摆动腿足端在根关节坐标系下的位置为：px＝[L1+L2cθs2+L3c(θs2+θs3)]cθs1py＝[L1+L2cθs2+L3c(θs2+θs3)]sθs1pz＝L2sθs2+L3s(θs2+θs3)式中，sθ＝sinθ，cθ＝cosθ；
根据上式得出摆动腿足端的工作空间，结合步骤一通过曲线拟合算法拟合出的局部环境拟合曲面，拟合出摆动腿足端可达区域：Ri＝Ti∩S
式中，Ti为摆动腿足端的工作空间，S为摆动腿足端所处的局部环境拟合曲面，表示如下：S(x,y)＝h
式中，S(x，y)代表局部环境单元点的坐标，而h代表单元点所处的地形高度；
步骤三：根据步骤二中所拟合出的摆动腿足端可达区域，结合多重约束落足点评估算法，对摆动腿足端可达区域内的可规划落足点进行评估分析；过程如下：首先，解析理想落足可接受点约束函数：建立摆动腿足端所处地形的结构几何模型：f＝(f1,f2,f3,f4)T
式中，f1，f2分别为曲面对于摆动腿足端在x、y方向上的一阶偏导数，f3，f4分别为曲面对于摆动腿足端在x、y方向上的二阶偏导数；
考虑到摆动腿足端和地形结构之间的相互约束关系，对上式作出相应变换，得出下式：F＝(F1,F2,F3,F4)T
式中，F1＝|f1|,F2＝|f2|，F3＝f3(0.55-f3)，F4＝f4(0.35-f4)；
式中，α＝(α1，α2，α3，α4)T为权重比向量，F＝(F1，F2，F3，F4)T为地形几何结构特征参数向量；
其次，解析理想落足点约束函数：提出四种基于六足机器人运动状态的约束：约束1：禁止落足点区域，其约束函数如下：式中，F’p＝(x'p,y'p)T为理想落足点在世界坐标系下的平面坐标，Fp＝(xp,yp)T为当前落足点在世界坐标系下的平面坐标，为落足点搜索方向角，RT(FF，FC，FR)为摆动相各条腿的步幅；
约束2：六足机器人静态稳定性，其约束函数如下：C2:SM(FF,FC,FR)≥0式中，SM(FF，FC，FR)为六足机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量；
约束3：摆动腿落足点可达约束，其约束函数如下：式中，RW(FF，FC，FR)和RL(FF，FC，FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离，W(FF，FC，FR)和L(FF，FC，FR)分别代表摆动相的横向和纵向摆动距离；
约束4：六足机器人步态效率，其约束函数如下：式中L'(FF，FC，FR)为摆动相的实际摆动距离；
然后，根据这四个约束耦合得出理想落足点评估函数如下：U’(F)＝βTC
式中，βT＝(β1，β2，β3，β4)为评估函数的权重比向量；
步骤四：根据步骤一至步骤三，结合多重姿态转换算法，解析六足机器人关节输出位置，并对关节转角进行多项式插补运算，完成六足机器人在非结构环境下的稳定运动控制，过程如下：首先，求解站立腿足端和各关节输出位置之间的映射关系：θd2＝2arctant1
θd3＝2arctant2-2arctant1式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]；
其次，求解站立腿足端在根关节坐标系下的位置：式中， 为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：式中， 为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵， 为机身处于初始位姿下时，机身坐标系与世界坐标系之间的位置坐标转换；
最后，结合上面两式，解析出站立腿各关节输出位置和机身重心位置之间的映射关系：θd1＝π-Atan2(byC,bxC)θd2＝2arctant1
θd3＝2arctant2-2arctant1式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]；
最后，对关节输出角进行多项式插补运算，完成六足机器人在非结构环境下的稳定运动控制。
一种六足机器人稳定运动控制方法
技术领域
[0001] 本发明涉及多足机器人非结构环境下局部环境建模、理想落足点评估及稳定运动控制领域，具体涉及基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法。
背景技术
[0002] 多足机器人具有丰富的驱动冗余和离散的步态点，能够实现复杂的运动方式，具有较强的地形适应能力。据统计，地球上有90％以上的地表属于崎岖表面，要使多足机器人在户外环境下进行作业，就必须对其在非结构环境下的运动进行规划，实现运动的稳定性和高效性。
[0003] 在非结构环境下，多足机器人的稳定运动包括三个过程：(1)机器人机身重心移到工作空间内指定的点；(2)拟合摆动腿足端所处环境，摆动腿抬起落到理想落足点；(3)摆动相和支撑相互换，新的摆动相落到理想落足点。其中，拟合摆动腿足端所处环境和足端理想落足点分析是多足机器人能否在非结构环境下稳定运动的关键。
[0004] 现有的局部环境建模算法，主要的思想是取局部环境中的一些特征点，然后利用这些特征点来模拟局部环境。这种算法的优点是环境拟合的准确度较高，缺点是要准确地拟合环境需要提取大量的特征点，而且，提取特征点的算法又具有许多种类，其中，不少算法的算法复杂度较高，因此，整个局部环境模拟算法不管是算法本身的复杂度还是时间复杂度都较高，不利于多足机器人在非结构环境下的运动规划。因此，目前迫切需要提出一种高效、精确的局部环境拟合算法，并将其应用于非结构环境下多足机器人稳定运动控制中。
[0005] 现有的摆动腿足端理想落足点评估算法，主要的思想是建立落足点评估约束函数，对局部环境中的落足点候选点进行评估，如哈工大的陈杰等通过分析局部地形信息获得地面几何参数，并由此建立约束函数，然后代入一组落足候选点的位置坐标进行代数分析，最终使得效用函数取得最大值的那组点为最佳落足点。这种方法的优点是运算量较小，缺点是建立效用函数的过程中并没有引入任何与机器人状态相关的参数，因此得到的一组落足点可能并不一定是最佳落足点。西班牙学者J.Estremera在分析地形参数的基础上，加入了机器人的步态稳定裕量，并由此建立了效用函数。这种方法的缺点是没有考虑到相邻腿坐标对摆动腿的影响，可能当摆动腿达到理想落足点时，出现机身“死锁”的情况。因此，目前迫切需要提出一种基于多种评估条件的落足点评估算法，包括对落足点所处局部环境几何结构的分析以及机器人本身位姿状态的分析。
发明内容
[0006] 鉴于现有局部环境拟合算法和理想落足点评估方法的效率低及评估过程中容易出现“坏点”的情况，本发明提出一种曲线拟合局部环境建模算法和多重约束落足点评估算法，并将两个算法发应用于多足机器人非结构环境下的稳定运动控制，最终提出一种地形适应能力强、高效的稳定运动控制方法。
[0007] 本发明解决其技术问题所采用的技术方案是：
[0008] 一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法，包括以下步骤：
[0009] 步骤一：基于曲线拟合算法对摆动腿足端所处局部环境进行建模，包括传感器位置分布，局部环境网格化，网格化局部环境点插补及二次曲线拟合；
[0010] 步骤二：根据步骤一中所拟合出的局部环境，结合摆动腿足端工作空间，拟合出摆动腿足端可达区域；
[0011] 步骤三：根据步骤二中所拟合出的摆动腿足端可达区域，结合多重约束落足点评估算法，对摆动腿足端可达区域内的可规划落足点进行评估分析；
[0012] 步骤四：根据步骤一至步骤三，结合多重姿态转换算法，解析机器人关节输出位置，并对关节转角进行多项式插补运算，完成机器人在非结构环境下的稳定运动控制。
[0013] 进一步，所述步骤一中，首先，完成测距传感器的位置分布，测距传感器主要分布在六足机器人的五个根关节处；其次，对局部环境进行网格化，用 来表示测距传感器在世界坐标系上的五个垂直投影点，再用 表示多足机器前后四个足
端点在世界坐标系上的五个垂直投影点，其位置坐标可以通过机器人正运动学求解获得；
至此，一共获得九个已知坐标的点，其表达式如下：
[0014]
[0015]
[0016] 用Gij＝[xij yij zij]T来表示网格的节点坐标，其中，i代表的是网格的行，j代表网格的列，即Gij代表的是网格第i行和第j列交点处的坐标，至此，一共获得20个网格点，实现对局部环境的网格化；
[0017] 然后，求解网格中未知点在世界坐标系下的位置坐标，首先求解G32的坐标位置：
[0018]
[0019]
[0020]
[0021] 其次，根据网格点G22、G32、G42可以拟合出一条二次曲线C1：
[0022] x＝xsensor1
[0023] z＝A1y2+B1y+C1
[0024] 而根据网格点G23、G33、G43也可以拟合出一条二次曲线C2：
[0025] x＝xsensor3
[0026] z＝A2y2+B2y+C2
[0027] 然后根据二次曲线C1和C2，可以求出网络点G12和G13的坐标位置：
[0028] G13＝C1×Π1(G11,G14)
[0029] G12＝C2×Π1(G11,G14)
[0030] 式中，∏1(G11，G14)为网格点G11和G14所在的平面，运算符“×”表示求解曲线和平面的交点；
[0031] 根据二次曲线C1和C2，还可以求出网络点G52和G53的坐标位置：
[0032] G53＝C1×Π1(G51,G54)
[0033] G52＝C2×Π1(G51,G54)
[0034] 最后，求解其余未知的六个网格节点坐标位置：
[0035]
[0036]
[0037] 式中k＝2，3，4。
[0038]
[0039]
[0040] 式中i＝2，3，4，j＝1，4；
[0041] 最后，根据网络节点G11、G12、G14得到一条二次曲线C3：
[0042] x＝xsensor2
[0043] z＝A3y2+B3y+C3
[0044] C3经过G11、G12、G13、G14这四个节点，根据上述方法同样可以得到另外一条二次曲线C4：
[0045] x＝xsensor4
[0046] z＝A4y2+B4y+C4
[0047] 根据上述曲面，可对摆动腿足端所处的局部环境曲面进行拟合，所得拟合曲面为：
[0048] S＝∑Ci(i＝1,2,3,4)。
[0049] 再进一步，所述步骤二中，首先，求解摆动腿足端在根关节坐标系的位置，令摆动腿连杆长为Li，关节转角为θsi，则摆动腿足端在根关节坐标系下的位置为：
[0050] px＝[L1+L2cθs2+L3c(θs2+θs3)]cθs1
[0051] py＝[L1+L2cθs2+L3c(θs2+θs3)]sθs1
[0052] pz＝L2sθs2+L3s(θs2+θs3)
[0053] 式中，sθ＝sinθ，cθ＝cosθ；
[0054] 根据上式得出摆动腿足端的工作空间，结合步骤一通过曲线拟合算法拟合出的局部环境曲面，拟合出摆动腿足端可达区域：
[0055] Ri＝Ti∩S
[0056] 式中，Ti为摆动腿足端的工作空间，S为摆动腿足端所处的局部环境拟合曲面，表示如下：
[0057] f(x,y)＝h
[0058] 式中，S(x，y)代表局部环境单元点的坐标，而h代表单元点所处的地形高度。
[0059] 更进一步，所述步骤三中，首先，解析理想落足可接受点约束函数：首先，建立摆动腿足端所处地形的结构几何模型：
[0060] f＝(f1,f2,f3,f4)T
[0061] 式中，f1，f2分别为曲面对于机器人足端在x、y方向上的一阶偏导数，f3，f4分别为曲面对于机器人足端在x、y方向上的二阶偏导数。
[0062] 考虑到摆动腿足端和地形结构之间的相互约束关系，对上式作出相应变换，得出下式：
[0063] F＝(F1,F2,F3,F4)T
[0064] 式中，F1＝|f1|,F2＝|f2|，F3＝f3(0.55-f3)，F4＝f4(0.35-f4)。
[0065] 则落足可接受点评估函数为
[0066] U(F)＝αTF
[0067] 式中，α＝(α1，α2，α3，α4)T为权重比向量，F＝(F1，F2，F3，F4)T为地形几何结构特征参数向量；
[0068] 其次，解析理想落足点约束函数：首先，提出四种基于机器人运动状态的约束：
[0069] 约束1：禁止落足点区域，其约束函数如下：
[0070]
[0071] 式中，F′p＝(x′p,y′p)T为理想落足点在世界坐标系下的平面坐标，Fp＝(xp,yp)T为当前落足点在世界坐标系下的平面坐标。 为落足点搜索方向角，RT(FF，FC，FR)为摆动相各条腿的步幅；
[0072] 约束2：机器人静态稳定性，其约束函数如下：
[0073] C2:SM(FF,FC,FR)≥0
[0074] 式中，SM(FF，FC，FR)为机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量；
[0075] 约束3：摆动腿落足点可达约束，其约束函数如下：
[0076]
[0077] 式中，RW(FF，FC，FR)和RL(FF，FC，FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离，W(FF，FC，FR)和L(FF，FC，FR)分别代表摆动相的横向和纵向摆动距离；
[0078] 约束4：机器人步态效率，其约束函数如下：
[0079]
[0080] 式中L′(FF，FC，FR)为摆动相的实际摆动距离；
[0081] 然后，根据这四个约束耦合得出理想落足点评估函数如下：
[0082] U(F)＝βTC
[0083] 式中，βT＝(β1，β2，β3，β4)为评估函数的权重比向量。
[0084] 所述步骤四中，首先，求解站立腿足端和各关节输出位置之间的映射关系：
[0085]
[0086] θd2＝2arctant1
[0087] θd3＝2arctant2-2arctant1
[0088] 式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]， 为站立腿足端在根关节坐标系下的位置；
[0089] 其次，求解站立腿足端在根关节坐标系下的位置：
[0090]
[0091] 式中， 为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：
[0092]
[0093] 式中， 为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵， 为机身处于理想位姿下时，机身坐标系与世界
坐标系之间的位置坐标转换， 为机身处于初始位姿下时，机身坐标系与
世界坐标系之间的位置坐标转换；
[0094] 最后，结合上面两式，解析出站立腿各关节输出位置和机身重心位置之间的映射关系：
[0095] θd1＝π-Atan2(byC,bxC)
[0096] θd2＝2arctant1
[0097] θd3＝2arctant2-2arctant1
[0098] 式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]，BAC(bxc,byc,bzc)为机身坐标系在根关节坐标系下的位置；
[0099] 最后，对关节输出角进行多项式插补运算，以六足机器人为例，完成机器人在非结构环境下的稳定运动控制。
[0100] 本发明的有益效果主要表现在：
[0101] 1、基于曲线拟合的局部环境建模算法提高了局部环境建模的准确性和高效性；
[0102] 2、基于多约束落足点评估算法能够防止得到的摆动腿理想落足点出现“坏点”；
[0103] 3、基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法，提高了多足机器人在非结构环境下运动的稳定性。
[0104] 4、基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法，提高了多足机器人对非结构环境的地形适应能力。
附图说明
[0105] 图1是本发明的基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法应用的六足机器人结构示意图；
[0106] 图2是六足机器人测距传感器的分布位置图；
[0107] 图3是在根关节坐标系下，多足机器人单摆动腿足端位置和关节输出位置之间的映射关系；
[0108] 图4是局部环境网格化节点图，图中，Gij代表网格节点位于第i行，第j列，PiS(i＝1,2,3,4,6)代表传感器在全局坐标系下的垂直投影点，PiF(i＝1,3,4,6)代表足端在全局坐标系下的垂直投影点；
[0109] 图5是通过网格点拟合出的4条二次曲线；
[0110] 图6是通过4条二次曲线拟合出的曲面环境；
[0111] 图7是在足端可达区域下32个落足候选点的地形特征值F1和F2；
[0112] 图8是在足端可达区域下32个落足候选点的地形特征值F3和F4；
[0113] 图9是基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法流程框图。
具体实施方式
[0114] 下面结合附图对本发明作进一步描述。
[0115] 参照图1～图9，一种基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法，以六足机器人为例，所述六足机器人包括六足机器人机身和六条相互独立的步行腿，所述机器人稳定运行控制方法包括以下步骤：
[0116] 步骤一：基于曲线拟合算法拟合出摆动腿足端所处局部环境，主要包括传感器位置分布，环境网格化，局部环境点插补及二次曲线拟合。
[0117] 首先，完成对测距传感器的位置分布，测距传感器主要分布在六足机器人的五个根关节处，如图2所示。其次，对局部环境进行网格化，用PiS[i＝0,1,2,3,4]来表示测距传感器在地面上的五个垂直投影点，再取六足机器人前后四个足端点，用 表示。至此，一共获得九个已知坐标的点，其表达式如下：
[0118]
[0119]
[0120] 用Gij＝[xij yij zij]T来表示网格的节点坐标，其中，i代表的是网格的行，j代表网格的列，即Gij代表的是网格第i行和第j列交点处的坐标，至此，一共获得20个网格点，如图3所示，图中，Gij代表网格节点位于第i行，第j列， 代表传感器在全局坐标系下的垂直投影点， 代表足端在全局坐标系下的垂直投影点。
[0121] 然后，对网格中未知点的位置进行插补，首先得出G32的坐标位置：
[0122]
[0123]
[0124]
[0125] 其次，根据网格点G22、G32、G42可以拟合出一条二次曲线C1：
[0126] x＝xsensor1
[0127] z＝A1y2+B1y+C1
[0128] 而根据网格点G23、G33、G43也可以拟合出一条二次曲线C2：
[0129] x＝xsensor3
[0130] z＝A2y2+B2y+C2
[0131] 然后根据二次曲线C1和C2，可以求出网络点G12和G13的坐标位置：
[0132] G13＝C1×Π1(G11,G14)
[0133] G12＝C2×Π1(G11,G14)
[0134] 式中，∏1(G11，G14)为网格点G11和G14所在的平面。
[0135] 根据二次曲线C1和C2，还可以求出网络点G52和G53的坐标位置：
[0136] G53＝C1×Π1(G51,G54)
[0137] G52＝C2×Π1(G51,G54)
[0138] 式中，∏1(G51，G54)为网格点G51和G54所在的平面。
[0139] 最后，求解其余未知的六个网格节点坐标位置：
[0140]
[0141]
[0142] 式中k＝2，3，4。
[0143]
[0144]
[0145] 式中i＝2，3，4，j＝1，4。
[0146] 最后，根据网络节点G11和G14可以得到一条二次曲线C3：
[0147] x＝xsensor2
[0148] z＝A3y2+B3y+C3
[0149] C3经过G11、G12、G13、G14这四个节点，根据上述方法同样可以得到另外一条二次曲线C4：
[0150] x＝xsensor4
[0151] z＝A4y2+B4y+C4
[0152] 根据上述曲面，可对摆动腿足端所处的局部环境曲面进行拟合，所得拟合曲面为：
[0153] S＝∑Ci(i＝1,2,3,4)
[0154] 假设六足机器人前后四条腿足端在其中一个根关节坐标系下的坐标分别是G11(130，90，26)、G14(190，90，28)、G51(130，30，35)、G54(190，30，30)，而五个传感器投影点在根关节坐标系下的坐标分别为G22(150，70，30)、G23(170，70，28)、G33(170，60，35)、G42(150，40，40)、G43(170，40，38)，根据上一小节未知点插补算法可得剩余几个点的坐标为G32(150，60，29)、G12(150，90，24.237)、G13(170，90，29.257)、G52(150，30，37.867)、G53(170，30，36.907)。
[0155] 首先，根据点G22、G32、G42坐标，在Matlab上拟合出二次曲线C1
[0156] x＝150
[0157] z＝-0.004308y2+0.3058y+31.61
[0158] 同理，拟合出C2-C4这三条二次曲线的代数表达式如下：
[0159] x＝170
[0160] z＝0.002825y2-0.4825y+49.8
[0161] y＝90
[0162] z＝0.008475x2-2.462x+202.8
[0163] y＝30
[0164] z＝-0.004784x2+1.483x-76.92
[0165] 然后，四条三维二次曲线如图4所示，通过则四条曲线模拟出的局部环境如图5所示，图中，不难看出这四条曲线精确地拟合出了足端所处的局部环境。
[0166] 步骤二：根据步骤一中所拟合出的局部环境，结合摆动腿足端工作空间，拟合出摆动腿足端可达区域。
[0167] 令摆动腿的连杆长为Li，关节转角为θi，建立摆动腿足端和关节输出位置之间的映射关系图如图6所示，由此求得摆动腿足端在根关节坐标系下的位置：
[0168] px＝[L1+L2cθs2+L3c(θs2+θs3)]cθs1
[0169] py＝[L1+L2cθs2+L3c(θs2+θs3)]sθs1
[0170] pz＝L2sθs2+L3s(θs2+θs3)
[0171] 式中，sθ＝sinθ，cθ＝cosθ，下同。
[0172] 利用上式得出摆动腿足端的工作空间，结合步骤一曲线拟合得到的局部环境曲面，解析出摆动腿足端可达区域：
[0173] Ri＝Li∩S
[0174] 式中，Li为摆动腿足端的工作空间，S为摆动腿足端所处的局部环境拟合曲线，可表示如下：
[0175] f(x,y)＝h
[0176] 式中，S(x，y)代表局部环境单元点的坐标，而h代表单元点所处的地形高度。可达区域的实际求解主要依据的是在空间坐标系中，摆动腿足端工作空间边界曲面的坐标和局部环境模型坐标的交集，如第二小节中摆动腿足端工作空间边界曲面三个坐标的取值范围分别是：-37.5≤X≤448.63、-215.45≤Y≤215.41、-18.549≤Z≤237.56；而局部环境模型三个坐标的取值范围分别是：150≤X≤170、20≤Y≤100、17.863≤Z≤41.283。不难看出，局部环境模型三个坐标的取值都包含在摆动腿足端工作空间三个坐标范围内。因此，摆动腿足端的可达区域即局部环境模型。当然，上面一小节对局部环境建模时所选取的点是为了仿真算法随意选取，在实际建模过程中，足端可达区域并不一定是局部环境模型。
[0177] 步骤三：根据步骤二中所拟合出的摆动腿足端可达区域，基于多维度落足点评估算法，对摆动腿足端可达区域内的可规划落足点进行评估，具体包括：
[0178] 首先，解析理想落足可接受点约束函数，先建立摆动腿足端所处地形的几何结构模型：
[0179] f＝(f1,f2,f3,f4)T
[0180] 式中，f1，f2分别为曲面对于机器人足端在x、y方向上的一阶偏导数，f3，f4分别为曲面对于机器人足端在x、y方向上的二阶偏导数。
[0181] 考虑到摆动腿足端和地形结构之间的相互约束关系，对上式作出相应变换，得出下式：
[0182] F＝(F1,F2,F3,F4)T
[0183] 式中，F1＝|f1|,F2＝|f2|，F3＝f3(0.55-f3)，F4＝f4(0.35-f4)。
[0184] 则落足可接受点的效用评估函数为
[0185] U(F)＝αTF
[0186] 式中，α＝(α1，α2，α3，α4)T为权重比向量，F＝(F1，F2，F3，F4)T为地形结构特征参数向量。通过Matlab中的SVM工具包，集合样本学习理论得出落足可接受点效用评估函数为：
[0187] U(F)＝αTF＝-0.2993F1-0.4115F2+0.8319F3+0.7489F4
[0188] 将80mmx40mm的曲面划分为32个10mmx10mm的区域，每一个区域近似看成一个落足候选点，得出32个落足候选点的地形几何特征向量如图7、图8所示。其次，解析理想落足点约束函数，首先，提出四个约束：
[0189] 约束1：禁止落足点区域，其效用函数如下：
[0190]
[0191] 式中，F′p＝(x′p,y′p)T为理想落足点的平面坐标，Fp＝(xp,yp)T为当前落足点的平面坐标。为搜索方向角，RT(FF，FC，FR)为摆动相各条腿的步幅。
[0192] 约束2：静态稳定性，其效用函数如下：
[0193] C2:SM(FF,FC,FR)≥0
[0194] 式中，SM(FF，FC，FR)为机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量。
[0195] 约束3：落足点可达约束，其效用函数如下：
[0196]
[0197] 式中，RW(FF，FC，FR)和RL(FF，FC，FR)分别代表理想落足点到当前摆动相所在点横向和纵向的距离，W(FF，FC，FR)和L(FF，FC，FR)分别代表摆动相的横向和纵向摆动距离。
[0198] 约束4：步态效率，其效用函数如下：
[0199]
[0200] 式中L′(FF，FC，FR)为摆动相的实际摆动距离。
[0201] 然后，根据这四个约束得出理想落足点的评估函数如下：
[0202] U(F)＝βTC
[0203] 式中，βT＝(β1，β2，β3，β4)为效用函数的权重比向量。
[0204] 步骤四：根据步骤一至步骤三，基于多重姿态转换算法，解析机器人关节输出位置，完成非结构环境下的步态优化，具体包括：
[0205] 首先，求解站立腿足端和站立腿各关节输出位置之间的映射关系：
[0206]
[0207] θd2＝2arctant1
[0208] θd3＝2arctant2-2arctant1
[0209] 式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]；
[0210] 其次，求解站立腿足端在根关节坐标系下的位置：
[0211]
[0212] 最后，结合上面两式，解析出站立腿各关节输出位置和机身重心位置之间的映射关系：
[0213] θd1＝π-Atan2(byC,bxC)
[0214] θd2＝2arctant1
[0215] θd3＝2arctant2-2arctant1
[0216] 式中，t1＝tan(θd2/2)，t2＝tan[(θd2-θd3)/2]。
[0217] 本发明基于曲线拟合建模和多重约束落足点评估的机器人稳定运动控制方法流程框图如图9所示，图中，要实现多足机器人在非结构环境下的稳定、高效运动，最终需要求得多足机器人在非结构环境下摆动腿和站立腿关节的输出位置，即关节转角θs和θd。站立腿的关节转角主要由机身工作空间和机身逆运动学来确定，而摆动腿的关节转角主要由理想落足点的位置和摆动腿运动学决定。
