一种改进的用于地面车辆路径跟踪的纯追踪控制方法与流程

本发明涉及地面车辆导航控制领域，尤其涉及一种改进的用于地面车辆路径跟踪的纯追踪控制方法。

背景技术：

近年来随着自动控制技术、导航技术和传感探测技术的快速发展，地面车辆自动驾驶技术得到了长足进步。在商业和农业领域，车辆自动驾驶技术能够解放人力，提高通行或作业效率。地面车辆自动驾驶的关键在于两个方面，车辆位置的精确测量和控制。其中车辆位置的精确测量可通过现代导航技术完成，已可实现厘米级定位测量，而车辆位置的精确控制还存在一定的挑战。车辆位置精确控制的关键在于路径跟踪控制，目前已有的路径跟踪控制方法主要包括pid控制、模糊控制、最优控制和纯追踪控制。pid控制基于误差生成控制策略，不依赖具体数学模型，应用广泛，但控制参数整定困难，需要一定的经验和大量实验；模糊控制不依赖车辆模型，但需要专家的经验制定控制规则，跟踪误差大且难以快速修正；最优控制依赖精确的车辆运动学和动力学模型，针对不同车辆行驶的不同路况，车辆运动学和动力学模型鲁棒性和适应性较差；纯追踪方法是一种几何方法，控制参数少，算法设计模拟人的驾驶行为，具有预见性，但前视距离的影响较大，即使使用动态调整方法进行自适应选择，在前轮期望转角较大时，仍会出现调节过度导致路径跟踪误差震荡的出现。

公开号为cn105867377a的发明公开了一种农业机械自动导航控制方法，该方法考虑了车速和规划路径的弯度，并基于此实现前视距离的动态调整，然后将模糊控制与纯追踪算法相结合得到前轮期望转角，实现农业机械的路径跟踪控制。该方法没有考虑前轮期望转角较大会导致路径跟踪误差会出现震荡的情况，在期望转角较大时难以实现规划路径的稳定准确跟踪。为解决上述问题，陈宁等(参见陈宁等,一种纯追踪模型改进算法,轻工机械,2014,32(4):69-72.)在分析纯追踪模型算法误差原理的基础上，提出了一种改进的纯追踪算法，将路径跟踪分解为三步，并分别计算出了每步的前轮期望转角。该方法能够在一定程度上提高路径跟踪精度，但该方法在计算每步的前轮期望转角时通过简化模型和近似获得，所得参数较为粗略，无法根据实际行驶情况进行动态调整，而且所得参数也是针对特性情况进行的优化，缺乏通用性。

技术实现要素：

为了克服上述技术方法的缺点，本发明的目的在于提供一种改进的用于地面车辆路径跟踪的纯追踪控制方法。该方法综合车辆横向偏差的正负和车辆航向偏差与理想直线校正航向角之间的关系选择合适的路径跟踪方法，实现稳定性好、精度高的路径跟踪控制。

为了达到上述目的，本发明所采用的技术方案如下：

一种改进的用于地面车辆路径跟踪的纯追踪控制方法，该方法根据车辆导航系统提供的导航参数，判断航向偏差与理想直线校正航向角之间的关系，并结合横向偏差的正负，选择采用小圆弧路径跟踪算法还是大圆弧路径跟踪算法：以田块任意垂直的两条边界为x和y轴，以两边界交点为原点o建立田块平面坐标系xoy，车辆当前位置为c(xc,yc)、期望位置为c’(xc’,yc’)、第一预瞄点为p(xp,yp)、第二预瞄点为p’(xp’,yp’)，c’p为规划路径(从c’指向p为规划路径正方向)，当前位置c与期望位置c’的横坐标差值为横向偏差δx，当前位置c相比于期望位置c’偏左时δx为负值，车辆当前位置c相比于期望位置c’偏右时δx为正值；线段cp为车辆轨迹的理想直线校正航向，正方向从c指向p，cp正方向与规划路径c’p正方向所成夹角为理想直线校正航向角，cp正方向相对于c’p正方向偏左时理想直线校正航向角为负值，偏右时理想直线校正航向角为正值；车辆实时航向为ce，正方向从c指向e，ce正方向与规划路径c’p正方向所成夹角为航向偏差δθ，ce正方向相对于c’p正方向偏左时δθ为负值，偏右时δθ为正值；圆弧cp为弧线校正路径；当车辆航向偏差δθ大于理想直线校正航向角(即ce在cp右侧)且横向偏差为正时，采用小圆弧路径跟踪算法；当车辆航向偏差δθ大于理想直线校正航向角(即ce在cp右侧)且横向偏差为负时，采用大圆弧路径跟踪算法；当车辆航向偏差δθ小于理想直线校正航向角(即ce在cp左侧)且横向偏差为负时，采用小圆弧路径跟踪算法；车辆航向偏差δθ小于理想直线校正航向角(即ce在cp左侧)且横向偏差为正时，采用大圆弧路径跟踪算法；当车辆航向偏差δθ等于理想直线校正航向角时，方向盘回正，按当前航向角行驶。

