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

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

【技术实现步骤摘要】
拖拉机自动驾驶系统的局部引导轨迹规划方法及装置
本专利技术属于农业车辆自动驾驶
，尤其涉及一种拖拉机自动驾驶系统局部引导轨迹规划方法及装置。
技术介绍
拖拉机自动驾驶系统是当前农业机械智能化领域不可或缺的一部分，具备自主行驶能力的农业车辆不仅有助于提高劳动生产率，而且有利于解决农村劳动力不足等问题。路径规划算法是拖拉机自动驾驶系统的关键技术，当前学术界的研究热点大多集中在田间全局路径规划上，自动驾驶系统跟踪全局路径需要经过两个阶段:1)从车辆当前位置出发逼近全局路径上的起始作业点，该过程中拖拉机行驶过的轨迹称为引导轨迹；2)从全局路径的起始作业点行驶至全局路径目标点。现有的自动驾驶系统主要依靠内置的路径跟踪算法完成第1阶段，由于缺少引导轨迹规划方法，导致引导轨迹距离较长，且在全局路径起始作业点处会出现震荡现象，跟踪误差较大。因此，拖拉机驾驶员为了提高作业精度，在第1阶段只能依靠手动驾驶，自动化程序较低。因此急需开发一种克服上述缺陷的拖拉机自动驾驶系统局部引导轨迹规划方法及装置
技术实现思路
本专利技术所要解决的技术问题在于提供一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其中，包含：步骤1：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；步骤2：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；步骤3：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹。上述的局部引导轨迹规划方法，其中，所述步骤1包含：步骤11：依据局部引导轨迹的初始点的航向角约束，选定控制点P0、P1、P2，其中所述控制点P1为拖拉机的初始位置，所述控制点P0、P1、P2三点共线且所述控制点P0和P2关于所述控制点P1对称，所述控制点P0和P2的矢量P0P2的方向角与拖拉机初始航向一致；步骤12：依据全局路径L起始作业点航向约束，选定控制点P3、P4、P5，其中所述控制点P4为全局路径L起始作业点位置，所述控制点P3、P4、P5三点共线且所述控制点P3和P5关于P4对称；步骤13：设定所述控制点P1及所述控制点P4的坐标参数分别为(x1，y1)和(x4，y4)；步骤14：基于所述B样条曲线与所述六个控制点P0、P1、P2、P3、P4、P5间的几何关系获得其余所述控制点P0、P2、P3、P5的坐标参数分别为P0(x1-l1cosθv，y1-l1sinθv)，P2(x1+l1cosθv，y1+l1sinθv)，P3(x4-l2cosθt，y4-l2sinθt)，P5(x4-l2cosθt，y4-l2sinθt)，其中l1为控制点P0与控制点P1、控制点P1与控制点P2之间的长度，l2为控制点P3与控制点P4、控制点P4与控制点P5之间的长度，θv为所述控制点P0和P2的矢量P0P2的方向角，θt为所述控制点P3和P5的矢量P3P5的方向角。上述的局部引导轨迹规划方法，其中，于所述步骤2中，将所述六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：其中，Fk,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，Pi+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)中的每个个体的l1及l2的长度进行一次测量，得到对应的所述局部引导轨迹的长度；步骤303：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；步骤304：记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；步骤305：判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群，继续执行所述步骤302。上述的局部引导轨迹规划方法，其中，所述步骤303包含：步骤3031：构建所述局部引导轨迹上每一节点的特征参数表达式；步骤3032：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；步骤3033：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；步骤3034：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。上述的局部引导轨迹规划方法，其中，于步骤3031中，建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝fy(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样条曲线表达式获得局部引导轨迹的轨迹函数；坐标参数优化单元：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹。上述的局部引导轨迹规划装置，其中，所述坐标参数设定单元包含：控制点选定模块：依据局部引导轨迹的初始点的航向角约束，选定控制点P0、P1、P2，其中所述控制点P1为拖拉机的初始位置，所述控制点P0、P1、P2三点共线且所述控制点P0和P2关于所述控制点P1对称，所述控制点P0和P2的矢量P0P2的方向角与拖拉机初始航向一致；依据全局路径起始作业点航向约束，选定控制点P3、P4、P5，其中所述控制点P4为全局路径起始作业点位置，所述控制点P3、P4、P5三点共线且所述控制点P3和P5关于P4对称；坐标参数获得模块：设定所述控制点P1及所述控制点P4的坐标参数分别为(x1，y1)和(x4，y4)；基于所述B样条曲线与所述六个控制点P0、P1、P2、P3、P4、P5间的几何关系获得其余所述控制点P0、P2、P3、P5的坐标参数分别为P0(x1-l1cosθv，y1-l1sinθv)，P2(x本文档来自技高网...

【技术保护点】
1.一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其特征在于，包含：步骤1：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；步骤2：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；步骤3：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹。

【技术特征摘要】
1.一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其特征在于，包含：步骤1：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；步骤2：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；步骤3：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹。2.如权利要求1所述的局部引导轨迹规划方法，其特征在于，所述步骤1包含：步骤11：依据局部引导轨迹的初始点的航向角约束，选定控制点P0、P1、P2，其中所述控制点P1为拖拉机的初始位置，所述控制点P0、P1、P2三点共线且所述控制点P0和P2关于所述控制点P1对称，所述控制点P0和P2的矢量P0P2的方向角与拖拉机初始航向一致；步骤12：依据全局路径起始作业点航向约束，选定控制点P3、P4、P5，其中所述控制点P4为全局路径起始作业点位置，所述控制点P3、P4、P5三点共线且所述控制点P3和P5关于P4对称；步骤13：设定所述控制点P1及所述控制点P4的坐标参数分别为(x1，y1)和(x4，y4)；步骤14：基于所述B样条曲线与所述六个控制点P0、P1、P2、P3、P4、P5间的几何关系获得其余所述控制点P0、P2、P3、P5的坐标参数分别为P0(x1-l1cosθv，y1-l1sinθv)，P2(x1+l1cosθv，y1+l1sinθv)，P3(x4-l2cosθt，y4-l2sinθt)，P5(x4-l2cosθt，y4-l2sinθt)，其中l1为控制点P0与控制点P1、控制点P1与控制点P2之间的长度，l2为控制点P3与控制点P4、控制点P4与控制点P5之间的长度，θv为所述控制点P0和P2的矢量P0P2的方向角，θt为所述控制点P3和P5的矢量P3P5的方向角。3.如权利要求2所述的局部引导轨迹规划方法，其特征在于，于所述步骤2中，将所述六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：其中，Fk,n为n阶B样条基函数，u为B样条曲线的节点，n为B样条曲线阶次，m为控制点个数，Pi+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)中的每个个体的l1及l2的长度进行一次测量，得到对应的所述局部引导轨迹的长度；步骤303：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；步骤304：记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；步骤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＝fy(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.一种拖拉机自动驾...

【专利技术属性】
技术研发人员：贾全，苑严伟，伟利国，付拓，赵博，毛文华，刘阳春，
申请(专利权)人：中国农业机械化科学研究院，
类型：发明
国别省市：北京,11