CN109656250A - 一种激光叉车的路径跟踪方法 - Google Patents

Abstract

本发明适用于自动控制技术领域，提供了一种激光叉车的路径跟踪方法，该方法包括如下步骤：S1、将行驶路径分割成预瞄点序列，即构成预瞄点路径；S2、定时获取行驶方向上距激光导航AGV车身最近的预瞄点，称为目标预瞄点；S3、控制激光叉车朝行目标预瞄点行驶。行驶路径进行分割，分割成预瞄点序列，控制激光叉车朝当前行驶方向上最近的预瞄点行驶，将基于路径曲线的跟踪转变成基于目标预瞄点的跟踪，简化跟踪计算，因此可以用于复杂路径的跟踪。

Description

一种激光叉车的路径跟踪方法

技术领域

本发明属于自动控制技术领域，提供了一种激光叉车的路径跟踪方法。

背景技术

随着物料输送系统、柔性制造系统、自动化立体仓库等的发展，AGV(AutomatedGuided Vehicle)作为物料输送系统和柔性制造系统中的重要组成部分，可以解决传统物流系统的弊端，在制造业输送环节得到了广泛的应用。激光叉车AGV相比于其他导航方式AGV具有明显的优势，表现在只需安装反光板和布置网络简单的施工、转弯半径小、路径更改方便、定位极度高等方面，激光叉车AGV能够根据计算机所规划的路径行驶，广泛的被应用在机械、电子、纺织、食品等行业。

路径跟踪控制是激光叉车AGV系统中的关键环节之一，其控制精度和质量直接关系到AGV小车运行的精确性及安全可靠性。在通过路径规划后，选择合适的路径跟踪方法，消除激光叉车AGV在行驶过程中实际行驶路径与参考轨迹之间产生的跟踪偏差，实现路径的准确跟踪显得尤其重要。现有的激光AGV路径跟踪方法是基于AGV当前的参考坐标和预设跟踪路径曲线上采用四分法划分的最近点的坐标，计算出横向偏差值及偏差角，将两者加权求和后修正舵轮的偏转角，从而使AGV完成跟踪的预设路径，方法是基于路径曲线进行跟踪，无法适合复杂的路线，具有局限性。

发明内容

本发明实施例提供一种激光叉车的路径跟踪方法，将跟踪路径进行分割，分割成预瞄点序列，控制激光叉车朝当前行驶方向上最近的预瞄点行驶，将基于路径曲线的跟踪转变成基于目标预瞄点的跟踪，简化跟踪计算。

为了实现上述目的，本发明提供了一种激光叉车的路径跟踪方法，该方法包括如下步骤：

S1、将行驶路径分割成预瞄点序列，即构成预瞄点路径；

S2、定时获取行驶方向上距激光导航AGV车身最近的预瞄点，称为目标预瞄点；

S3、控制激光叉车朝行目标预瞄点行驶。

进一步的，所述步骤S1具体包括如下步骤：

S11、提取行驶路径中的转折点，相邻两转折点之间构成一个路段；

S12、将路段分割成子路段，保存子路段的端点坐标，形成端点路径；

S13、对端点路径进行平滑处理，平滑处理后的端点即为预瞄点。

进一步的，所述步骤S12包括如下步骤：

S121.计算路段的欧式距离，即两个转折点间的距离；

S122、若两转折点间的距离大于距离阈值，则按照设定的步长对路径进行分段，分割成子路段，若两转折点间的距离小于或等于距离阈值，则将路段等分成两个子路段，保存各子路段端点。

进一步的，基于模糊PID控制器控制激光叉车朝目标预瞄点行驶，即将横向位置偏差l_d及车辆当前行驶方向与期望行驶方向的夹角偏差α作为输入模糊控制器，利用模糊控制算法在线对参数进行修改后得到K_p，K_i，K_d参数值，再通过PID控制器得出控制前轮偏角量，从而引导激光AGV朝目标预瞄点行进。

进一步的，在步骤S1之前还包括：

基于激光叉车的实际尺寸来构建激光叉车模型，即定义两万向轮之间的距离，舵轮距两万向轮所在直线的距离，激光雷达安装于舵轮的正上方，两个万向轮的中心位置为前转向轮的控制点及激光叉车的中心。

本发明提供的激光叉车路径跟踪方法具有如下有益技术效果：

1.行驶路径进行分割，分割成预瞄点序列，控制激光叉车朝当前行驶方向上最近的预瞄点行驶，将基于路径曲线的跟踪转变成基于目标预瞄点的跟踪，简化跟踪计算，因此可以用于复杂路径的跟踪。

2.利用Bezier函数对分割点进行平滑处理，激光叉车AGV在进行路径跟踪时，遇到拐角点无需在拐角点附近区域完成减速，停止再加速的过程，从而整体提高了作业效率。