进一步的，所述的小圆弧路径跟踪算法通过以下步骤进行实现与修正：

(1)每个控制周期为1个外循环大周期，1个外循环大周期包含多个内循环小周期，内循环次数默认设置为4，并可根据实际需要进行调整；

(2)在新的外循环周期内得到第一组导航定位数据，确定第一预瞄点p点坐标，计算小圆弧所对应的圆心半径，并换算为前轮期望转角；

(3)按前轮期望转角控制车辆前轮转向，车辆开始沿cp圆弧航线行驶；

(4)在内循环小周期①内监测车辆航向偏差δθ和横向偏差δx，如果δθ*δx>0，则继续沿cp圆弧航线行驶，同时判断车辆位置在cp圆弧的内侧还是外侧，并根据判断结果对前轮转向进行小幅度调整校正车辆实际轨迹；如果δθ*δx≤0，则进入内循环小周期②；

(5)在内循环小周期②内，将预瞄点由第一预瞄点p点改为第二预瞄点p’点，并判断车辆实时航向ce与新的理想直线校正航向cp’的关系和横向偏差δx的正负，如果ce在cp’左侧且δx<0，或ce在cp’右侧且δx>0，则继续沿cp圆弧航向行驶；如果ce在cp’左侧且δx>0，或ce在cp’右侧且δx<0，则进入内循环小周期③；

(6)在内循环小周期③内，控制前轮回正，车辆按照新的理想直线航向cp’行驶，并根据车辆实时位置与cp’的相对位置关系对其实际轨迹进行小幅度调整；

(7)重复以上步骤(4)～(6)，直至外循环大周期结束。

进一步的，所述的大圆弧路径跟踪算法通过以下步骤进行实现：

(1)每个控制周期为1个外循环，1个外循环大周期包含多个内循环小周期，内循环次数默认设置为4，并可根据实际需要进行调整；

(2)在新的外循环大周期内得到第一组导航定位数据，确定第一预瞄点p，计算大圆弧所对应的圆心半径，并换算为前轮期望转角；

(3)按前轮期望转角控制车辆前轮转向，车辆开始沿cp圆弧航线行驶；

(4)在内循环小周期①内监测车辆航向偏差δθ和横向偏差δx，如果δθ*δx<0，则继续沿cp圆弧航线行驶，同时判断车辆位置在cp圆弧的内侧还是外侧，并根据判断结果对前轮转向进行小幅度调整校正车辆实际轨迹；如果δθ*δx≥0，则进入内循环小周期②；

(5)在内循环小周期②内，控制车辆前轮回正，直接按0°航向角继续行驶，并根据车辆实时位置对其实际轨迹进行小幅调整；

(6)重复以上步骤(4)～(5)，直至外循环大周期结束。

所述的预瞄点确定方法为：前视距离可以为预设值或通过考虑车辆速度和规划路径弯度的动态计算方法获得，在规划路径上搜索离车辆当前位置最近的点，以此为起点向前截取前视距离得到第一预瞄点p，向前截取2倍前视距离得到第二预瞄点p’。

本发明的有益效果是：

本发明对纯追踪算法进行了改进，综合考虑车辆横向偏差的正负和车辆航向偏差与理想直线校正航向角之间的关系选择合适的路径跟踪方法，可根据车辆实时运动状态对车辆前进方向进行动态调整，尤其能够解决前轮期望转角较大时过度调节的情况，能够实现稳定性好、精度高的路径跟踪控制。

附图说明

图1a是当δx>0且ce在cp右侧时小圆弧路径跟踪算法原理图；

图1b是当δx<0且ce在cp左侧时小圆弧路径跟踪算法原理图；

图2a是当δx>0且ce在cp左侧时大圆弧路径跟踪算法原理图；

图2b是当δx<0且ce在cp右侧时大圆弧路径跟踪算法原理图；

图3是小圆弧路径跟踪算法流程图；

图4是大圆弧路径跟踪算法流程图。

具体实施方式

为对本发明有进一步的了解，下面结合说明书附图和实施例对本发明作进一步说明，但本发明并不局限于以下实施例。

结合附图1a、1b、图2a、2b所示，以田块任意垂直的两条边界为x和y轴，以两边界交点为原点o建立田块平面坐标系xoy，车辆当前位置为c(xc,yc)、期望位置为c’(xc’,yc’)、第一预瞄点为p(xp,yp)、第二预瞄点为p’(xp’,yp’)，c’p为规划路径(从c’指向p为规划路径正方向)，当前位置c与期望位置c’的横坐标差值为横向偏差δx，当前位置c相比于期望位置c’偏左时δx为负值，车辆当前位置c相比于期望位置c’偏右时δx为正值；线段cp为车辆轨迹的理想直线校正航向(从c指向p为理想直线校正航向为正方向)，cp正方向与规划路径c’p正方向所成夹角为理想直线校正航向角，cp正方向相对于c’p正方向偏左时理想直线校正航向角为负值，偏右时理想直线校正航向角为正值；车辆实时航向为ce(从c指向e为车辆实时航向正方向)，ce正方向与规划路径c’p正方向所成夹角为航向偏差δθ，ce正方向相对于c’p正方向偏左时δθ为负值，偏右时δθ为正值；圆弧cp为弧线校正路径。

