[发明专利]一种六足机器人稳定运动控制方法

权利要求书

1.一种六足机器人稳定运动控制方法，其特征在于：包括以下步骤：

步骤一：基于曲线拟合算法对摆动腿足端所处局部环境进行建模，包括测距传感器位置分布，局部环境网格化，网格化局部环境点插补及二次曲线拟合；过程如下：

首先，完成测距传感器的位置分布，测距传感器分布在六足机器人的五个根关节处；其次，对局部环境进行网格化，用来表示测距传感器在世界坐标系上的五个垂直投影点，再用表示六足机器人前后四个足端点在世界坐标系上的四个垂直投影点，其位置坐标通过六足机器人正运动学求解获得；至此，一共获得九个已知坐标的点，其表达式如下：

用G_ij＝[x_ij y_ij z_ij]^T来表示网格节点的坐标，其中，i代表的是网格的行，i＝1,2,3,4,5，j代表网格的列，j＝1,2,3,4，即G_ij代表的是网格第i行和第j列交点处的坐标，至此，一共获得20个网格节点，实现对局部环境的网格化；G₁₁、G₁₄、G₅₁、G₅₄分别表示六足机器人前后四个足端点在网格中的坐标，分别等于P₄^F、P₁^F、P₆^F、P₃^F的位置坐标；G₂₂、G₂₃、G₃₃、G₄₂、G₄₃分别表示测距传感器在网格中的坐标，分别等于P₄^S、P₁^S、P₂^S、P₆^S、P₃^S的位置坐标；

然后，求解网格中未知点在世界坐标系下的位置坐标，首先求解G₃₂的坐标位置：

其次，根据G₂₂、G₃₂、G₄₂拟合出一条二次曲线C1：

x＝x_sensor1

z＝A₁y²+B₁y+C₁

而根据G₂₃、G₃₃、G₄₃也拟合出一条二次曲线C2：

x＝x_sensor3

z＝A₂y²+B₂y+C₂

然后根据二次曲线C1和C2，能够求出G₁₂和G₁₃的坐标位置：

G₁₃＝C₁×Π₁(G₁₁,G₁₄)

G₁₂＝C₂×Π₁(G₁₁,G₁₄)

式中，∏₁(G₁₁，G₁₄)为G₁₁和G₁₄所在的平面，运算符“×”表示求解曲线和平面的交点；

根据二次曲线C₁和C₂，求出G₅₂和G₅₃的坐标位置：

G₅₃＝C₁×Π₁(G₅₁,G₅₄)

G₅₂＝C₂×Π₁(G₅₁,G₅₄)

式中，∏₁(G₅₁，G₅₄)为G₅₁和G₅₄所在的平面；

最后，求解其余未知的六个网格节点的坐标位置：

式中k＝2，3，4；

式中i＝2，3，4，j＝1，4；

最后，根据G₁₁、G₁₂、G₁₄得到一条二次曲线C3：

x＝x_sensor2

z＝A₃y²+B₃y+C₃

C3经过G₁₁、G₁₂、G₁₃、G₁₄，根据上述方法同样得到另外一条二次曲线C4：

x＝x_sensor4

z＝A₄y²+B₄y+C₄

根据上述曲线，对摆动腿足端所处的局部环境曲面进行拟合，所得局部环境拟合曲面为：

S＝∑Ci(i＝1,2,3,4)；

步骤二：根据步骤一中所拟合出的局部环境拟合曲面，结合摆动腿足端工作空间，拟合出摆动腿足端可达区域；过程如下：

首先，求解摆动腿足端在根关节坐标系的位置，令摆动腿连杆长为L_i，关节转角为θ_si，则摆动腿足端在根关节坐标系下的位置为：

p_x＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]cθ_s1

p_y＝[L₁+L₂cθ_s2+L₃c(θ_s2+θ_s3)]sθ_s1

p_z＝L₂sθ_s2+L₃s(θ_s2+θ_s3)

式中，sθ＝sinθ，cθ＝cosθ；

根据上式得出摆动腿足端的工作空间，结合步骤一通过曲线拟合算法拟合出的局部环境拟合曲面，拟合出摆动腿足端可达区域：

