[发明专利]一种基于GNSS的农机泊车路径规划与跟踪控制方法

权利要求书

1.一种基于GNSS的农机泊车路径规划与跟踪控制方法，其特征在于，包括：

获取车辆位置信息和泊车场景信息；

使用泊车路径规划器根据车辆位置信息和泊车场景信息规划出最优路径；

使用泊车控制器控制农机按照最优路径到达预期位置。

2.根据权利要求1所述的一种基于GNSS的农机泊车路径规划与跟踪控制方法，其特征在于，所述使用泊车路径规划器根据车辆位置信息和泊车场景信息规划出最优路径包括：

泊车路径规划器参数初始化；

泊车路径离散点采样；

曲线连接离散点获得初始路径；

检测初始路径是否有障碍物碰撞，若有障碍物碰撞则重新规划初始路径，若没有障碍物碰撞则进行下一步；

对初始路径进行平滑优化。

3.根据权利要求1所述的一种基于GNSS的农机泊车路径规划与跟踪控制方法，其特征在于，所述使用泊车控制器控制农机按照最优路径到达预期位置包括：

使用前馈控制器控制农机前行时转弯；

使用反馈控制器实时纠正农机位姿。

4.根据权利要求2所述的一种基于GNSS的农机泊车路径规划与跟踪控制方法，其特征在于，所述泊车路径离散点采样包括：

泊车区域采样包括目标偏置采样和转角约束采样；

给定固定的目标偏置概率P0(P01)，生成随机数P(P1)，若PP0则将产生的随机点设置为目标点；

采用高斯分布采样限制采样的方向与范围：

x,y为本次采样路径坐标，x_s，y_s为上一次采样路径点坐标，θ为本次采样航向，θ_s为上一次采样航向，ρ为每次采样长度，MSA为最大航向角约束，step为采样步长，N(0,1)为均值0方差为1的高斯分布，U(0,1)为在0到1范围内的均匀分布。

5.根据权利要求2所述的一种基于GNSS的农机泊车路径规划与跟踪控制方法，其特征在于，所述检测初始路径是否有障碍物碰撞，若有障碍物碰撞则重新规划初始路径，若没有障碍物碰撞则进行下一步包括：

其中障碍物与农机的碰撞描述使用OBB有向包围盒算法：对于任意的2个凸多边形物体，若能找到任意一个分离轴使这2个物体在该轴上的投影不发生重叠，则判定2个物体之间不发生碰撞；

将农机和障碍物描述为有向包围盒，在采样过程中作为碰撞检测的依据，如果检测到有障碍物碰撞则重新规划初始路径，如果没有检测到障碍物碰撞则进行下一步。