一种改进的用于地面车辆路径跟踪的纯追踪控制方法，其特征在于，该方法根据车辆导航系统提供的导航参数，判断车辆航向偏差与理想直线校正航向角之间的关系，并结合横向偏差的正负，选择采用小圆弧路径跟踪算法还是大圆弧路径跟踪算法：当ce在cp右侧且δx>0时，采用小圆弧路径跟踪算法；当ce在cp右侧且δx<0时，采用大圆弧路径跟踪算法；当ce在cp左侧且δx<0时，采用小圆弧路径跟踪算法；当ce在cp左侧且δx>0时，采用大圆弧路径跟踪算法；当航向偏差等于理想直线校正航向角时，方向盘回正，按当前航向角行驶。

结合附图1a、1b、图3所示，虽然车辆沿圆弧cp行驶可以到达第一预瞄点p点，但在p点附近的航向偏差太大，通过p点后必然出现过调节导致横向偏差增大，所以必须在p点之前航向偏差还不太大时就提前开始p点之后的第二预瞄点p’，并逐渐偏离圆弧cp而向第二预瞄点p’行驶。因此，小圆弧路径跟踪算法通过以下步骤进行实现与修正：

(1)每个控制周期为1个外循环大周期，1个外循环大周期包含多个内循环小周期，内循环次数默认设置为4，并可根据实际需要进行调整；

(2)在新的外循环周期内得到第一组导航定位数据，确定第一预瞄点p点坐标，计算小圆弧所对应的圆心半径，并换算为前轮期望转角；

(3)按前轮期望转角控制车辆前轮转向，车辆开始沿cp圆弧航线行驶；

(4)在内循环小周期①内监测车辆航向偏差δθ和横向偏差δx，如果δθ*δx>0，则继续沿cp圆弧航线行驶，同时判断车辆位置在cp圆弧的内侧还是外侧，并根据判断结果对前轮转向进行小幅度调整校正车辆实际轨迹；如果δθ*δx≤0，则进入内循环小周期②；

(5)在内循环小周期②内，将预瞄点由p点改为下一预瞄点p’，并判断车辆实时航向ce与新的理想直线校正航向cp’的关系和横向偏差δx的正负，如果ce在cp’左侧且δx<0，或ce在cp’右侧且δx>0，则继续沿cp圆弧航向行驶；如果ce在cp’左侧且δx>0，或ce在cp’右侧且δx<0，则进入内循环小周期③；

(6)在内循环小周期③内，控制前轮回正，车辆按照新的理想直线航向cp’行驶，并根据车辆实时位置与cp’的相对位置关系对其实际轨迹进行小幅度调整；

(7)重复以上步骤(4)～(6)，直至外循环大周期结束。

结合图2a、2b、图4所示，由于大圆弧轨迹在第一预瞄点p点处与规划路径偏离不大，无需进行再次修正，大圆弧路径跟踪算法通过以下步骤进行实现：

(1)每个控制周期为1个外循环，1个外循环大周期包含多个内循环小周期，内循环次数默认设置为4，并可根据实际需要进行调整；

(2)在新的外循环大周期内得到第一组导航定位数据，确定第一预瞄点p点坐标，计算大圆弧所对应的圆心半径，并换算为前轮期望转角；

(3)按前轮期望转角控制车辆前轮转向，车辆开始沿cp圆弧航线行驶；

(4)在内循环小周期①内监测车辆航向偏差δθ和横向偏差δx，如果δθ*δx<0，则继续沿cp圆弧航线行驶，同时判断车辆位置在cp圆弧的内侧还是外侧，并根据判断结果对前轮转向进行小幅度调整校正车辆实际轨迹；如果δθ*δx≥0，则进入内循环小周期②；

(5)在内循环小周期②内，控制车辆前轮回正，直接按0°航向角继续行驶，并根据车辆实时位置对其实际轨迹进行小幅调整；

(6)重复以上步骤(4)～(5)，直至外循环大周期结束。

所述的预瞄点确定方法为：前视距离可以为预设值或通过考虑车辆速度和规划路径弯度的动态计算方法获得，在规划路径上搜索离车辆当前位置最近的点，以此为起点向前截取前视距离得到第一预瞄点p，向前截取2倍前视距离得到第二预瞄点p’；前视距离预设值通过公式l＝t*v计算，其中l为前视距离，t为控制外循环大周期时间，v为车辆行驶速度；前视距离动态计算方法为l＝lp+k1v+k2ω，其中lp为前视距离的基数，k1为速度系数，数值为正，k2为弯度系数，数值为负，ω为车辆规划路径弯度。

上述具体实施方式用来解释说明本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明作出的任何修改和改变，都落入本发明的保护范围。