[发明专利]拖拉机自动驾驶系统的局部引导轨迹规划方法及装置

权利要求书

1.一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其特征在于，包含：

步骤1：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；

步骤2：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；

步骤3：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹；

其中，所述步骤1包含：

步骤11：依据局部引导轨迹的初始点的航向角约束，选定控制点P₀、P₁、P₂，其中所述控制点P₁为拖拉机的初始位置，所述控制点P₀、P₁、P₂三点共线且所述控制点P₀和P₂关于所述控制点P₁对称，所述控制点P₀和P₂的矢量P₀P₂的方向角与拖拉机初始航向一致；

步骤12：依据全局路径起始作业点航向约束，选定控制点P₃、P₄、P₅，其中所述控制点P₄为全局路径起始作业点位置，所述控制点P₃、P₄、P₅三点共线且所述控制点P3和P₅关于P₄对称；

步骤13：设定所述控制点P₁及所述控制点P₄的坐标参数分别为(x₁，y₁)和(x₄，y₄)；

步骤14：基于所述B样条曲线与所述六个控制点P₀、P₁、P₂、P₃、P₄、P₅间的几何关系获得其余所述控制点P₀、P₂、P₃、P₅的坐标参数分别为P₀(x₁-l₁cosθ_v，y₁-l₁sinθ_v)，P₂(x₁+l₁cosθ_v，y₁+l₁sinθ_v)，P₃(x₄-l₂cosθ_t，y₄-l₂sinθ_t)，P₅(x₄-l₂cosθ_t，y₄-l₂sinθ_t)，其中l₁为控制点P₀与控制点P₁、控制点P₁与控制点P₂之间的长度，l₂为控制点P₃与控制点P₄、控制点P₄与控制点P₅之间的长度，θ_v为所述控制点P₀和P₂的矢量P₀P₂的方向角，θ_t为所述控制点P₃和P₅的矢量P₃P₅的方向角；

其中，所述步骤3包含：

步骤301：初始化种群Q(t)；

步骤302：对种群Q(t)中的每个个体的l₁及l₂的长度进行一次测量，得到对应的所述局部引导轨迹的长度；

步骤303：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；

步骤304：记录最优的l₁及l₂数值和对应的适应度，以最优的l₁及l₂对应的个体作为下一代进化目标；

步骤305：判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群，继续执行所述步骤302。