[发明专利]基于改进纯跟踪控制的差速型AGV路径跟随方法及系统

权利要求书

1.一种基于改进纯跟踪控制的差速型AGV路径跟随方法，其特征在于，包括：

对设定的若干个控制点坐标利用三次B样条曲线拟合，得到AGV跟随的路径轨迹，所述路径轨迹由若干个路径段组成；

将AGV轮轴的中点作为AGV参考点，所述AGV参考点离当前路径段最近的点选取为当前的轨迹参考点；

根据所述轨迹参考点，计算得到转弯半径，根据所述转弯半径，规划出AGV的设定速度；

根据获取的AGV差速左右轮的当前速度差，计算预瞄标示长度；

根据当前路径段的段起点至轨迹参考点的路径长度、当前路径段的长度、当前路径段与所述路径轨迹的关系和预瞄标示长度，计算预瞄点坐标；

根据所述预瞄点坐标和AGV参考点，计算预瞄距离和AGV参考点与轨迹参考点的夹角；

利用构建的差速型AGV运动学模型，根据所述设定速度、预瞄距离和AGV参考点与轨迹参考点的夹角，分别计算差速左右轮的速度。

2.根据权利要求1所述的一种基于改进纯跟踪控制的差速型AGV路径跟随方法，其特征在于，所述轨迹参考点选取具体包括：

通过所述AGV参考点(X_r,Y_r)与所设轨迹参考点，计算得到两点距离公式；

对所述两点距离公式进行求导，利用牛顿迭代法计算出求导后的两点距离公式的最小值，得到在当前路径段上离所述AGV参考点(X_r，Y_r)最近的第一移动系数；

根据所述第一移动系数和构成当前路径段的所有控制点坐标，计算出当前的轨迹参考点(X_c，Y_c)。

3.根据权利要求2所述的一种基于改进纯跟踪控制的差速型AGV路径跟随方法，其特征在于，所述根据所述轨迹参考点，计算得到转弯半径，根据所述转弯半径，规划出AGV的设定速度具体包括：

将所述第一移动系数加一步长和减一步长，得到后控制点坐标(X_c+1，Y_c+1)和前控制点坐标(X_c-1，Y_c-1)；

根据所述轨迹参考点(X_c，Y_c)、后控制点坐标(X_c+1，Y_c+1)和前控制点坐标(X_c-1，Y_c-1)，计算得到所述轨迹参考点处的转弯半径R_c；

设所述转弯半径的最大阈值为R_max，最小阈值为R_min，设设定速度最大阈值为V_max，最小阈值为V_min，对设定速度进行规划，得到规划关系式：

4.根据权利要求2所述的一种基于改进纯跟踪控制的差速型AGV路径跟随方法，其特征在于，所述预瞄点坐标计算过程具体包括：

根据所述轨迹参考点(X_c，Y_c)，利用数值积分法求取当前路径段的段起点至轨迹参考点的路径长度S(t)；

当S(t)+L_pp≤S_k(1)时，则预瞄点位于当前路径段上，其中，L_pp为预瞄标定长度，S_k(1)为当前路径段的长度；

利用牛顿迭代法计算出所述预瞄点在当前路径段上的第二移动系数；

根据所述第二移动系数和构成当前路径段的所有控制点坐标，计算出所述预瞄点坐标。

5.根据权利要求4所述的一种基于改进纯跟踪控制的差速型AGV路径跟随方法，其特征在于，所述预瞄点坐标计算过程还具体包括：

当S(t)+L_ppS_k(1)，且当前路径段不为所述路径轨迹中最后一段时，则预瞄点位于下一段路径段上；

利用牛顿迭代法计算出所述预瞄点在下一段路径段上的第三移动系数；

根据所述第三移动系数和构成下一段路径段的所有控制点坐标，计算出所述预瞄点坐标。