CN109933057A - 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置 - Google Patents

Abstract

本发明公开了一种拖拉机自动驾驶系统的局部引导轨迹规划方法及装置，局部引导轨迹规划方法包含：步骤1：设取局部引导轨迹为B样条曲线，基于B样条曲线设定局部引导轨迹的六个控制点的坐标参数；步骤2：根据六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；步骤3：基于量子遗传算法对坐标参数进行优化以得到最优的局部引导轨迹。

Description

拖拉机自动驾驶系统的局部引导轨迹规划方法及装置

技术领域

本发明属于农业车辆自动驾驶技术领域，尤其涉及一种拖拉机自动驾驶系统局部引导轨迹规划方法及装置。

背景技术

拖拉机自动驾驶系统是当前农业机械智能化领域不可或缺的一部分，具备自主行驶能力的农业车辆不仅有助于提高劳动生产率，而且有利于解决农村劳动力不足等问题。

路径规划算法是拖拉机自动驾驶系统的关键技术，当前学术界的研究热点大多集中在田间全局路径规划上，自动驾驶系统跟踪全局路径需要经过两个阶段:1)从车辆当前位置出发逼近全局路径上的起始作业点，该过程中拖拉机行驶过的轨迹称为引导轨迹；2)从全局路径的起始作业点行驶至全局路径目标点。现有的自动驾驶系统主要依靠内置的路径跟踪算法完成第1阶段，由于缺少引导轨迹规划方法，导致引导轨迹距离较长，且在全局路径起始作业点处会出现震荡现象，跟踪误差较大。因此，拖拉机驾驶员为了提高作业精度，在第1阶段只能依靠手动驾驶，自动化程序较低。因此急需开发一种克服上述缺陷的拖拉机自动驾驶系统局部引导轨迹规划方法及装置

发明内容

本发明所要解决的技术问题在于提供一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其中，包含：

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

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

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

上述的局部引导轨迹规划方法，其中，所述步骤1包含：

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

步骤12：依据全局路径L起始作业点航向约束，选定控制点P₃、P₄、P₅，其中所述控制点P₄为全局路径L起始作业点位置，所述控制点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₅的方向角。

上述的局部引导轨迹规划方法，其中，于所述步骤2中，将所述六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

上述的局部引导轨迹规划方法，其中，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

上述的局部引导轨迹规划方法，其中，所述步骤3包含：

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

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

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

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

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

上述的局部引导轨迹规划方法，其中，所述步骤303包含：

步骤3031：构建所述局部引导轨迹上每一节点的特征参数表达式；

步骤3032：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；

步骤3033：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；

步骤3034：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