3.为了提高车辆在直线或弯道环境中车辆行驶的平稳性，采用了模糊PID算法根据车体姿态与预瞄点的横向位置偏差及角度偏差实时动态调整PID参数后，输出前向舵轮的期望偏转角，该方法能够以较小的偏差跟踪设定的路线，提高AGV的整体运行效率，同时保证了AGV准确的到达预设的目标位置。

附图说明

图1为本发明实施例提供的激光叉车路径跟踪方法流程图；

图2为本发明实施例提供的激光叉车跟踪路径示意图；

图3为本发明实施例提供的激光叉车端点路径平滑处理对照图；

图4为本发明实施例提供的横向位置偏差及偏角几何示意图；

图5为本发明实施例提供的模糊PID控制流程图；

图6为本发明实施例提供的激光叉车模型示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明提供的激光叉车路径跟踪方法是将行驶路径进行分割，分割成预瞄点序列，控制激光叉车朝当前行驶方向上最近的预瞄点行驶，将基于路径曲线的跟踪转变成基于目标预瞄点的跟踪，简化跟踪计算，因此可以用于复杂路径的跟踪。

图1为本发明实施例提供的激光叉车路径跟踪方法流程图，该方法包括：

S1、将行驶路径分割成预瞄点序列，即构成预瞄点路径。

在本发明实施例中，本发明中的行驶路径是基于环境拓扑地图构建的，基于激光叉车当前任务的起点位置及终点位置来规划行驶路径，路径的规划可以采用floyd算法来获取起点位置到目标位置的最短行驶路径，假定激光叉车需要从节点P2运行至节点P8，如图2所示，本发明中的行驶路径分割方法具体如下：

S11、提取行驶路径中的转折点，相邻两转折点之间构成一个路段；

在本发明实施例中，将图2中的转折点放入List1{P2,P3,P4,P5,P6,P7,P8}中，需要记录的信息为路径A(P2,P3)、B(P1,P4)、C(P4,P5)、D(P5,P6)、E(P6,P9)、F(P7,P8)

S12、将路段分割成子路段，保存子路段的端点坐标，形成端点路径；

在本发明实施例中，为了确保路径跟踪的精准性，需要将路段进行再次分割，路段的分割方法具体如下：

S121、计算路段的欧式距离，即两个转折点间的距离，欧式距离的计算公式如公式(1)所示；

其中，(P_i.x，P_i.y)为第i个转折点P_i的坐标，(P_i+1.x，P_i+1.y)为第i+1个转折点P_i+1的坐标,dist(P_i,P_i+1)为第i个转折点P_i到第i+1个转折点P_i+1间的欧式距离。

S122、若两转折点间的距离大于距离阈值，则按照设定的步长对路径进行分段，分割成子路段，若两转折点间的距离小于或等于距离阈值，则将路段等分成两个子路段，保存各子路段端点；

在本发明实施例中，基于公式(2)来对路段进行再分割，公式(2)如下所示：

其中，j为设定的步长，(P_i’.x，P_i’.y)为子路段的断点坐标。

S13、对端点路径进行平滑处理，平滑处理后的端点即为预瞄点。

在本发明实施例中，采用二阶Bezier函数模型对断点路径进行平滑处理，二阶Bezier函数模型如公式(3)所示：

B(t)＝(1-t)²P₀+2t(1-t)P₁+t²P₂t∈[0,1] (3)

其中，参数t在[0，1]范围内连续变，t的取值与子路段的截取步长相关，t的取值为步长与路段欧式距离的比值，基于二阶Bezier函数模型处理后的端点路径平滑性更好，如图3所示中的路径2所示，路径1为二阶Bezier函数模型处理前的端点路径，激光叉车基于路径2行驶，避免了在拐角处出现减速-停止-加速的过程。

S2、定时获取行驶方向上距激光导航AGV车身最近的预瞄点，称为目标预瞄点；

在本发明实施例中，如图4所示，在当前行驶方向上找出距激光导航AGV的车身最近的预瞄点(Gx,Gy)，预瞄点和中心点(Cx,Cy)之间的横向距离为横向位置偏差(l_d)，车辆当前行驶方向与期望行驶方向的夹角偏差(α)，中心点到预瞄点所形成的向量即表示期望行驶方向。

S3、控制激光叉车朝行目标预瞄点行驶。

在本发明实施例中，基于模糊PID控制器控制激光叉车朝目标预瞄点行驶，模糊PID控制器主要是由模糊控制器和PID控制器结合而成，模糊控制器以横向位置偏差(l_d)及车辆当前行驶方向与期望行驶方向的夹角偏差α作为输入，利用模糊算法对PID控制器的参数K_p，K_i，K_d进行自适应整定，使被控对象保持在良好的动、静态稳定状态。如图5所示，相比传统的PID控制，模糊自适应PID更加的灵活稳定，特别是对于时变性和非线性较大的被控对象，其优点更加突出。

传统的PID可用下面公式进行描述：