R_i＝T_i∩S

式中，T_i为摆动腿足端的工作空间，S为摆动腿足端所处的局部环境拟合曲面，表示如下：

S(x,y)＝h

式中，S(x，y)代表局部环境单元点的坐标，而h代表单元点所处的地形高度；

步骤三：根据步骤二中所拟合出的摆动腿足端可达区域，结合多重约束落足点评估算法，对摆动腿足端可达区域内的可规划落足点进行评估分析；过程如下：首先，解析理想落足可接受点约束函数：建立摆动腿足端所处地形的结构几何模型：

f＝(f₁,f₂,f₃,f₄)^T

式中，f₁，f₂分别为曲面对于摆动腿足端在x、y方向上的一阶偏导数，f₃，f₄分别为曲面对于摆动腿足端在x、y方向上的二阶偏导数；

考虑到摆动腿足端和地形结构之间的相互约束关系，对上式作出相应变换，得出下式：

F＝(F₁,F₂,F₃,F₄)^T

式中，F₁＝|f₁|,F₂＝|f₂|，F₃＝f₃(0.55-f₃)，F₄＝f₄(0.35-f₄)；

则落足可接受点评估函数为

U(F)＝α^TF

式中，α＝(α₁，α₂，α₃，α₄)^T为权重比向量，F＝(F₁，F₂，F₃，F₄)^T为地形几何结构特征参数向量；

其次，解析理想落足点约束函数：提出四种基于六足机器人运动状态的约束：约束1：禁止落足点区域，其约束函数如下：

式中，F’_p＝(x'_p,y'_p)^T为理想落足点在世界坐标系下的平面坐标，F_p＝(x_p,y_p)^T为当前落足点在世界坐标系下的平面坐标，为落足点搜索方向角，R_T(F_F，F_C，F_R)为摆动相各条腿的步幅；

约束2：六足机器人静态稳定性，其约束函数如下：

C₂:S_M(F_F,F_C,F_R)≥0

式中，S_M(F_F，F_C，F_R)为六足机器人机身重心在新支撑相形成的支撑多边形内的稳定裕量；

约束3：摆动腿落足点可达约束，其约束函数如下：

式中，R_W(F_F，F_C，F_R)和R_L(F_F，F_C，F_R)分别代表理想落足点到当前摆动相所在点横向和纵向的距离，W(F_F，F_C，F_R)和L(F_F，F_C，F_R)分别代表摆动相的横向和纵向摆动距离；

约束4：六足机器人步态效率，其约束函数如下：

式中L'(F_F，F_C，F_R)为摆动相的实际摆动距离；

然后，根据这四个约束耦合得出理想落足点评估函数如下：

U’(F)＝β^TC

式中，β^T＝(β₁，β₂，β₃，β₄)为评估函数的权重比向量；

步骤四：根据步骤一至步骤三，结合多重姿态转换算法，解析六足机器人关节输出位置，并对关节转角进行多项式插补运算，完成六足机器人在非结构环境下的稳定运动控制，过程如下：

首先，求解站立腿足端和各关节输出位置之间的映射关系：

θ_d2＝2arctant₁

θ_d3＝2arctant₂-2arctant₁

式中，t1＝tan(θ_d2/2)，t2＝tan[(θ_d2-θ_d3)/2]；

其次，求解站立腿足端在根关节坐标系下的位置：

式中，为机身处于理想目标位姿时机身坐标系与世界坐标系之间的位置转换矩阵，其表达式如下：

式中，为机身处于理想位姿下时，根关节坐标系和机身坐标系之间的位置转换矩阵，为机身处于初始位姿下时，机身坐标系与世界坐标系之间的位置坐标转换；

最后，结合上面两式，解析出站立腿各关节输出位置和机身重心位置之间的映射关系：

θ_d1＝π-Atan2(^by_C,^bx_C)

θ_d2＝2arctant₁

θ_d3＝2arctant₂-2arctant₁

式中，t₁＝tan(θ_d2/2)，t₂＝tan[(θ_d2-θ_d3)/2]；

最后，对关节输出角进行多项式插补运算，完成六足机器人在非结构环境下的稳定运动控制。