[发明专利]一种双足机器人落脚位置控制系统与方法

权利要求书

1.一种双足机器人落脚位置控制系统，其特征在于，包括激光雷达或视觉传感器(1)、陀螺仪(3)、六维力传感器(6)以及工控机，激光雷达或视觉传感器(1)、陀螺仪(3)、六维力传感器(6)均与工控机进行信号传输；工控机中的模糊控制器以x_sd、x_en，y_sd、y_en，θ_sd、θ_pt分别作为输入量，进行模糊处理，得到在可落脚区域内摆动脚的具体落脚位置；其中：x_sd、y_sd为六维力传感器(6)获取的双足机器人支撑脚的实时位置，x_en、y_en为激光雷达或视觉传感器(1)获取的障碍物在机器人本体坐标系中的位置，θ_sd为陀螺仪(3)获取的双足机器人初始偏航角，θ_pt是利用当前路径节点和下一步相关联的路径节点组成的直线与世界坐标系x轴的夹角。

2.根据权利要求1所述的双足机器人落脚位置控制系统，其特征在于，所述其中：x_pt、y_pt为距离当前支撑脚位置最近的路径节点，x_pt+1、y_pt+1为当前摆动脚落脚后距离落脚位置最近的路径节点，x_next、y_next为可落脚范围内摆动脚的具体落脚位置，x_i、y_i为路径节点。

3.一种根据权利要求1-2任意所述的双足机器人落脚位置控制系统的控制方法，其特征在于，规划通行路径，根据双足机器人步长约束、运动能力、脚的尺寸与中心偏移，确定摆动脚的在通行路径上的可落脚区域，模糊处理确定机器人的具体落脚位置；

所述模糊处理包括模糊化、模糊规则推理以及去模糊化；

对x_en、x_sd模糊处理过程为：输入为x_pt-x_pt-1、x_en，输出为Δx_next；

(1)模糊化

x_pt-x_pt-1模糊化为后退、小步前进、中等步前进、大步前进，对应的范围分别为：[-D_min,0]、x_en模糊化为很近、近、中等、远、很远，对应的范围分别为：[4D_max,5D_max]；Δx_next模糊化为后退、短、中等、长，对应的范围分别为：[-D_min,0]、其中：D_max为摆动脚向前可移动最大距离，D_min为摆动脚向后可移动最大距离；所有的输入都使用三角法作为隶属度函数，并且各自隶属度值为μ_x；

(2)模糊规则推理

如果x_pt-x_pt-1＝后退，那么x_next＝后退；

如果x_pt-x_pt-1＝小步前进，且x_en＝很近；那么x_next＝短；

如果x_pt-x_pt-1＝小步前进，且x_en＝近；那么x_next＝短；

如果x_pt-x_pt-1＝小步前进，且x_en＝中等；那么x_next＝中等；

如果x_pt-x_pt-1＝小步前进，且x_en＝远；那么x_next＝长；

如果x_pt-x_pt-1＝小步前进，且x_en＝很远；那么x_next＝长；

如果x_pt-x_pt-1＝中等步前进，且x_en＝很近；那么x_next＝短；

如果x_pt-x_pt-1＝中等步前进，且x_en＝近；那么x_next＝短；

如果x_pt-x_pt-1＝中等步前进，且x_en＝中等；那么x_next＝中等；

如果x_pt-x_pt-1＝中等步前进，且x_en＝远；那么x_next＝长；

如果x_pt-x_pt-1＝中等步前进，且x_en＝很远；那么x_next＝长；

如果x_pt-x_pt-1＝大步前进，且x_en＝很近；那么x_next＝短；

如果x_pt-x_pt-1＝大步前进，且x_en＝近；那么x_next＝短；

如果x_pt-x_pt-1＝大步前进，且x_en＝中等；那么x_next＝中等；

如果x_pt-x_pt-1＝大步前进，且x_en＝远；那么x_next＝中等；

如果x_pt-x_pt-1＝大步前进，且x_en＝很远；那么x_next＝长；

(3)利用重心法去模糊化

对y_en、y_sd模糊处理过程为：输入为y_pt-y_pt-1、y_en，输出为Δy_next；

(1)模糊化

y_pt-y_pt-1模糊化为向内、向外小摆、向外中摆、向外大摆，对应范围分别为：[L_min,B]、y_en模糊化为很近、近、中等、远、很远，对应的范围分别为：[L_max,2L_max]、[2L_max,3L_max]、[3L_max,4L_max]；Δy_next模糊化为向内、小、中等、大，对应的范围分别为：[L_min,B]、其中L_max为向外侧可移动最大距离，L_min为向内侧可移动最大距离，B为双足机器人直立时两腿重心投影间距离；所有的输入都使用三角法作为隶属度函数，并且各自隶属度值为μ_y；

