一种公路自动侧方停车路径规划及控制方法

1.本发明涉及自主智能停车技术领域，尤其涉及一种公路自动侧方停车路径规划及控制方法。


背景技术：

2.随着生活水平的不断提高，汽车已经在民众中普及。人工智能、机器视觉和导航等相关技术的不断发展，则促进了自动驾驶汽车走向实用化。然而，对于普通轿车而言，其寻找停车位、停车入位的整个过程，必须依靠驾驶员高超的驾驶技术，或由驾驶员在语音或影像的辅助之下完成，自动化程度低，耗时耗力。同时，公路侧方停车相对较难，其停车准确性受制于驾驶员的熟练度。
3.在现有的自动停车技术中，由于车辆未能进行实时路径规划，且由于公路侧方停车位置和姿态具有随机性，在公路行车环境下，难以寻找合适的停车位；在当前技术下，即使找到了合适的停车位，自动驾驶汽车也难以在无误差或误差很小的情况下完成停车任务，可能出现偏移，歪斜等情况，或是反复调整姿态，难以一步到位；此外，无人驾驶汽车事故频发，自动停车过程的安全性难以得到保障。因此，一种新的公路自动侧方停车方法及车辆是十分必要的。


技术实现要素：

4.为了克服现有技术存在的缺陷与不足，本发明提供了一种公路自动侧方停车路径规划及控制方法，能够自动识别停车位、规划可行路径并按照所设计的控制律完成停车任务，有效提高了停车准确度，具有自主性、智能性、实用性和安全性。
5.为了达到上述目的，本发明采用以下技术方案：
6.一种公路自动侧方停车路径规划及控制方法，包括以下步骤：
7.s01：目标车辆收到侧方停车指令，进入路径规划过程；
8.所述路径规划过程，定义为目标车辆计算决策模块根据本发明提出的路径规划算法进行路径规划的过程；目标车辆计算决策模块可以根据所接收的停车指令的初末位姿自主且智能地规划出前行停车或倒行停车的运动路径。
9.s02：若目标车辆位于可行区域之外，则进入停车引导过程；
10.所述可行区域，定义为目标停车位附近能够产生可行停车轨迹的区域；
11.进一步规定，若目标车辆不位于可行区域，则需要通过驾驶员进行停车引导，使其位于可行区域，并且该引导应使目标车辆容易到达。停车引导过程结束后，将重新进入路径规划过程。
12.s03：若目标车辆位于可行区域，则按照规划出的停车路径，进入停车过程；
13.所述停车过程，定义为目标车辆自动沿轨迹运动至目标车位的过程。停车过程中，目标车辆根据停车控制算法沿规划路径运动，并且在所设计的线速度与角速度控制量下自动地完成停车过程；
14.s04：目标车辆在停车过程中，若检测到干扰，则进入停车等待过程；
15.所述干扰，定义为目标车辆在停车过程中，前进的停车轨迹与其他对象发生干扰的现象；
16.所述停车等待过程，定义为干扰存在时，停车过程暂停，目标车辆保持当前位姿等待干扰消失的过程。干扰消失后，继续进入停车过程；
17.s05：目标车辆到达目标位置后，停车结束；
18.所述目标位置，即为停车位。目标车辆到达目标位置，必须同时满足两个条件：条件一，目标车辆垂直投影的几何中心与停车位的几何中心相重合(或在允许误差范围内)；条件二，目标车辆的车身朝向与停车位几何形状的长边相平行(或在允许误差范围内)。
19.作为本发明一种公路自动侧方停车路径规划及控制方法进一步的优化方案，针对前行停车和倒行停车两类轨迹，所述路径规划算法包括末态确认，初态确认，轨迹解析，可行性分析等步骤；
20.所述末态确认步骤，定义为在所述路径规划过程期间，计算决策模块建立环境模型，确认正常停车位置和姿态，并建立右手笛卡尔坐标系的步骤。正常停车位置和姿态即停车过程的末态。计算决策模块接收传感器模块的信息，并加以处理生成环境模型，以确定目标位置以及目标位置垂直投影的几何中心的位置。以目标位置垂直投影的几何中心为原点，以目标位置车头所指方向为横轴正方向，建立右手笛卡尔坐标系；
21.所述初态确认步骤，定义为传感器模块在所述路径规划过程期间，获取目标车辆自身位置信息和姿态信息的步骤；
22.所述轨迹解析步骤，定义为计算决策模块根据目标车辆的末态和初态，在当前坐标系下规划并计算停车轨迹的步骤；已知轨迹为一段线段与两段圆弧组合，并且第二段圆弧的终点目标位置垂直投影的几何中心即为停车位的几何中心。易知末态确认步骤中所建立的坐标系的横轴为第二段圆弧的一条切线，切点即为目标位置垂直投影的几何中心。则第二段圆弧轨迹的圆心一定在纵轴上运动；
23.根据车辆的初末状态，判断目标车辆适合前行停车或倒行停车；
24.若上述方法无法找到可行路径，即目标车辆位于可行区域之外，则进入停车引导过程。
25.作为本发明一种公路自动侧方停车路径规划及控制方法进一步的优化方案，所述停车控制算法，定义为设计目标车辆的线速度和角速度，使车辆能够精确地沿规划路径运动，且无需规划路径期望的角速度和线速度。
26.在两类路径中，目标车辆将采用以下的切换控制模式或连贯控制模式：
27.所述切换控制模式，指在线段轨迹与第一段圆弧轨迹交接处，第一段圆弧轨迹与第二段圆弧轨迹交接处分别切换控制律和末状态。
28.(1)直线轨迹的控制模式：
29.由于直线运动是最简单的运动形式，因此采取匀速直线运动的运动模式，线速度为v
l
，保证v
l
≤v
max
即可；
30.(2)第一段圆弧轨迹的控制模式：
31.为了便于计算车辆行驶于圆弧轨迹上的控制律，定义车辆在任意时刻的状态为：
[0032][0033]
则末状态与实时状态之间的偏差表示为：
[0034][0035]
其中，gd由第一段圆弧轨迹运动的末状态o
″
点的位姿构成。
[0036]
设计目标车辆具有饱和特性的线速度、角速度的控制律如下：
[0037]
v＝-k1tanh[xe(t)cosθe(t) ye(t)sinθe(t)]
[0038][0039]
其中，k1≤vm，|k1 k2|≤ωm，|k
1-k2|≤ωm，以满足输入饱和约束的条件。
[0040]
通过构造李雅普诺夫函数：
[0041][0042]
可以证明，目标车辆镇定于目标位置是稳定的；
[0043]
(3)第二段圆弧轨迹的控制模式：
[0044]
将末状态切换至目标车位；
[0045]
控制律模式与第一段圆弧轨迹完全相同，若ge满足
[0046][0047]
则说明目标车辆已经运行至第二段圆弧轨迹的末端，即目标停车位处，停车过程结束。
[0048]
所述切换控制模式能够提高停车过程的准确度，然而车辆在切换点处必须完全静止，导致停车过程不连贯。为了弥补上述不足，目标车辆也可以采用连贯控制模式：
[0049]
所述连贯控制模式，指目标车辆在线段和第一段圆弧轨迹处采用恒定线速度和角速度进行运动，行驶至第二段圆弧轨迹时在采用切换控制模式中所述圆弧轨迹的控制律实现停车入库。采用切换控制模式或连贯控制模式可根据环境要求调整。
[0050]
值得说明的是，目标车辆从开始运动至到达目标位置，其三维空间中任何一点不得与环境干扰。
[0051]
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
[0052]
1.本发明能够自动寻找停车位，具有实时性。目标车辆装有传感器模块，通过激光雷达实现对周围环境的建模。公路两侧的停车位多为直停车位，其尺寸为宽2.5m，长大于等于5m，一般标准是2.5m*5.3m。而16线激光雷达在100m的测量距离内，至少能够保证2cm的精度，其精度远小于停车位尺寸。并且16线激光雷达拥有32万点每秒的实时出点数，确保车辆能够实时更新地图信息，并确定停车位的位置；
[0053]
2.本发明能够自动实现侧方停车过程。本发明在停车过程中完全不需要人工干预，实现停车入位全自动，自动化程度高。操作者只需要下达侧方停车指令，目标车辆便会
自动寻找车位、规划路线，然后沿规划路径自主行驶至目标车位内；
[0054]
3.本发明的算法是完备的，可以运用于所有情况。针对公路停车时可能遇到的所有情况，本发明提出了前行停车和倒行停车两种停车模式，确保目标车辆位于可行区域内，在正常驾驶的情况下，总能完成停车过程；
[0055]
4.本发明针对侧方停车过程中普遍倒行的状况，提出了前行停车和倒行停车两种停车模式，并且在允许的情况下，优先采用前行停车模式，简化了停车过程；在前行停车无法完成相应停车任务的情况下，再采用倒行停车模式；
[0056]
5.本发明具有安全性。在整个自动停车过程中，当目标车辆检测到干扰时，会立即停止停车过程，通过中断转入停车等待过程，直到干扰消失才重新回到停车过程，进而保证目标车辆运行的安全性，即不会对目标车辆自身造成损伤，也不会危及其他人身财产安全；另外，所述目标车辆具有速度饱和约束，即具有最大线速度和最大角速度，进一步保证了行车过程的安全性。
附图说明
[0057]
图1是本发明的方法流程图；
[0058]
图2是本发明路径规划算法的流程图；
[0059]
图3是本发明轨迹解析步骤的流程图；
[0060]
图4是本发明前行停车的示意图；
[0061]
图5是本发明倒行停车的示意图；
具体实施方式
[0062]
下面结合附图对本发明的技术方案做进一步的详细说明：
[0063]
本发明可以以许多不同的形式实现，而不应当认为限于这里所述的实施例。相反，提供这些实施例以便使本公开透彻且完整，并且将向本领域技术人员充分表达本发明的范围。
[0064]
针对全部可能的两类路径，所述停车算法包括末态确认，初态确认，轨迹解析，可行性分析等步骤；
[0065]
所述末态确认步骤，定义为在所述路径规划过程期间，计算决策模块建立环境模型，确认正常停车位置和姿态，并建立右手笛卡尔坐标系的步骤。正常停车位置和姿态即停车过程的末态，该状态是确定的，即所述目标位置。将o
′
定义为目标位置垂直投影的几何中心。计算决策模块接收传感器模块的信息，并加以处理生成环境模型，以确定目标位置以及o
′
的位置。以o
′
(0，0，0)为原点，以目标位置车头所指方向为横轴正方向，建立右手笛卡尔坐标系，横纵轴分别记作x
′
和y
′
；
[0066]
所述初态确认步骤，定义为传感器模块在所述路径规划过程期间，获取目标车辆自身位置信息和姿态信息的步骤；将o定义为目标车辆起始位置垂直投影的几何中心，将圆弧轨迹的圆心角记为θ
′
。
[0067]
所述轨迹解析步骤，定义为计算决策模块根据目标车辆的末态和初态，在当前坐标系下规划并计算停车轨迹的步骤；已知轨迹为一段线段与两段圆弧组合，并且第二段圆弧的终点o
′
即为停车位的几何中心。易知末态确认步骤中所建立的坐标系的横轴x
′
为第二
段圆弧的一条切线，切点为o
′
。记第二段圆弧轨迹的圆心为c
′
(m1′
，n1′
)，半径为r
′
；则c
′
一定在纵轴y
′
上运动，且：
[0068][0069]
记r
′
为第二段圆弧的半径，将r
′
分为m等份，则n
′
，r
′
的计算方法为：
[0070][0071]
其中k为整数且k∈(0，m]，依次将k从1取到m。
[0072]
记c
′
(0，n
′
)为第二段圆弧圆心。取第二段圆弧中与目标车辆同侧的劣弧，将该段劣弧圆心角记为θ
′
，θ
′
∈(0，π/2)，θ
′
的计算方法为：
[0073][0074]
其中p、q均为正整数且q∈(0，p]，依次将q从1取到p。
[0075]
每确定一个k，相对应将确定p个q；每确定一组k和q，相对应将确定一个o
″
。易知共能够确定m
p
个o
″
点。在这些点中，选取在误差范围内合适的点，作为末段圆弧的停车起点，也是第一段圆弧的停车终点。
[0076]
由此确定的目标车辆第二段圆弧轨迹为：
[0077]
x
′2 (y
′‑r′
)2＝r
′2，0≤θ≤θ
′
[0078]
其中θ为目标车辆在t时刻时质心位置与目标位置所夹圆弧轨迹的圆心角，容易证明，θ也是任意瞬时车辆速前进方向与x
′
轴夹角；
[0079]
将以r
′k和θ
′
为半径和圆心角所作的圆弧终点记为o
″
，将原坐标系的原点o
′
移动至o
″
处，并以o
″
处车头所指方向为横轴正方向，建立右手笛卡尔坐标系，横纵轴分别记作x
″
和y
″
。第二段轨迹的起点作为第一段轨迹的停车终点，用同样的方法寻找第一段圆弧轨迹，并且使该圆弧与目标车辆位于中间位置和初始位置时前进方向的延长线同时相切，则一段线段加两段外切的圆弧构成停车轨迹。
[0080]
第一段圆弧位于x
″o″y″
坐标系下，圆心记为c
″
，半径为r
″
；第二段圆弧位于x
′o′y′
坐标系下，圆心记为c
′
，半径为r
′
。并记第一段圆弧与线段的切点为a
″
(x1″
，y1″
)，初始位置在x
″o″y″
坐标系下坐标为o(x0″
，y0″
)；
[0081]
此时目标车辆轨迹为：
[0082][0083]
其中，θ
″
为圆c
″
在o处切线与x
″
轴夹角，θ1为目标车辆在第一段圆弧上t时刻转过的角度，θ2为目标车辆在第一段圆弧上t时刻转过的角度；
[0084]
上述轨迹即为前行停车路径，目标车辆沿前行停车路径执行停车任务时，采取前行停车的模式。
[0085]
所述目标车辆必须满足：从目标车辆开始运动至到达目标位置，其三维空间中任何一点不与环境发生干扰。其计算方法为：选取位于目标车辆投影几何中心与c
′
连线上，且
位于目标车辆投影最外侧和最内侧的一点，分别记作q、p，将q与c
′
之间的距离记作rq′
，将p与c
′
之间的距离记作r
p
′
，则q、p两点的运动轨迹为：
[0086]
x
′2 (y
′‑r′‑r′q)2＝(r
′
r
′q)2，0≤θ≤θ
′
[0087]
x
′2 (y
′‑r′
r
′
p
)2＝(r
′‑r′
p
)2，0≤θ≤θ
′
[0088]
若目标车辆满足：
[0089]
任意时刻θ、p两点连线与环境均无干扰。
[0090]
则可以推知目标车辆上任何一点都不会与环境发生干扰，否则该轨迹不符合要求；
[0091]
若上述轨迹不可行，则在目标车辆相对目标位置的另一侧，进行第一类轨迹分析，得到两圆轨迹的中间位置，再进行坐标变换，并作圆使该圆与目标车辆位于中间位置和初始位置时前进方向的延长线同时相切，则一段线段加两段内切的圆弧构成停车轨迹。
[0092]
值得说明的是，此时停车轨迹在形式上与前行停车路径完全相同，而两段圆弧轨迹由外切变为内切，停车方式由前行停车变为倒行停车。
[0093]
上述轨迹即为倒行停车路径，即目标车辆采取倒车入库的模式。
[0094]
若以上轨迹均不可行，则目标车辆位于可行区域之外，则进入停车引导过程。
[0095]
在两类路径中，目标车辆将采用以下的切换控制模式或连贯控制模式：
[0096]
所述切换控制模式，指在线段轨迹与第一段圆弧轨迹交接处，第一段圆弧轨迹与第二段圆弧轨迹交接处分别切换控制律和末状态。
[0097]
设目标车辆到达初始位置时的时刻为初始时刻，记作t0；此时车辆的状态为初始状态，在x
′o′y′
坐标系下表示为o(x0′
，y0′
，θ0′
)；
[0098]
将目标车辆到达匀速直线运动的末状态时的时刻记作t1；匀速直线运动的末状态即车辆位于直线轨迹的末端(或第一段圆弧的始端)时的状态，该状态同样为车辆在第一段圆弧运动时的初始状态，在x
′o′y′
坐标系下表示为a(x1′
，y1′
，θ1′
)。
[0099]
将目标车辆到达第一段圆弧末端时的时刻记作t2；目标车辆第一段圆弧轨迹上的末状态即车辆位于第一段圆弧末端时的状态，该状态同样为车辆在第二段圆弧轨迹上的初始状态，在x
″o″y″
坐标系下表示为o
″
(0，0，0)，在x
′o′y′
坐标系下表示为o
″
(x
′2，y
′2，θ
′2)；
[0100]
(1)直线轨迹的控制模式：
[0101]
由于直线运动是最简单的运动形式，因此采取匀速直线运动的运动模式，线速度为v
l
，保证v
l
≤v
max
即可；
[0102]
当传感器模块检测到车辆当前状态接近a(x1′
，y1′
，θ1′
)时，将减速至a处，切换第一段圆弧轨迹对应的控制律和末状态。
[0103]
(2)第一段圆弧轨迹的控制模式：
[0104]
为了便于计算车辆行驶于圆弧轨迹上的控制律，定义车辆在任意时刻的状态为：
[0105][0106]
则末状态与实时状态之间的偏差表示为：
[0107][0108]
其中，gd由第一段圆弧轨迹运动的末状态o
″
点的位姿构成。
[0109]
车辆质心相对于坐标原点的角度记作α，则
[0110]
α＝2arctan(ye(t)/xe(t))
[0111]
并记α和θe(ti)之间的姿态误差为αe，则
[0112]
αe＝θe(t)-α
[0113]
设计目标车辆具有饱和特性的线速度、角速度的控制律如下：
[0114]
v＝-k1tanh[xe(t)cosθe(t) ye(t)sinθe(t)]
[0115][0116]
其中，k1≤vm，|k1 k2|≤ωm，|k
1-k2|≤ωm，以满足输入饱和约束的条件。
[0117]
通过构造李雅普诺夫函数：
[0118][0119]
其沿运动轨迹和控制输入的时间导数为：
[0120][0121]
根据拉萨尔不变集原理(la salle
′
s invariance principle)，当t
→
∞时，闭环系统是稳定的。
[0122]
可以证明，目标车辆镇定于目标位置是稳定的；
[0123]
设计的线速度控制律和角速度控制律在调整车辆走向和轨迹方面起着重要的作用。设计的角速度能够将目标车辆的运动方向控制到理论轨迹的各点切线上，最终使姿态偏差达到并保持为零(即)；
[0124]
若ge满足
[0125][0126]
则说明目标车辆已经运行至第一段圆弧轨迹的末端，准备切换至第二段圆弧轨迹的控制模式，易知此时t＝t2。
[0127]
(3)第二段圆弧轨迹的控制模式：
[0128]
将末状态切换至目标车位；
[0129]
控制律模式与第一段圆弧轨迹完全相同，若ge满足
[0130][0131]
则说明目标车辆已经运行至第二段圆弧轨迹的末端，即目标停车位处，停车过程结束。
[0132]
所述切换控制模式能够提高停车过程的准确度，然而车辆在切换点处必须完全静止，导致停车过程不连贯。为了弥补上述不足，目标车辆也可以采用连贯控制模式。
[0133]
所述连贯控制模式，指目标车辆在线段和第一段圆弧轨迹处采用恒定线速度和角速度进行运动，行驶至第二段圆弧轨迹时在采用切换控制模式中所述圆弧轨迹的控制律实现停车入库。采用切换控制模式或连贯控制模式可根据环境要求调整。
[0134]
值得说明的是，上述控制律适用于前行停车和倒行停车两类停车模式；两类停车模式的差别仅在于两段圆弧轨迹切向的不同。
[0135]
本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
[0136]
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。