[发明专利]基于预瞄距离自适应的轮式机器人控制方法及控制系统

权利要求书

1.一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，为机器人设置期望路径，重复执行以下步骤使机器人沿所述期望路径行驶：

步骤S1，获取当前路径曲率和当前纵向速度，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离，获得与所述最优预瞄距离对应的预瞄点，预瞄距离为预瞄点在机器人坐标系下的纵轴坐标，所述参考表中罗列了路径曲率和纵向速度的不同取值组合对应的最优预瞄距离；

所述参考表的建立方法包括：

分别为预瞄距离、路径曲率和纵向速度设置取值范围，在路径曲率的取值范围和纵向速度的取值范围内构建路径曲率和纵向速度的多个不同的取值组合，通过第一方法获得每个取值组合对应的最优预瞄距离，所述第一方法包括：

步骤S11，设置种群规模，将预瞄距离作为粒子的位置变量，将预瞄距离的取值范围作为粒子的搜索空间，初始化历史最优位置以及历史最优位置的初始值对应的适应度值，初始化全局最优位置以及全局最优位置的初始值对应的适应度值；设t为迭代次数，令t的初始值为1；通过随机函数初始化每个粒子的位置和速度；

步骤S12，第t次迭代过程包括：

计算每个粒子的适应度值，将全局最优位置更新为适应度值最小的粒子的位置；若适应度最小值小于历史最优值位置对应的适应度值，将历史最优位置更新为适应度值最小的粒子的位置，将更新后的历史最优位置与最小适应度值对应，否则不更新历史最优位置；更新每个粒子的位置和速度；

步骤S13，若t≥T或者本次迭代的最小适应度值收敛至预设适应度阈值，则进入步骤S14，否则令t＝t+1,返回执行步骤S12；所述T为预设的最大迭代次数；

步骤S14，迭代结束，将历史最优位置作为与取值组合对应的最优预瞄距离；

步骤S2，基于所述预瞄点进行横向控制和/或纵向控制；

所述横向控制为：获取机器人转向轮基于所述预瞄点的目标转角，控制机器人的转向轮按照所述目标转角偏转；

所述纵向控制为：根据机器人当前纵向速度、预瞄点坐标和预瞄点的期望纵向速度获得机器人纵向总的期望加速度，根据机器人纵向总的期望加速度获得机器人各车轮的目标力矩，控制各车轮按照各自的目标力矩转动；

所述纵向控制中获得车轮的目标力矩的过程包括：

步骤A，获取机器人纵向总的期望加速度a_ref为：

其中，v_c表示机器人当前在大地坐标系下的纵向速度，v_p表示在大地坐标系下预瞄点处的期望纵向速度，t_p是机器人保持当前纵向速度v_c到达预瞄点在大地坐标系下的纵向坐标位置的时间；

步骤B，根据机器人纵向总的期望加速度a_ref建立如下等式：

其中，F_l表示机器人总的期望纵向力，m表示机器人质量，α表示道路坡度，ρ表示空气密度，C_d表示空气阻力系数，A_wind表示机器人迎风面面积；

步骤C，每个车轮平均分配力矩，每个车轮的目标力矩为：

其中，N表示车轮总数，R表示车轮半径；所述机器人的转向轮为前轮；

在所述步骤S2中，所述横向控制中获得前轮的目标转角的过程包括：

步骤S21,建立机器人的单轴运动模型，所述单轴运动模型包含在机器人坐标系xoy下的三个自由度，三个自由度分别为沿纵向x轴的平动、沿横向y轴的平动和绕竖直方向的转动即横摆角；

步骤S22,根据如下公式获得预瞄点在机器人坐标系xoy下坐标

其中，x_e为预瞄点在机器人坐标系xoy下的纵轴坐标，定义为预瞄距离；y_e表示预瞄点在机器人坐标系xoy下的横轴坐标，定义为横向预瞄误差；表示机器人在机器人坐标系xoy下位于预瞄点的期望横摆角；表示预瞄点P在大地坐标系XOY中的坐标，X_p、Y_p、分别表示预瞄点P在大地坐标系XOY中的X轴坐标、Y轴坐标、预瞄点P处切线与X轴的夹角；表示当前机器人质心在大地坐标系XOY中的坐标，X_c、Y_c、分别表示当前机器人质心在大地坐标系XOY中的X轴坐标、Y轴坐标、横摆角；

根据预瞄点在机器人坐标系xoy下坐标和所述单轴运动模型获得机器人的单点视觉预瞄模型；

步骤S23，结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角；

在大地坐标系XOY下，所述机器人的单轴运动模型表示为：

其中，δ_f表示机器人的前轮转角；表示机器人质心在大地坐标系XOY下的纵向的加速度；表示机器人质心在大地坐标系XOY下的横向的加速度；表示机器人质心在大地坐标系XOY下的横摆角加速度；变量表示机器人质心在大地坐标系XOY下的横向速度，表示机器人质心在大地坐标系XOY下的横摆角速度，m表示机器人质量，C_lf表示前轮纵向刚度，C_lr表示后轮的纵向刚度，s_f表示前轮的滑移率，s_r表示后轮的滑移率；变量C_cf表示前轮侧偏刚度，a表示机器人质心距前轴的距离；变量C_cr表示后轮侧偏刚度，b表示机器人质心距后轴的距离；变量变量I_z表示，机器人绕z轴的转动惯量；变量

所述机器人的单点视觉预瞄模型为：

其中，表示在机器人坐标系中机器人的横向偏差变化率，表示机器人坐标系中机器人质心在预瞄点处的横摆角速度，表示当前机器人质心在大地坐标系的横摆角速度，v_p表示在大地坐标系中预瞄点期望的纵向速度，ρ_p表示预瞄点的路径曲率，表示大地坐标系下机器人质心处的横向速度；

结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角的过程具体包括：

步骤S231，基于滑膜变结构理论获得控制对象X的状态方程为：

其中，分别表示机器人驶向预瞄点过程中任意时间点在大地坐标系中机器人质心的纵向速度、横向速度、横摆角速度，表示在机器人坐标系中的横向偏差变化率，表示对X中各分量求导，得到

第一系数矩阵f(X)为：表示预瞄点期望纵向加速度；

第二系数矩阵B为：B＝[b₁ b₂ b₃ b₄ b₅ b₆]^T；

第三系数矩阵W为：W＝[00000v_p]^T；

步骤S232，以横向预瞄误差及横向预瞄误差变化率作为控制目标，则滑模面可设计为：

其中，c表示滑膜参数；s表示滑膜；

步骤S233，设当s→0时，滑膜趋近率设置为等速趋近率，满足如下关系：

其中，g表示切换增益，sat(·)表示饱和函数，Δ表示边界层厚度，转换系数q＝1/Δ；

步骤S234，根据关系式得到等式根据等式和控制对象X的状态方程得到机器人前轮的目标转角δ_d为：