(2)模糊规则推理

如果y_pt-y_pt-1＝向内，那么Δy_next＝向内；

如果y_pt-y_pt-1＝向外小摆，且y_en＝很近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外小摆，且y_en＝近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外小摆，且y_en＝中等；那么Δy_next＝中等；

如果y_pt-y_pt-1＝向外小摆，且y_en＝远；那么Δy_next＝大；

如果y_pt-y_pt-1＝向外小摆，且y_en＝很远；那么Δy_next＝大；

如果y_pt-y_pt-1＝向外中摆，且y_en＝很近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外中摆，且y_en＝近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外中摆，且y_en＝中等；那么Δy_next＝中等；

如果y_pt-y_pt-1＝向外中摆，且y_en＝远；那么Δy_next＝大；

如果y_pt-y_pt-1＝向外中摆，且y_en＝很远；那么Δy_next＝大；

如果y_pt-y_pt-1＝向外大摆，且y_en＝很近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外大摆，且y_en＝近；那么Δy_next＝小；

如果y_pt-y_pt-1＝向外大摆，且y_en＝中等；那么Δy_next＝中等；

如果y_pt-y_pt-1＝向外大摆，且y_en＝远；那么Δy_next＝大；

如果y_pt-y_pt-1＝向外大摆，且y_en＝很远；那么Δy_next＝大；

(3)利用重心法去模糊化

对θ_sd、θ_pt模糊理过程为：输入为θ_sd、θ_pt，输出为Δθ_next；

(1)模糊化

θ_sd模糊化为内偏、小幅度外偏、中等幅度外篇、大幅度外偏，对应的范围分别为：[-θ_min,0]、θ_pt模糊化为负大方向、负小方向、中等、正小方向、正大方向，对应的范围分别为：Δθ_next模糊化为内偏、小幅度外偏、中等幅度外篇、大幅度外偏，对应的范围分别为：[-θ_min,0]、其中：θ_min为摆动脚向内侧最大偏航角度，θ_max为摆动脚向外侧最大偏航角；所有的输入都使用三角法作为隶属度函数，并且各自隶属度值为μ_θ；

(2)模糊规则推理

当支撑脚为左脚时：

如果θ_sd＝内偏，且θ_pt＝负大方向；那么Δθ_next＝大幅度外偏；

如果θ_sd＝内偏，且θ_pt＝负小方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝内偏，且θ_pt＝中等；那么Δθ_next＝小幅度外偏；

如果θ_sd＝内偏，且θ_pt＝正小方向；那么Δθ_next＝内偏；

如果θ_sd＝内偏，且θ_pt＝正大方向；那么Δθ_next＝内偏；

如果θ_sd＝小幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝大幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝中等；那么Δθ_next＝小幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝内偏；

如果θ_sd＝小幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝大幅度外偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝中等；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝大幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝小幅度外偏；

如果θ_sd＝大幅度外偏，且θ_pt＝中等；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝内偏；

当支撑脚为右脚时：

如果θ_sd＝内偏，且θ_pt＝负大方向；那么Δθ_next＝内偏；

如果θ_sd＝内偏，且θ_pt＝负小方向；那么Δθ_next＝内偏；

如果θ_sd＝内偏，且θ_pt＝中等；那么Δθ_next＝小幅度外偏；

如果θ_sd＝内偏，且θ_pt＝正小方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝内偏，且θ_pt＝正大方向；那么Δθ_next＝大幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝内偏；

如果θ_sd＝小幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝内偏；

如果θ_sd＝小幅度外偏，且θ_pt＝中等；那么Δθ_next＝小幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝中等幅度外偏；

如果θ_sd＝小幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝大幅度外偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝中等；那么Δθ_next＝内偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝小幅度外偏；

如果θ_sd＝中等幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝中等幅度内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝负大方向；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝负小方向；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝中等；那么Δθ_next＝内偏；

如果θ_sd＝大幅度外偏，且θ_pt＝正小方向；那么Δθ_next＝小幅度外偏；

如果θ_sd＝大幅度外偏，且θ_pt＝正大方向；那么Δθ_next＝中等幅度内偏；

(3)去模糊化

利用重心法去模糊化：

4.根据权利要求3所述的双足机器人落脚位置控制方法，其特征在于，机器人的具体落脚位置为(x_next,y_next,θ_next)，且x_next＝x_sd+Δx_next，y_next＝y_sd+Δy_next，θ_next＝θ_sd+Δθ_next。