其中，e(k)为输入与设定值之间的偏差，从系统的稳定性、响应速度、超调量和稳态误差各方面来考虑，比例系数K_p的作用是加快系统响应速度，提高系统的调节精度；积分系数K_i的作用是消除稳态误差；微分系数K_d的作用是改善系统的动态特性，一直响应过程中偏差的变化。固定的K_p，K_i，K_d参数很难满足系统的整体要求，因此通过建立三个参数K_p，K_i，K_d三个参数与输入量之间的模糊关系，在车辆运行中通过不断检测输入量后，根据模糊控制原理来对ΔK_p，ΔK_i，ΔK_d三个参数进行在线调整，从而满足当输入量不同时，前向舵轮旋转的角度不同，使激光AGV具有良好的动态和静态性能。

将横向位置偏差(l_d)及车辆当前行驶方向与期望行驶方向的夹角偏差α作为输入，利用模糊控制算法在线对参数进行修改后得到K_p，K_i，K_d参数值，再通过PID控制器得出控制前轮偏角量，从而引导激光AGV朝目标预瞄点行进。

根据所建立的直角坐标系，可得如下公式：

l_d＝G_y-C_y

因此，通过模糊PID控制器计算得到的控制量实时改变舵轮的角度，提高了小车在直线或弯道环境中车辆行驶的平稳性。

在本发明实施例中，在步骤S1具之前还包括：

基于激光叉车的实际尺寸来构建激光叉车模型，并根据控制需求将此模型简化，得到非线性的车辆路径跟踪控制模型。其中车体两个定向轮之间的距离为W，舵轮距两万向轮所在直线的距离为L(如图6所示)，在构建地图时，以激光雷达的初始位置建立直角坐标系统，假设激光雷达安装的位置在舵轮的正上方，得到了激光叉车的位姿(x,y,θ)。激光叉车AGV采用的是前向舵轮转向，因此路径跟踪控制实际上是通过控制前轮转向实现对车体的行驶轨迹的跟踪，为了路径跟踪的准确性，需要将控制点设为两个定向轮的中心位置，该位置坐标为(x',y',θ)＝(x+L*cosθ,y+L*sinθ,θ)。

本发明提供的激光叉车路径跟踪方法具有如下有益技术效果：

1.行驶路径进行分割，分割成预瞄点序列，控制激光叉车朝当前行驶方向上最近的预瞄点行驶，将基于路径曲线的跟踪转变成基于目标预瞄点的跟踪，简化跟踪计算，因此可以用于复杂路径的跟踪。

2.利用Bezier函数对分割点进行平滑处理，激光叉车AGV在进行路径跟踪时，遇到拐角点无需在拐角点附近区域完成减速，停止再加速的过程，从而整体提高了作业效率。

3.为了提高车辆在直线或弯道环境中车辆行驶的平稳性，采用了模糊PID算法根据车体姿态与预瞄点的横向位置偏差及角度偏差实时动态调整PID参数后，输出前向舵轮的期望偏转角，该方法能够以较小的偏差跟踪设定的路线，提高AGV的整体运行效率，同时保证了AGV准确的到达预设的目标位置。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种激光叉车的路径跟踪方法，其特征在于，所述方法包括如下步骤：

S1、将行驶路径分割成预瞄点序列，即构成预瞄点路径；

S2、定时获取行驶方向上距激光导航AGV车身最近的预瞄点，称为目标预瞄点；

S3、控制激光叉车朝行目标预瞄点行驶。

2.如权利要求1所述激光叉车的路径跟踪方法，其特征在于，所述步骤S1具体包括如下步骤：

S11、提取行驶路径中的转折点，相邻两转折点之间构成一个路段；

S12、将路段分割成子路段，保存子路段的端点坐标，形成端点路径；

S13、对端点路径进行平滑处理，平滑处理后的端点即为预瞄点。

3.如权利要求2所述激光叉车的路径跟踪方法，其特征在于，所述步骤S12包括如下步骤：

S121.计算路段的欧式距离，即两个转折点间的距离；

S122、若两转折点间的距离大于距离阈值，则按照设定的步长对路径进行分段，分割成子路段，若两转折点间的距离小于或等于距离阈值，则将路段等分成两个子路段，保存各子路段端点。

4.如权利要求1所述激光叉车的路径跟踪方法，其特征在于，基于模糊PID控制器控制激光叉车朝目标预瞄点行驶，即将横向位置偏差l_d及车辆当前行驶方向与期望行驶方向的夹角偏差α作为输入模糊控制器，利用模糊控制算法在线对参数进行修改后得到K_p，K_i，K_d参数值，再通过PID控制器得出控制前轮偏角量，从而引导激光AGV朝目标预瞄点行进。

5.如权利要求1所述激光叉车的路径跟踪方法，其特征在于，在步骤S1之前还包括：

基于激光叉车的实际尺寸来构建激光叉车模型，即定义两万向轮之间的距离，舵轮距两万向轮所在直线的距离，激光雷达安装于舵轮的正上方，两个万向轮的中心位置为前转向轮的控制点及激光叉车的中心。