基于北斗定位的农机自动驾驶系统关键技术研发的制作方法

[0001]
本发明属于车辆自主泊车技术领域，特别涉及一种基于北斗定位的农机自动驾驶系统关键技术研发中的一种基于非线性优化的高效平行泊车规划方法。


背景技术：

[0002]
近年来，随着国内汽车保有量的迅速增长，城市内停车位日趋紧张和狭小。对于车位过于狭窄的情况，驾驶员往往难以很好地控制汽车进行快速准确的泊车，由泊车引发的事故几率大大升高。
[0003]
自主泊车系统可以帮助驾驶员进行准确安全的泊车，这种系统使用测距传感器和车速传感器自动地检测车位大小，然后规划出一条的泊车路径，最后自动地控制车辆的转向系统同、制动系统和动力系统遵循规划出的路径完成泊车。
[0004]
如在专利号为“cn105035075b”、名称为“一种用于自主平行泊车的路径规划方法”的专利中，公开了一种用于自主平行泊车的路径规划方法，其特征在于：所述方法用于自主泊车系统检测到可用泊车空间而将车辆自动地平行停泊于所述泊车空间中，所述方法包含以下步骤：检测车位后方第一障碍物上边缘，以及车位前方第二障碍物下边缘，以及本车右边缘与第二障碍物左边缘间距；基于阈值确定车位大小是否足以泊车；确定平行泊车的第二阶段路径；确定平行泊车的第一阶段路径；控制车辆转向系统与动力系统遵循所述第一阶段路径与第二阶段路径。
[0005]
但是以上专利中，路径存在不光滑和曲率不连续的情况，就会造成实际泊车过程中需要停车原地打方向盘的问题，而这种情况难以适应实际的停车需求。


技术实现要素：