上述的局部引导轨迹规划方法，其中，于步骤3031中，建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝f_x(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中所述特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式： 为前轮转向角度，L为拖拉机的轴距。

上述的局部引导轨迹规划方法，其中，步骤3032中，所述约束模型包含：曲率约束模型：为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：

上述的局部引导轨迹规划方法，其中，步骤3033中，根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：

本发明还提供一种拖拉机自动驾驶系统的局部引导轨迹规划装置，其中，包含：

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

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

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

上述的局部引导轨迹规划装置，其中，所述坐标参数设定单元包含：

控制点选定模块：依据局部引导轨迹的初始点的航向角约束，选定控制点P₀、P₁、P₂，其中所述控制点P₁为拖拉机的初始位置，所述控制点P₀、P₁、P₂三点共线且所述控制点P₀和P₂关于所述控制点P₁对称，所述控制点P₀和P₂的矢量P₀P₂的方向角与拖拉机初始航向一致；依据全局路径起始作业点航向约束，选定控制点P₃、P₄、P₅，其中所述控制点P₄为全局路径起始作业点位置，所述控制点P₃、P₄、P₅三点共线且所述控制点P3和P₅关于P₄对称；

坐标参数获得模块：设定所述控制点P₁及所述控制点P₄的坐标参数分别为(x₁，y₁)和(x₄，y₄)；基于所述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₅的方向角。

上述的局部引导轨迹规划装置，其中，所述轨迹函数获得单元将所述六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

上述的局部引导轨迹规划装置，其中，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

上述的局部引导轨迹规划装置，其中，所述坐标参数优化单元包含：

初始化模块：初始化种群Q(t)；

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

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

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

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

上述的局部引导轨迹规划装置，其中，所述评估模块包含：

特征参数表达式获得模块：构建所述局部引导轨迹上每一节点的特征参数表达式；

约束模型获得模块：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；

计算模块：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；

标记模块：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

上述的局部引导轨迹规划装置，其中，所述特征参数表达式获得模块建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝f_x(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中所述特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式：为前轮转向角度，L为拖拉机的轴距。

上述的局部引导轨迹规划装置，其中，所述约束模型包含：曲率约束模型：为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：

上述的局部引导轨迹规划装置，其中，所述计算模块步根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：

本发明针对于现有技术其功效在于，本发明在现有研究基础上基于量子遗传算法和B样条理论，提出了一种拖拉机自动驾驶系统的局部引导轨迹规划方法及装置，通过本发明的局部引导轨迹规划方法及装置以解决现有拖拉机自动驾驶系统引导轨迹距离较长，且在全局路径起始作业点附近跟踪误差较大的问题，进而减少局部引导轨迹距离，提高全局路径导航控制精度。

附图说明

图1为本发明局部引导轨迹规划方法的流程图；

图2为图1中步骤1的分步骤流程图；

图3为图1中步骤3的分步骤流程图；

图4为图3中步骤303的分步骤流程图；

图5为本发明引导轨迹示意图；

图6为本发明二轮车模型示意图；

图7为本发明起止点航向约束与B样条控制点几何关系示意图；

图8为本发明局部引导轨迹规划装置的结构示意图。

具体实施方式

兹有关本发明的详细内容及技术说明，现以一较佳实施例来作进一步说明，但不应被解释为本发明实施的限制。

请参照图1至图7，图1为本发明局部引导轨迹规划方法的流程图；图2为图1中步骤1的分步骤流程图；图3为图1中步骤3的分步骤流程图；图4为图3中步骤303的分步骤流程图；图5为本发明引导轨迹示意图；图6为本发明二轮车模型示意图；图7为本发明起止点航向约束与B样条控制点几何关系示意图。如图1-7所示，本发明的微局部引导轨迹规划方法包含：

步骤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₅的方向角。

值得注意的是，当样条曲线的控制点较少时不易求得满足约束的期望路径，控制点过多则会造成方程求解困难，增加运算时间。本实施例采用六个控制点P₀～P₅生成满足约束条件的三阶B样条引导轨迹。

现有文献大多采用优化算法直接求取N个控制点，对于6个控制点P(x，y)需要优化的参数为12个，使用迭代方法求解时运算量非常大。为了提高轨迹规划速度，有必要研究曲线约束与控制点间的关系以简化计算。对于三阶B样条曲线，当三个相邻的控制点在同一条直线上且间距相等时，生成的B样条曲线与直线相切于中间的控制点，如图7所示。

依据引导轨迹初始点的航向角约束，选择控制点P₀,P₁,P₂。当三个控制点满足如下条件，即可满足拖拉机初始状态约束：

1)选定拖拉机初始位置为P₁；

2)P₀,P₁,P₂三点共线；

3)矢量P₀P₂的方向角与拖拉机初始航向一致；

4)P₀和P₂关于P₁对称。

同理，依据全局路径起始作业点航向约束，确定控制点P₃,P₄,P₅，并选定P₄为全局路径起始作业点位置坐标。

设线段P₀P₁和P₁P₂的长度为l₁，线段P₃P₄和P₄P₅的长度为l₂。依据起止点航向约束选定控制点P₁和P₄后，只要确定l₁和l₂的数值即可求得其它控制点并生成引导轨迹，这样便把最优轨迹规划问题转变为求l₁和l₂的最优解问题，相比求解12个参数，该方法只需要优化2个参数，有效减少了算法的运算量。再利用P₁及P₄的的坐标参数与l₁和l₂，计算得到其余控制点的坐标参数，其中拖拉机初始位置P₁(x₁，y₁)和全局路径起始作业点P₄(x₄，y₄)已知，其余四个控制点的坐标参数分别为：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)。

又进一步地，于步骤2中，将六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

在本实施例中，当B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

再进一步地，步骤3中采用量子遗传算法求解l₁和l₂，也可采用遗传算法、粒子群算法等方法进行求解。量子遗传算法(quantum genetic algorithm，QGA)是遗传算法和量子计算结合产生的一种新的概率进化算法，比传统遗传算法具有更快的收敛速度和并行处理能力。

将l₁、l₂作为个体进行寻优，本实施例把引导轨迹距离最短作为优化目标，实际应用时还可以将轨迹最平滑等参数作为优化目标。将引导轨迹距离取负值作为适应度函数(轨迹越短，适应度值越大)，步骤3按照以下算法流程循环执行直到达到结束条件：

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

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

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

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

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

更进一步地，步骤303包含：

步骤3031：构建局部引导轨迹上每轨迹点的特征参数表达式；

