[发明专利]一种前轮快速航向修正方法及装置

说明书

本发明公开了一种前轮快速航向修正方法及装置，所述方法包括以下步骤：计算偏航角并追加比例系数对偏航角进行加权；获取前轮转向控制公式，对前轮进行转向控制，并对偏航角进行PID调节；获取偏航系数，所述偏航系数代表车辆当前运动轨迹相比目标路径的偏离程度；根据偏航系数，动态调节PID参数；本发明的优点在于：实现对前轮较为精准的转向控制，跟踪控制结果较为准确。

技术领域

本发明涉及农业机器人、农机路径跟随领域，更具体涉及一种前轮快速航向修正方法及装置。

背景技术

基于单车模型和纯追踪算法来控制车辆进行路径跟踪是最为常见的算法之一，纯追踪的原理图如图1和图2所示，δ表示前轮转角，α表示偏航角，l_d表示车辆后轮到目标路径点的距离，L表示前轮和后轮之间的轴距，R表示转向半径，算法的核心思想是通过前轮实时调整角度，保证后轮贴合在轨迹上。但是该算法在横向控制方面存在严重的缺陷。

1、转向不足问题。当预瞄距离较短时，车辆转弯会存在明显的延迟。核心是偏航角作为计算参数，在转向控制上有不足，具有一定延迟性。

2、横向误差收敛慢。前视距离的设置不当，也会导致车身在运动过程中，产生明显的震荡(蛇行)。传统的PID参数调节，实时性差，而且收敛时间长，跟踪控制结果准确性差。PID参数的拟合也十分复杂，对于农机来说，复杂多变的地貌条件决定不可能找出一套标准的PID参数适应全地形。

中国专利授权公告号CN105867377B，公开了一种农业机械自动导航控制方法，该方法包括如下步骤：(1)GPS接收机实时测得当前时刻农业机械的运动状态数据；(2)采用预瞄控制方法，将运动状态数据与预定义路径数据进行比对分析，推算出横向偏差和航向偏差；(3)通过模糊控制算法进行模糊化，然后将模糊量导入模糊控制器中，得到前轮转角；通过改进型纯追踪算法建立几何模型，从而导求出前轮转角；将两种算法得到的前轮转角合成一个前轮转角；(4)最后根据前轮转角实时对农业机械进行控制，实现路径跟踪。该发明由两种导航方法结合在一起实现自动控制，是稳定性良好，抗干扰能力强，能够提高系统的鲁棒性。两种方法的结合，优势互补，使其满足旱地和水田的作业需要。但是该发明没有前轮转角的参数进行动态调整，从而在复杂的农机环境下，车身震荡而跟踪控制结果不够准确。

发明内容

本发明所要解决的技术问题在于现有技术农机前轮航向控制由于复杂的地形条件导致震荡幅度大，偏离航向而导致跟踪控制结果不够准确。

本发明通过以下技术手段实现解决上述技术问题的：一种前轮快速航向修正方法，所述方法包括以下步骤：

步骤一：计算偏航角并追加比例系数对偏航角进行加权；

步骤二：利用公式对前轮进行转向控制，其中，α表示加权后的偏航角，l_d表示车辆后轮到目标路径点的距离，L表示前轮和后轮之间的轴距，pid(α)表示对加权后的偏航角进行PID调节；

步骤三：获取偏航系数，所述偏航系数代表车辆当前运动轨迹相比目标路径的偏离程度；

步骤四：根据偏航系数，动态调节PID参数。

本发明获取偏航系数，所述偏航系数代表车辆当前运动轨迹相比目标路径的偏离程度，根据偏航系数，动态调节PID参数，然后对加权后的偏航角实时进行PID调节，从而实现对前轮较为精准的转向控制。

进一步地，所述步骤一包括：

通过公式α′＝α1+α2计算偏航角，其中，α1表示车身与目标点的夹角，α2表示目标点处切线和前轮车轴的夹角；

通过公式α＝β·α1+δ·α2对偏航角追加比例系数得到加权后的偏航角，其中，β表示车身与目标点的夹角的系数，δ表示目标点处切线和前轮车轴的夹角的系数，β+δ＝1。

进一步地，所述步骤三包括：