[0006]
为解决上述问题，本发明的首要目的在于提供一种基于北斗定位的农机自动驾驶系统关键技术研发，该方法可获得泊车轨迹和曲率连续，即车辆沿着该轨迹行驶时方向盘变化连续，不需要停车。
[0007]
本发明的另一个目的在于提供一种基于北斗定位的农机自动驾驶系统关键技术研发，该方法获得的泊车轨迹准确，且求解效率高效。
[0008]
为实现上述目的，本发明的技术方案如下。
[0009]
一种基于北斗定位的农机自动驾驶系统关键技术研发，即一种基于非线性优化的高效平行泊车规划方法，其特征在于，该方法的具体步骤如下：
[0010]
s1：根据泊车的初始点和终点制定泊车的初始路径，并在初始路径上选取核心坐标，所述核心坐标设置为一个以上；
[0011]
s2：根据初始路径选取轨迹函数；
[0012]
s3：将核心坐标及泊车的初始点和终点的曲率信息作为轨迹函数的输入值，计算出轨迹规划的控制点，并将该控制点作为变量初始值；
[0013]
s4：制定目标函数、约束条件和变量取值范围，并根据目标函数、约束条件、变量取
值范围和变量初始值进行序列的二次规划，得到优化后的轨迹函数的最优解；
[0014]
s5：根据最优解绘制泊车轨迹的最终轨迹函数，该最终轨迹函数为泊车的轨迹曲线。根据初始点和终点制定的泊车的初始路径往往是模糊的或者仅具备示意效果，其路径的曲率往往是不连续的，而曲率的不连续意味着停车过程中需要停车原地打方向盘，进行转弯调节，该种泊车轨迹难以适应实际的停车需求，精度较低，而泊车的轨迹的确定往往与初始解的选取有关，当初始解越接近最优解，则计算出的轨迹越准确，所以最优解能够被确定就成为了泊车轨迹是否准确的关键因素，本发明在该初始路径的基础上确定停车过程中的必经位置，即核心坐标，并根据初始轨迹确定停车的最接近的轨迹函数的类型，将核心坐标和初始点和重点的曲率信息作为上述最接近的轨迹函数的输入值，计算出轨迹函数的控制点，并根据泊车过程中的约束条件、控制点的取值范围和目标函数等进行控制点的二次规划，利用规划后得到的最优解，计算出泊车的最终轨迹函数。该种利用控制点、约束条件、目标函数和控制点的取值范围的控制等方式对控制点的二次规划所得到的的最优解更准确和高效，
[0015]
进一步地，所述s1中核心坐标选取初始路径中坐标点的曲率变化的转折点。例如，车辆的起始点、开始转弯点、进入停车位的转弯点和停车点等，均是车辆停车过程中的必经点。
[0016]
进一步地，所述s2中轨迹函数可选取b样条函数，多项式函数，三角函数，反切函数或者正切函数。
[0017]
进一步地，所述s4中目标函数包含的优化目标为航向角、路径长度和泊车曲率，所述航向角为车辆的瞬时方位角。
[0018]
进一步地，所述s4中约束条件根据车辆的顶点坐标关系建立，包括起点约束、车辆顶点约束、起点斜率约束、路径曲率约束、碰撞约束和终点航向约束。其中约束条件的选取主要用来避障，保证车辆能够准确驶入到停车点。
[0019]
进一步地，所述s4中变量取值范围以控制点为中心且浮动空间以3为上下限。
[0020]
进一步地，所述目标函数的计算方法具体为：
[0021]
s21：对航向角、路径长度和泊车曲率进行归一化处理；
[0022]
s22：对归一化处理后的航向角、路径长度和泊车曲率分别选择相同的权重进行加权；
[0023]
s23：计算出加权之后的最小值，将该最小值作为目标函数。
[0024]
进一步地，对序列的二次规划包括采用sqp求解器对目标函数、约束条件、变量取值范围和变量初始值进行求解。
[0025]
本发明根据车辆泊车过程中的行驶轨迹，制定出车辆泊车的初始路径，根据该路径选择较为接近的轨迹函数类型，并根据车辆泊车过程中的必经位置点信息和初始点以及终点的曲率信息，计算出轨迹函数的控制点信息，将该控制点信息以及泊车过程中的约束条件、优化目标、控制点的取值范围的选取和控制，对控制点进行优化，得到最优解，并利用该最优解计算出泊车的最终轨迹函数，即泊车的最佳路径。
[0026]
在确定了最优解后，获得最终轨迹的方式也变得高效，相较于传统情况下对非线性函数的优化具有高效，准确的特点。
附图说明
[0027]
图1是本发明的流程图。
[0028]
图2泊车的初始轨迹的示意图。
[0029]
图3是本发明的小车顶点坐标示意图。
[0030]
图4是本发明的优化后的b样条泊车路径示意图。
[0031]
图5是本发明的优化后的泊车轨迹的横轴坐标与路径长度的关系示意图。
[0032]
图6是本发明的优化后的泊车轨迹的横轴坐标与曲率的关系示意图。
[0033]
图7是本发明的优化后的泊车轨迹的横轴坐标与x轴夹角的关系示意图。
具体实施方式
[0034]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
[0035]
参照图1-7所示，为本发明的一种基于北斗定位的农机自动驾驶系统关键技术研发，该方法的具体步骤如下：
[0036]
s1：根据泊车的初始点和终点制定泊车的初始路径，并在初始路径上选取核心坐标，所述核心坐标设置为一个以上；
[0037]
s2：根据初始路径选取轨迹函数；
[0038]
s3：将核心坐标及泊车的初始点和终点的曲率信息作为轨迹函数的输入值，计算出轨迹规划的控制点，并将该控制点作为变量初始值；
[0039]
s4：制定目标函数、约束条件和变量取值范围，并根据目标函数、约束条件、变量取值范围和变量初始值进行序列的二次规划，得到优化后的轨迹函数的最优解；
[0040]
s5：根据最优解绘制泊车轨迹的最终轨迹函数，该最终轨迹函数为泊车的轨迹曲线。根据初始点和终点制定的泊车的初始路径往往是模糊的或者仅具备示意效果，其路径的曲率往往是不连续的，而曲率的不连续意味着停车过程中需要停车原地打方向盘，进行转弯调节，该种泊车轨迹难以适应实际的停车需求，精度较低，而泊车的轨迹的确定往往与初始解的选取有关，当初始解越接近最优解，则计算出的轨迹越准确，所以最优解能够被确定就成为了泊车轨迹是否准确的关键因素，本发明在该初始路径的基础上确定停车过程中的必经位置，即核心坐标，并根据初始轨迹确定停车的最接近的轨迹函数的类型，将核心坐标和初始点和重点的曲率信息作为上述最接近的轨迹函数的输入值，计算出轨迹函数的控制点，并根据泊车过程中的约束条件、控制点的取值范围和目标函数等进行控制点的二次规划，利用规划后得到的最优解，计算出泊车的最终轨迹函数。该种利用控制点、约束条件、目标函数和控制点的取值范围的控制等方式对控制点的二次规划所得到的的最优解更准确和高效，
[0041]
在本实施例中，车辆要素的数据如表1所示；车位要素的数据如表2所示；
[0042]
表1
[0043]
参数符号单位数值车长l
c
m4.155车宽l
k
m1.645
轴距lm2.405前悬长度l
f
m0.8后悬长度l
r
m0.95
[0044]
表2
[0045][0046][0047]
如图1所示，泊车的初始路径由c1c2，c2c3，c3c4三段曲线构成，则核心坐标点的选取分别为c1，c2，c3和c4，根据圆弧直线法可以求出c1，c2， c3和c4这四点的坐标，计算过程如下：
[0048]
(r1+r2)sinθ+s1cosθ＝s
[0049]
(r1+r2)(1-cosθ)+s1sinθ＝h
[0050][0051]
当两个相切圆弧最小并且相等的时候，车走过的路程最短，并且容易控制： r2＝r1＝r
min
[0052]
直线s1的方程为：
[0053]
y＝ax+b
→
ax-y+b＝0
[0054]
以车库左端中部顶点为坐标原点，以车库另外一端为正方向，建立直角坐标系，则o1坐标是(δs+s,h-r1)，o2坐标是(δs,r2)，两点到直线s1距离为：
[0055][0056]
取车辆的转弯半径为最小半径：
[0057]
r1＝r2＝d1＝d2＝r
min
[0058]
带入解得：
[0059][0060]
[0061][0062]
规划终点坐标为c1(x1,y1)(例如(1.2，-1.5))，起始点坐标c4(x4,y4) (例如(9.6,1.7))，h＝y4，δs＝x1，s＝x
4-x1，利用公式(11)，求出曲线s1斜率a，求出曲线的倾斜角θ。则可以求出来起点圆弧与直线相切的点c2(x2,y2)
[0063]
x2＝x1+r2·
sinθ
[0064]
y2＝y1+(r
2-r2·
cosθ)
[0065]
求出终点圆弧与直线相切的点c3(x3,y3)
[0066]
x3＝x
4-r1·
sinθ
[0067]
y3＝y
4-(r
1-r1·
cosθ)
[0068]
据此求出c1、c2、c3、c4四点核心坐标信息。
[0069]
其中，所述s1中核心坐标选取初始路径中坐标点的曲率变化的转折点。例如，车辆的起始点、开始转弯点、进入停车位的转弯点和停车点等，均是车辆停车过程中的必经点。
[0070]
其中，所述s2中轨迹函数可选取b样条函数，多项式函数，三角函数，反切函数或者正切函数。
[0071]
本发明中，轨迹函数采用b样条函数，b样条具有高阶连续平滑的特点。根据泊车路径曲率的连续性要求b样条函数二次可导，因此3次4阶b样条曲线可满足泊车路径曲率连续性。
[0072]
b样条曲线总方程为：
[0073][0074]
p
i
是控制曲线的特征点，f
i,k
(t)是k阶b样条曲线基函数，3次b样条曲线基函数为：
[0075][0076][0077][0078][0079][0080]
将基函数代入方程：
[0081]
p(t)＝p0*f
0,3
(t)+p1*f
1,3
(t)+p2*f
2,3
(t)+p3*f
3,3
(t)
[0082]
令则有：
[0083][0084][0085][0086]
在本实施例中，b样条函数是3次的，故有上述定义可知，n＝3；
[0087]
所以每段控制点的个数就是即k＝0，k＝1，k＝2，k＝3，共四个控制点。
[0088]
本文控制点个数为6个；p1(t)，p2(t)，p3(t)，p4(t)，p5(t)，p6(t)
[0089]
故m+n+1＝6；故m+1＝3故有三段b样条曲线；
[0090]
三段b样条曲线的含义是该b样条曲线是分段函数，分成三段，每段的表达式有上述定义给出：
[0091]
第一段c1-c2：
[0092]
p
1，3
(t)＝p1(t)*f
0，3
(t)+p2(t)*f
1，3
(t)+p3(t)*f
2，3
(t)+p4(t)*f
3，3
(t)
[0093]
第二段c2-c3：
[0094]
p
2，3
(t)＝p2(t)*f
0，3
(t)+p3(t)*f
1，3
(t)+p4(t)*f
2，3
(t)+p5(t)*f
3，3
(t)
[0095]
第三段c3-c4：
[0096]
p
2，3
(t)＝p3(t)*f
0，3
(t)+p4(t)*f
1，3
(t)+p5(t)*f
2，3
(t)+p6(t)*f
3，3
(t)
[0097]
由上述表达式可看出，这三段控制点之间均存在控制点重复的情况，所以由图1知有三段曲线c1c2，c2c3，c3c4。每段曲线需要四个控制点,相互间公用3个公共点，则一共需要反求出6个公共点。反求6个公共点需要6组方程， c1、c2、c3、c4只能提供四组方程，由于起始点和终点的曲率为0，则由曲率求解公式，它们的横纵坐标二阶导为0，故加入初始点控制方程，可以得到6 组控制方程。
[0098]
1)c1为第1段曲线起点，t＝0，
[0099][0100]
2)c2为第2段曲线起点，t＝0，
[0101][0102]
3)c3为第3段曲线起点，t＝0，
[0103][0104]
4)c4为第4段曲线终点，t＝1，
[0105][0106]
5)利用起始点曲率为0，t＝0，
[0107][0108]
6)利用终点曲率为0，t＝1，
[0109][0110]
整理可得：
[0111][0112]
由上述公式解方程可得路径规划的6个控制点：
[0113][0114]
其中，所述s4中目标函数包含的优化目标为航向角、路径长度和泊车曲率，所述航向角为车辆的瞬时方位角。
[0115]
车辆的瞬时方位角计算：
[0116][0117]
泊车曲率计算：
[0118][0119]
路径长度计算：
[0120][0121]
其中，所述s4中约束条件根据车辆的顶点坐标关系建立，包括起点约束、车辆顶点约束、起点斜率约束、路径曲率约束、碰撞约束和终点航向约束。其中约束条件的选取主要用来避障，保证车辆能够准确驶入到停车点。
[0122]
在本实施例中，如图2所示，小车四个顶点之间的坐标关系为：
[0123]
x
a
＝x+(l
f
+l)cosθ-l
k
/2
·
sinθ
[0124]
y
a
＝y+(l
f
+l)sinθ+l
k
/2
·
cosθ
[0125]
x
b
＝x+(l
f
+l)cosθ+l
k
/2
·
sinθ
[0126]
y
b
＝y+(l
f
+l)sinθ-l
k
/2
·
cosθ
[0127]
x
c
＝x-l
r
·
cosθ+l
k
/2
·
sinθ
[0128]
y
c
＝y-l
r
·
sinθ-l
k
/2
·
cosθ
[0129]
x
d
＝x-l
r
·
cosθ-l
k
/2
·
sinθ
[0130]
y
d
＝y-l
r
·
sinθ+l
k
/2
·
cosθ
[0131]
则约束条件具体为：
[0132]
起点约束：
[0133]
约束点和规划起点之间的容差在0.01m。
[0134]
约束点和规划起点之间曲率的容差在0.01。
[0135]
车辆顶点约束：
[0136]
为避免车辆左前端点a与道路边界发生碰撞，要求满足y
a
≤l
w
。使得车辆完全倒入，则y
a
最小值为负数。车辆不与车库左壁发生碰撞，x
d
最大值大于0.05。车辆不超出车库下边线，y
d
≥-l
k
。所有点的y值小于-1.1。
[0137]
起点斜率约束：
[0138]
约束点和规划起点之间的容差在0.01。
[0139]
曲率约束：
[0140]
路径曲率不大于后轴中点轨迹曲率，ρ
max
≤1/r
[0141]
碰撞约束：
[0142]
避免车辆右轮廓线bc与车位前障碍物或者车位前方的车辆刮擦，要求，x
c
＞l
c
且x
c
＜l
c
时，tanθ
·
(l
c-x
c
)+y
c
≥0。
[0143]
终点航向约束：
[0144]
为使车辆在泊车结束时轮胎回正，终点航向角0.03。
[0145]
其中，所述s4中变量取值范围以控制点为中心且浮动空间以3为上下限。
[0146]
其中，所述目标函数的计算方法具体为：
[0147]
s21：对航向角、路径长度和泊车曲率进行归一化处理；归一化时，航向角最大取的是0.5rad，路径长度取的最大为10米，曲率总值取的是最小转弯半径是的曲率；
[0148]
s22：对归一化处理后的航向角、路径长度和泊车曲率分别选择相同的权重进行加权；
[0149]
s23：计算出加权之后的最小值，将该最小值作为目标函数；
[0150]
目标函数：d＝φ/0.5+ρ/(1/r)+l/10。
[0151]
其中，对序列的二次规划包括采用sqp求解器对目标函数、约束条件、变量取值范围和变量初始值进行求解。
[0152]
优化结果如图3-6所示，参照图3-6所示，车轮先向右转动方向盘，然后向左转。曲线的最大曲率ρ
max
，此时x轴坐标由图可知。路径长度最长取值与 x轴坐标的关系。与x轴的夹角及x轴坐标的关系。
[0153]
以上仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除