步骤3032：根据拖拉机自身运动学或动力学限制条件建立约束模型；

步骤3033：根据轨迹函数计算局部引导轨迹上各轨迹点的特征参数；

步骤3034：通过约束模型对每一轨迹点的特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

其中，于步骤3031中，建立引导轨迹方程，引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝f_x(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中，特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式：为前轮转向角度，L为拖拉机的轴距。

其中，步骤3032中，为保证规划的引导轨迹具有可跟踪性，实际实施过程中需要依据拖拉机的自身运动学或动力学限制条件建立引导轨迹约束模型。约束模型包含：曲率约束模型：为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：本实施例基于车辆运动学特征构建上述约束模型，以下说明构建约束模型的具体方式：

一、构建曲率约束模型：

拖拉机曲率与转弯半径的关系为

其中R为转弯半径；

拖拉机最小转弯半径主要受轴距和转向前轮的最大转向角限制，其数值是固定的，计算公式为：

式中R_min为最小转弯半径，为为前轮最大转角的角度值；因此基于曲率特征参数表达式及上述公式1和2获得曲率约束模型：

二、构建初始航向约束模型：

为避免拖拉机在引导轨迹的起始时刻出现原地转向或震荡现象，需要对引导轨迹的初始航向进行约束，建立如下约束条件：

|θ_s-θ_v|≤ε (3)

式中θ_s为路径航向，θ_v为车身原始航向，ε为小正数。

三、构建起始作业点处的路径跟踪精度约束模型：

为了保证拖拉机引导轨迹与全局路径结合处的平滑度，提高全局路径起始作业点S_start处的路径跟踪精度，建立如下约束条件：

|θ_s-θ_t|≤ε (4)

式中θ_t为全局路径起始作业点航向。

四、构建转向角度范围约束模型：

拖拉机转向轮角度范围决定了其路径跟踪的灵活性，如果引导轨迹某点处等效转角过大，该点处的路径将不具备可跟踪性。因此，需要对等效前轮转角进行约束。由曲率特征参数表达式及前轮转向角度特征参数表达式可建立等效前轮转角约束模型如下：

其中，步骤3033中，根据轨迹函数的1阶导数及2阶导数及特征参数表达式获得每一轨迹点的特征参数，其中轨迹函数的1阶导数及2阶导数分别为：

将公式6和7代入特征参数表达式可求解得到局部引导轨迹上各个轨迹点的曲率κ、航向角θ和等效前轮转角

请参照图8，图8为本发明局部引导轨迹规划装置的结构示意图。如图8所示，本发明的部引导轨迹规划装置包含：坐标参数设定单元11、轨迹函数获得单元12及坐标参数优化单元13；坐标参数设定单元11设取局部引导轨迹为B样条曲线，基于B样条曲线设定局部引导轨迹的六个控制点的坐标参数；轨迹函数获得单元12根据六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；坐标参数优化单元13基于量子遗传算法对坐标参数进行优化以得到最优的局部引导轨迹。

进一步地，坐标参数设定单元11包含：控制点选定模块111及坐标参数获得模块112；控制点选定模块111依据局部引导轨迹的初始点的航向角约束，选定控制点P₀、P₁、P₂，其中控制点P₁为拖拉机的初始位置，控制点P₀、P₁、P₂三点共线且控制点P₀和P₂关于控制点P₁对称，控制点P₀和P₂的矢量P₀P₂的方向角与拖拉机初始航向一致；依据全局路径起始作业点航向约束，选定控制点P₃、P₄、P₅，其中控制点P₄为全局路径起始作业点位置，控制点P₃、P₄、P₅三点共线且所述控制点P3和P₅关于P₄对称；坐标参数获得模块112设定控制点P₁及控制点P₄的坐标参数分别为(x₁，y₁)和(x₄，y₄)；基于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₅的方向角。

又进一步地，轨迹函数获得单元12将六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

其中，当B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

再进一步地，坐标参数优化单元13包含：初始化模块131、测量模块132、评估模块133、记录模块134及判断模块135；初始化模块131初始化种群Q(t)；测量模块132对种群Q(t)中的每个个体的l₁及l₂的长度进行一次测量，得到对应的局部引导轨迹的长度；评估模块133对各个局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；记录模块134记录最优的l₁及l₂数值和对应的适应度，以最优的l₁及l₂对应的个体作为下一代进化目标；判断模块135判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群。

更进一步地，评估模块133包含：特征参数表达式获得模块1331、约束模型获得模块1332、计算模块1333及标记模块1334；特征参数表达式获得模块1331构建局部引导轨迹上每一节点的特征参数表达式；约束模型获得模块1332根据拖拉机自身运动学或动力学限制条件建立约束模型；计算模块1333根据轨迹函数计算局部引导轨迹上各轨迹点的特征参数；标记模块1334通过约束模型对每一轨迹点的特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

其中，特征参数表达式获得模块建立引导轨迹方程，引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝f_x(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式：为前轮转向角度，L为拖拉机的轴距。

其中，约束模型包含：曲率约束模型：为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：

其中，计算模块1333根据轨迹函数的1阶导数及2阶导数及特征参数表达式获得所述每一轨迹点的特征参数，其中迹函数的1阶导数及2阶导数分别为：

当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员可根据本发明做出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明权利要求的保护范围。

Claims (18)

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

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

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

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

2.如权利要求1所述的局部引导轨迹规划方法，其特征在于，所述步骤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.如权利要求2所述的局部引导轨迹规划方法，其特征在于，于所述步骤2中，将所述六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

4.如权利要求3所述的局部引导轨迹规划方法，其特征在于，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

5.如权利要求3所述的局部引导轨迹规划方法，其特征在于，所述步骤3包含：

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

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

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

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

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

6.如权利要求5所述的局部引导轨迹规划方法，其特征在于，所述步骤303包含：

步骤3031：构建所述局部引导轨迹上每轨迹点的特征参数表达式；

步骤3032：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；

步骤3033：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；

步骤3034：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

7.如权利要求6所述的局部引导轨迹规划方法，其特征在于，于步骤3031中，建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中所述特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式： 为前轮转向角度，L为拖拉机的轴距。

8.如权利要求7所述的局部引导轨迹规划方法，其特征在于，步骤3032中，所述约束模型包含：曲率约束模型： 为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：

9.如权利要求8所述的局部引导轨迹规划方法，其特征在于，步骤3033中，根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：

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

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

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

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

11.如权利要求10所述的局部引导轨迹规划装置，其特征在于，所述坐标参数设定单元包含：

控制点选定模块：依据局部引导轨迹的初始点的航向角约束，选定控制点P₀、P₁、P₂，其中所述控制点P₁为拖拉机的初始位置，所述控制点P₀、P₁、P₂三点共线且所述控制点P₀和P₂关于所述控制点P₁对称，所述控制点P₀和P₂的矢量P₀P₂的方向角与拖拉机初始航向一致；依据全局路径起始作业点航向约束，选定控制点P₃、P₄、P₅，其中所述控制点P₄为全局路径起始作业点位置，所述控制点P₃、P₄、P₅三点共线且所述控制点P3和P₅关于P₄对称；

坐标参数获得模块：设定所述控制点P₁及所述控制点P₄的坐标参数分别为(x₁，y₁)和(x₄，y₄)；基于所述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₅的方向角。

12.如权利要求11所述的局部引导轨迹规划装置，其特征在于，所述轨迹函数获得单元将所述六个控制点P₀、P₁、P₂、P₃、P₄、P₅的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：

其中，F_k,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，P_i+k为第i+k个控制点坐标参数，k＝0，1，…，n，u∈[0,1]，i＝1,2，…，m-n，整条曲线由m-n段B样条曲线平滑连接而成，每段曲线由n+1个控制点生成。

13.如权利要求12所述的局部引导轨迹规划装置，其特征在于，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：

其中，

14.如权利要求12所述的局部引导轨迹规划装置，其特征在于，所述坐标参数优化单元包含：

初始化模块：初始化种群Q(t)；

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

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

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

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

15.如权利要求14所述的局部引导轨迹规划装置，其特征在于，所述评估模块包含：

特征参数表达式获得模块：构建所述局部引导轨迹上每一节点的特征参数表达式；

约束模型获得模块：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；

计算模块：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；

标记模块：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。

16.如权利要求15所述的局部引导轨迹规划装置，其特征在于，所述特征参数表达式获得模块建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝f_y(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为其中所述特征参数表达式包含：曲率特征参数表达式：κ为曲率；航向角度特征参数表达式：θ为航向角度；及前轮转向角度特征参数表达式： 为前轮转向角度，L为拖拉机的轴距。

17.如权利要求16所述的局部引导轨迹规划装置，其特征在于，所述约束模型包含：曲率约束模型： 为前轮最大转角的角度值；初始航向约束模型：|θ_s-θ_v|≤ε，θ_s为路径航向，θ_v为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θ_s-θ_t|≤ε，θ_t为全局路径起始作业点航向；转向角度范围约束模型为：

18.如权利要求17所述的局部引导轨迹规划方法，其特征在于，所述计算模块步根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：