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

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

Info

Publication number
CN109933057B
CN109933057B CN201711362529.8A CN201711362529A CN109933057B CN 109933057 B CN109933057 B CN 109933057B CN 201711362529 A CN201711362529 A CN 201711362529A CN 109933057 B CN109933057 B CN 109933057B
Authority
CN
China
Prior art keywords
guide track
control point
point
track
local guide
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201711362529.8A
Other languages
English (en)
Other versions
CN109933057A (zh
Inventor
贾全
苑严伟
伟利国
付拓
赵博
毛文华
刘阳春
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chinese Academy of Agricultural Mechanization Sciences
Original Assignee
Chinese Academy of Agricultural Mechanization Sciences
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chinese Academy of Agricultural Mechanization Sciences filed Critical Chinese Academy of Agricultural Mechanization Sciences
Priority to CN201711362529.8A priority Critical patent/CN109933057B/zh
Publication of CN109933057A publication Critical patent/CN109933057A/zh
Application granted granted Critical
Publication of CN109933057B publication Critical patent/CN109933057B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

拖拉机自动驾驶系统的局部引导轨迹规划方法及装置
技术领域
本发明属于农业车辆自动驾驶技术领域，尤其涉及一种拖拉机自动驾驶系统局部引导轨迹规划方法及装置。
背景技术
拖拉机自动驾驶系统是当前农业机械智能化领域不可或缺的一部分，具备自主行驶能力的农业车辆不仅有助于提高劳动生产率，而且有利于解决农村劳动力不足等问题。
路径规划算法是拖拉机自动驾驶系统的关键技术，当前学术界的研究热点大多集中在田间全局路径规划上，自动驾驶系统跟踪全局路径需要经过两个阶段: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样条曲线表达式获得局部引导轨迹的所述轨迹函数为：
Figure BDA0001512047120000021
其中，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样条基函数为：
Figure BDA0001512047120000031
其中，
Figure BDA0001512047120000032
上述的局部引导轨迹规划方法，其中，所述步骤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阶导数为
Figure BDA0001512047120000041
其中所述特征参数表达式包含：曲率特征参数表达式：
Figure BDA0001512047120000042
κ为曲率；航向角度特征参数表达式：
Figure BDA0001512047120000043
θ为航向角度；及前轮转向角度特征参数表达式：
Figure BDA0001512047120000044
Figure BDA0001512047120000045
为前轮转向角度，L为拖拉机的轴距。
上述的局部引导轨迹规划方法，其中，步骤3032中，所述约束模型包含：曲率约束模型：
Figure BDA00015120471200000411
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure BDA0001512047120000048
上述的局部引导轨迹规划方法，其中，步骤3033中，根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：
Figure BDA0001512047120000049
Figure BDA00015120471200000410
本发明还提供一种拖拉机自动驾驶系统的局部引导轨迹规划装置，其中，包含：
坐标参数设定单元：设取局部引导轨迹为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(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的方向角。
上述的局部引导轨迹规划装置，其中，所述轨迹函数获得单元将所述六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：
Figure BDA0001512047120000051
其中，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样条基函数为：
Figure BDA0001512047120000052
其中，
Figure BDA0001512047120000061
上述的局部引导轨迹规划装置，其中，所述坐标参数优化单元包含：
初始化模块：初始化种群Q(t)；
测量模块：对种群Q(t)中的每个个体的l1及l2的长度进行一次测量，得到对应的所述局部引导轨迹的长度；
评估模块：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；
记录模块：记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；
判断模块：判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群。
上述的局部引导轨迹规划装置，其中，所述评估模块包含：
特征参数表达式获得模块：构建所述局部引导轨迹上每一节点的特征参数表达式；
约束模型获得模块：根据拖拉机自身运动学或动力学限制条件建立所述约束模型；
计算模块：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；
标记模块：通过所述约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。
上述的局部引导轨迹规划装置，其中，所述特征参数表达式获得模块建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝fy(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为
Figure BDA0001512047120000062
其中所述特征参数表达式包含：曲率特征参数表达式：
Figure BDA0001512047120000071
κ为曲率；航向角度特征参数表达式：
Figure BDA0001512047120000072
θ为航向角度；及前轮转向角度特征参数表达式：
Figure BDA0001512047120000073
为前轮转向角度，L为拖拉机的轴距。
上述的局部引导轨迹规划装置，其中，所述约束模型包含：曲率约束模型：
Figure BDA0001512047120000074
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure BDA0001512047120000075
上述的局部引导轨迹规划装置，其中，所述计算模块步根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：
Figure BDA0001512047120000076
Figure BDA0001512047120000077
本发明针对于现有技术其功效在于，本发明在现有研究基础上基于量子遗传算法和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：依据局部引导轨迹的初始点的航向角约束，选定控制点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的方向角。
值得注意的是，当样条曲线的控制点较少时不易求得满足约束的期望路径，控制点过多则会造成方程求解困难，增加运算时间。本实施例采用六个控制点P0～P5生成满足约束条件的三阶B样条引导轨迹。
现有文献大多采用优化算法直接求取N个控制点，对于6个控制点P(x，y)需要优化的参数为12个，使用迭代方法求解时运算量非常大。为了提高轨迹规划速度，有必要研究曲线约束与控制点间的关系以简化计算。对于三阶B样条曲线，当三个相邻的控制点在同一条直线上且间距相等时，生成的B样条曲线与直线相切于中间的控制点，如图7所示。
依据引导轨迹初始点的航向角约束，选择控制点P0,P1,P2。当三个控制点满足如下条件，即可满足拖拉机初始状态约束：
1)选定拖拉机初始位置为P1；
2)P0,P1,P2三点共线；
3)矢量P0P2的方向角与拖拉机初始航向一致；
4)P0和P2关于P1对称。
同理，依据全局路径起始作业点航向约束，确定控制点P3,P4,P5，并选定P4为全局路径起始作业点位置坐标。
设线段P0P1和P1P2的长度为l1，线段P3P4和P4P5的长度为l2。依据起止点航向约束选定控制点P1和P4后，只要确定l1和l2的数值即可求得其它控制点并生成引导轨迹，这样便把最优轨迹规划问题转变为求l1和l2的最优解问题，相比求解12个参数，该方法只需要优化2个参数，有效减少了算法的运算量。再利用P1及P4的的坐标参数与l1和l2，计算得到其余控制点的坐标参数，其中拖拉机初始位置P1(x1，y1)和全局路径起始作业点P4(x4，y4)已知，其余四个控制点的坐标参数分别为：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)。
又进一步地，于步骤2中，将六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的轨迹函数为：
Figure BDA0001512047120000101
其中，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样条基函数为：
Figure BDA0001512047120000102
其中，
Figure BDA0001512047120000103
再进一步地，步骤3中采用量子遗传算法求解l1和l2，也可采用遗传算法、粒子群算法等方法进行求解。量子遗传算法(quantum genetic algorithm，QGA)是遗传算法和量子计算结合产生的一种新的概率进化算法，比传统遗传算法具有更快的收敛速度和并行处理能力。
将l1、l2作为个体进行寻优，本实施例把引导轨迹距离最短作为优化目标，实际应用时还可以将轨迹最平滑等参数作为优化目标。将引导轨迹距离取负值作为适应度函数(轨迹越短，适应度值越大)，步骤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阶导数为
Figure BDA0001512047120000111
其中，特征参数表达式包含：曲率特征参数表达式：
Figure BDA0001512047120000112
κ为曲率；航向角度特征参数表达式：
Figure BDA0001512047120000113
θ为航向角度；及前轮转向角度特征参数表达式：
Figure BDA0001512047120000114
为前轮转向角度，L为拖拉机的轴距。
其中，步骤3032中，为保证规划的引导轨迹具有可跟踪性，实际实施过程中需要依据拖拉机的自身运动学或动力学限制条件建立引导轨迹约束模型。约束模型包含：曲率约束模型：
Figure BDA0001512047120000115
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure BDA0001512047120000116
本实施例基于车辆运动学特征构建上述约束模型，以下说明构建约束模型的具体方式：
一、构建曲率约束模型：
拖拉机曲率与转弯半径的关系为
Figure BDA0001512047120000121
其中R为转弯半径；
拖拉机最小转弯半径主要受轴距和转向前轮的最大转向角限制，其数值是固定的，计算公式为：
Figure BDA0001512047120000122
式中Rmin为最小转弯半径，
Figure BDA0001512047120000123
为为前轮最大转角的角度值；因此基于曲率特征参数表达式及上述公式1和2获得曲率约束模型：
Figure BDA0001512047120000124
二、构建初始航向约束模型：
为避免拖拉机在引导轨迹的起始时刻出现原地转向或震荡现象，需要对引导轨迹的初始航向进行约束，建立如下约束条件：
|θs-θv|≤ε (3)
式中θs为路径航向，θv为车身原始航向，ε为小正数。
三、构建起始作业点处的路径跟踪精度约束模型：
为了保证拖拉机引导轨迹与全局路径结合处的平滑度，提高全局路径起始作业点Sstart处的路径跟踪精度，建立如下约束条件：
|θs-θt|≤ε (4)
式中θt为全局路径起始作业点航向。
四、构建转向角度范围约束模型：
拖拉机转向轮角度范围决定了其路径跟踪的灵活性，如果引导轨迹某点处等效转角过大，该点处的路径将不具备可跟踪性。因此，需要对等效前轮转角进行约束。由曲率特征参数表达式及前轮转向角度特征参数表达式可建立等效前轮转角约束模型如下：
Figure BDA0001512047120000125
其中，步骤3033中，根据轨迹函数的1阶导数及2阶导数及特征参数表达式获得每一轨迹点的特征参数，其中轨迹函数的1阶导数及2阶导数分别为：
Figure BDA0001512047120000131
Figure BDA0001512047120000132
将公式6和7代入特征参数表达式可求解得到局部引导轨迹上各个轨迹点的曲率κ、航向角θ和等效前轮转角
Figure BDA0001512047120000133
请参照图8，图8为本发明局部引导轨迹规划装置的结构示意图。如图8所示，本发明的部引导轨迹规划装置包含：坐标参数设定单元11、轨迹函数获得单元12及坐标参数优化单元13；坐标参数设定单元11设取局部引导轨迹为B样条曲线，基于B样条曲线设定局部引导轨迹的六个控制点的坐标参数；轨迹函数获得单元12根据六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；坐标参数优化单元13基于量子遗传算法对坐标参数进行优化以得到最优的局部引导轨迹。
进一步地，坐标参数设定单元11包含：控制点选定模块111及坐标参数获得模块112；控制点选定模块111依据局部引导轨迹的初始点的航向角约束，选定控制点P0、P1、P2，其中控制点P1为拖拉机的初始位置，控制点P0、P1、P2三点共线且控制点P0和P2关于控制点P1对称，控制点P0和P2的矢量P0P2的方向角与拖拉机初始航向一致；依据全局路径起始作业点航向约束，选定控制点P3、P4、P5，其中控制点P4为全局路径起始作业点位置，控制点P3、P4、P5三点共线且所述控制点P3和P5关于P4对称；坐标参数获得模块112设定控制点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(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的方向角。
又进一步地，轨迹函数获得单元12将六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的轨迹函数为：
Figure BDA0001512047120000141
其中，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样条基函数为：
Figure BDA0001512047120000142
其中，
Figure BDA0001512047120000143
再进一步地，坐标参数优化单元13包含：初始化模块131、测量模块132、评估模块133、记录模块134及判断模块135；初始化模块131初始化种群Q(t)；测量模块132对种群Q(t)中的每个个体的l1及l2的长度进行一次测量，得到对应的局部引导轨迹的长度；评估模块133对各个局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；记录模块134记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；判断模块135判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群。
更进一步地，评估模块133包含：特征参数表达式获得模块1331、约束模型获得模块1332、计算模块1333及标记模块1334；特征参数表达式获得模块1331构建局部引导轨迹上每一节点的特征参数表达式；约束模型获得模块1332根据拖拉机自身运动学或动力学限制条件建立约束模型；计算模块1333根据轨迹函数计算局部引导轨迹上各轨迹点的特征参数；标记模块1334通过约束模型对每一轨迹点的特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。
其中，特征参数表达式获得模块建立引导轨迹方程，引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝fy(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为
Figure BDA0001512047120000151
其中特征参数表达式包含：曲率特征参数表达式：
Figure BDA0001512047120000152
κ为曲率；航向角度特征参数表达式：
Figure BDA0001512047120000153
θ为航向角度；及前轮转向角度特征参数表达式：
Figure BDA0001512047120000154
为前轮转向角度，L为拖拉机的轴距。
其中，约束模型包含：曲率约束模型：
Figure BDA0001512047120000155
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure BDA0001512047120000156
其中，计算模块1333根据轨迹函数的1阶导数及2阶导数及特征参数表达式获得所述每一轨迹点的特征参数，其中迹函数的1阶导数及2阶导数分别为：
Figure BDA0001512047120000157
Figure BDA0001512047120000158
当然，本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员可根据本发明做出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明权利要求的保护范围。

Claims (14)

1.一种拖拉机自动驾驶系统的局部引导轨迹规划方法，其特征在于，包含：
步骤1：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；
步骤2：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；
步骤3：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹；
其中，所述步骤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包含：
步骤301：初始化种群Q(t)；
步骤302：对种群Q(t)中的每个个体的l1及l2的长度进行一次测量，得到对应的所述局部引导轨迹的长度；
步骤303：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；
步骤304：记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；
步骤305：判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群，继续执行所述步骤302。
2.如权利要求1所述的局部引导轨迹规划方法，其特征在于，于所述步骤2中，将所述六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：
Figure FDA0002617648550000021
其中，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个控制点生成。
3.如权利要求2所述的局部引导轨迹规划方法，其特征在于，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：
Figure FDA0002617648550000022
其中，
Figure FDA0002617648550000023
其中，u1,u2,u3分别为u的一次方，二次方，三次方；Fk,3(u)表示三阶B样条基函数。
4.如权利要求3所述的局部引导轨迹规划方法，其特征在于，所述步骤303包含：
步骤3031：构建所述局部引导轨迹上每轨迹点的特征参数表达式；
步骤3032：根据拖拉机自身运动学或动力学限制条件建立约束模型；
步骤3033：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；
步骤3034：通过约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。
5.如权利要求4所述的局部引导轨迹规划方法，其特征在于，于步骤3031中，建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝fy(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为
Figure FDA0002617648550000031
其中所述特征参数表达式包含：曲率特征参数表达式：
Figure FDA0002617648550000032
κ为曲率；航向角度特征参数表达式：
Figure FDA0002617648550000033
θ为航向角度；及前轮转向角度特征参数表达式：
Figure FDA0002617648550000034
Figure FDA0002617648550000035
为前轮转向角度，L为拖拉机的轴距；
其中，s(u)为各段关于位置x,y的方程；
x＝fx(u)、y＝fy(u) ，为建立关于u的x,y的函数。
6.如权利要求5所述的局部引导轨迹规划方法，其特征在于，步骤3032中，约束模型包含：曲率约束模型：
Figure FDA0002617648550000036
Figure FDA0002617648550000037
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure FDA0002617648550000038
7.如权利要求6所述的局部引导轨迹规划方法，其特征在于，步骤3033中，根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：
Figure FDA0002617648550000039
Figure FDA0002617648550000041
其中，P为泛指当前局部轨迹上的控制点。
8.一种拖拉机自动驾驶系统的局部引导轨迹规划装置，其特征在于，包含：
坐标参数设定单元：设取局部引导轨迹为B样条曲线，基于所述B样条曲线设定局部引导轨迹的六个控制点的坐标参数；
轨迹函数获得单元：根据所述六个控制点的坐标参数及B样条曲线表达式获得局部引导轨迹的轨迹函数；
坐标参数优化单元：基于量子遗传算法对所述坐标参数进行优化以得到最优的局部引导轨迹；
其中，所述坐标参数优化单元包含：
初始化模块：初始化种群Q(t)；
测量模块：对种群Q(t)中的每个个体的l1及l2的长度进行一次测量，得到对应的所述局部引导轨迹的长度；
评估模块：对各个所述局部引导轨迹的长度进行适应度评估，评估过程中若引导轨迹不满足约束条件适应度设为负无穷；
记录模块：记录最优的l1及l2数值和对应的适应度，以最优的l1及l2对应的个体作为下一代进化目标；
判断模块：判断是否满足结束条件，是则利用当前最优解计算得到期望的最优的局部引导轨迹并退出，否则经过量子门操作，改变染色体量子比特编码，形成下一代种群；
其中，所述坐标参数设定单元包含：
控制点选定模块：依据局部引导轨迹的初始点的航向角约束，选定控制点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(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的方向角。
9.如权利要求8所述的局部引导轨迹规划装置，其特征在于，所述轨迹函数获得单元将所述六个控制点P0、P1、P2、P3、P4、P5的坐标参数带入B样条曲线表达式获得局部引导轨迹的所述轨迹函数为：
Figure FDA0002617648550000051
其中，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个控制点生成。
10.如权利要求9所述的局部引导轨迹规划装置，其特征在于，当所述B样条曲线为3阶，控制点个数为6个时，B样条基函数为：
Figure FDA0002617648550000052
其中，
Figure FDA0002617648550000053
其中，u1,u2,u3分别为u的一次方，二次方，三次方；Fk,3(u)表示三阶B样条基函数。
11.如权利要求10所述的局部引导轨迹规划装置，其特征在于，所述评估模块包含：
特征参数表达式获得模块：构建所述局部引导轨迹上每一节点的特征参数表达式；
约束模型获得模块：根据拖拉机自身运动学或动力学限制条件建立约束模型；
计算模块：根据所述轨迹函数计算所述局部引导轨迹上各轨迹点的特征参数；
标记模块：通过约束模型对每一所述轨迹点的所述特征参数进行验证，若引导轨迹不满足约束条件适应度设为负无穷。
12.如权利要求11所述的局部引导轨迹规划装置，其特征在于，所述特征参数表达式获得模块建立引导轨迹方程，所述引导轨迹方程为：s(u)＝S(x，y)，其中x、y是定义在高斯坐标系下的轨迹点位置坐标，s是局部引导轨迹，根据所述引导轨迹方程获得局部引导轨迹s上各点处的特征参数表达式，令x＝fx(u)、y＝fy(u)，其中u为B样条曲线的节点，则x，y对u的1～2阶导数为
Figure FDA0002617648550000061
其中所述特征参数表达式包含：曲率特征参数表达式：
Figure FDA0002617648550000062
κ为曲率；航向角度特征参数表达式：
Figure FDA0002617648550000063
θ为航向角度；及前轮转向角度特征参数表达式：
Figure FDA0002617648550000064
Figure FDA0002617648550000065
为前轮转向角度，L为拖拉机的轴距；
其中，s(u)为各段关于位置x,y的方程；
x＝fx(u)、y＝fy(u) ，为建立关于u的x,y的函数。
13.如权利要求12所述的局部引导轨迹规划装置，其特征在于，约束模型包含：曲率约束模型：
Figure FDA0002617648550000066
Figure FDA0002617648550000067
为前轮最大转角的角度值；初始航向约束模型：|θs-θv|≤ε，θs为路径航向，θv为车身原始航向，ε—小正数；起始作业点处的路径跟踪精度约束模型：|θs-θt|≤ε，θt为全局路径起始作业点航向；转向角度范围约束模型为：
Figure FDA0002617648550000068
14.如权利要求13所述的局部引导轨迹规划方法，其特征在于，所述计算模块步根据所述轨迹函数的1阶导数及2阶导数及所述特征参数表达式获得所述每一轨迹点的所述特征参数，其中所述轨迹函数的1阶导数及2阶导数分别为：
Figure FDA0002617648550000071
Figure FDA0002617648550000072
其中，P为泛指当前局部轨迹上的控制点。
CN201711362529.8A 2017-12-18 2017-12-18 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置 Active CN109933057B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711362529.8A CN109933057B (zh) 2017-12-18 2017-12-18 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711362529.8A CN109933057B (zh) 2017-12-18 2017-12-18 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置

Publications (2)

Publication Number Publication Date
CN109933057A CN109933057A (zh) 2019-06-25
CN109933057B true CN109933057B (zh) 2020-10-23

Family

ID=66982252

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711362529.8A Active CN109933057B (zh) 2017-12-18 2017-12-18 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置

Country Status (1)

Country Link
CN (1) CN109933057B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110533730A (zh) * 2019-08-26 2019-12-03 智久(厦门)机器人科技有限公司 基于b样条曲线的路径规划方法、装置、计算机可读存储介质
CN111123942B (zh) * 2019-12-30 2022-06-24 广东嘉腾机器人自动化有限公司 Agv小车的样条路径生成方法、装置以及agv小车
CN111552296B (zh) * 2020-05-14 2021-03-26 宁波智能装备研究院有限公司 一种基于弯曲柱坐标系的局部平滑轨迹规划方法
CN113835423B (zh) * 2020-06-08 2024-03-26 苏州艾吉威机器人有限公司 基于b样条曲线的路径规划方法、运动控制方法及系统
WO2021248798A1 (zh) * 2020-06-08 2021-12-16 苏州艾吉威机器人有限公司 路径跟踪方法、系统、装置及计算机可读存储介质
CN112036537B (zh) * 2020-08-04 2023-12-26 中国农业大学 用于平地机导航的三维路径规划方法及系统
CN114593742B (zh) * 2020-12-04 2024-04-19 上海汽车集团股份有限公司 一种智能驾驶车辆的局部路径规划方法、装置及设备
CN114184206B (zh) * 2021-12-03 2024-04-19 北京车慧达科技有限公司 一种基于车辆轨迹点生成行驶路线的方法和装置
CN115617928A (zh) * 2022-05-20 2023-01-17 丰疆智能科技股份有限公司 农机信息共享方法、系统、电子设备及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3508619B2 (ja) * 1999-05-13 2004-03-22 株式会社デンソー 車両用ナビゲーション装置
CN105197010B (zh) * 2014-06-04 2018-03-27 长春孔辉汽车科技股份有限公司 辅助泊车系统以及辅助泊车控制方法
CN106843211B (zh) * 2017-02-07 2019-11-08 东华大学 一种基于改进遗传算法的移动机器人路径规划方法

Also Published As

Publication number Publication date
CN109933057A (zh) 2019-06-25

Similar Documents

Publication Publication Date Title
CN109933057B (zh) 拖拉机自动驾驶系统的局部引导轨迹规划方法及装置
CN112356830B (zh) 一种基于模型强化学习的智能泊车方法
Li et al. Time-optimal maneuver planning in automatic parallel parking using a simultaneous dynamic optimization approach
WO2018176593A1 (zh) 一种面向无人自行车的局部避障路径规划方法
CN106569496B (zh) 一种运动路径的规划方法
CN110989577B (zh) 自动驾驶决策方法及车辆的自动驾驶装置
CN111238518B (zh) 一种基于改进rrt算法的智能农业车辆路径规划方法
WO2019042295A1 (zh) 一种无人驾驶路径规划方法、系统和装置
CN107092266B (zh) 一种移动车轨迹跟踪控制方法
Ding et al. Trajectory tracking of redundantly actuated mobile robot by MPC velocity control under steering strategy constraint
Zhang et al. Automatic vehicle parallel parking design using fifth degree polynomial path planning
CN112269965A (zh) 一种非完整约束条件下的连续曲率路径优化方法
CN112092825B (zh) 一种基于机器学习的车道保持方法
CN110789530B (zh) 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和系统
CN111459160A (zh) 一种开放道路无人洗扫车大规模轨迹平滑方法
Guo et al. Optimal navigation for AGVs: A soft actor–critic-based reinforcement learning approach with composite auxiliary rewards
CN104898659A (zh) 一种基于模型预测的人机协同控制方法
Xiao et al. Reinforcement learning-driven dynamic obstacle avoidance for mobile robot trajectory tracking
CN117031961A (zh) 基于模型预测控制的自动驾驶汽车交互性决策规划方法
Horváth et al. Enhancement of pure-pursuit path-tracking algorithm with multi-goal selection
CN113158415B (zh) 一种基于误差分析的车辆轨迹相似度评估方法
CN111427346B (zh) 适用于车型机器人的局部路径规划与追踪方法
Xie et al. An optimal path-planning algorithm for unmanned rollers with constraints on roller attitude
CN114509086A (zh) 智能车辆在连续弯道场景下的最优轨迹规划方法及系统
Zhang et al. An NSGA-II-based multi-objective trajectory planning method for autonomous driving

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant