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

技术特征：
1.一种公路自动侧方停车路径规划及控制方法，其特征在于，应用本方法的目标车辆由车辆主体、传感器模块、计算决策模块、控制模块组成；所述目标车辆，定义为具有非完整约束和速度饱和约束，即不能侧向移动且自身线速度、角速度饱和受限的轮式车辆，其最大线速度v
max
和最大角速度ω
max
视具体的车辆型号与环境要求而定；具体表现为：所述非完整约束是指速度变量数小于位形维数且满足波发夫(pfaffian)约束的不可积系统；其特征体现在目标车辆几何中心可以与二维平面的任何一点重合，却只能沿轮胎滚动方向运动，而不能侧向滑动，即存在如下的约束方程：运动学模型为：其中，(x，y，θ)为目标车辆的实时位置和姿态，v(t)为目标车辆运动的实际线速度，ω(t)为目标车辆的角速度；所述速度饱和约束是由目标车辆自身物理性质及环境导致的车辆速度阈值，具体表现为，车辆停车过程中具有最大线速度v
max
，最大角速度ω
max
，视具体的车辆型号与环境要求而定；所述车辆主体为一般的小轿车；所述传感器模块由16线激光雷达和惯性测量单元(imu)组成；所述计算决策模块为嵌入到目标车辆内部的微处理器，与传感器模块和控制模块电气相连，用于接收传感器模块传来的数据，并加以处理，以建立实时环境和路径；所述控制模块接收来自于微处理器的控制信息，并控制车辆主体的运动。2.根据权利要求1所述一种公路自动侧方停车路径规划及控制方法，其特征在于，车辆将目标停车位的位姿定义为车辆的末位姿，记为(x，y，θ)＝[0，0，0]；通过末位姿在给定算法下自主地规划出停车路径；所述停车路径是由一段直线和两段相切的圆弧组合而成，且目标车辆不需要计算所规划路径上的期望线速度和角速度，可以根据停车指令自动完成停车过程。3.根据权利要求1和权利要求2所述一种公路自动侧方停车路径规划及控制方法，其特征在于，包括：301.目标车辆收到侧方停车指令，进入路径规划过程；所述路径规划过程，定义为目标车辆计算决策模块根据本发明提出的路径规划算法进行路径规划的过程；目标车辆计算决策模块可以根据所接收的停车指令的初末位姿自主且智能地规划出前行停车或倒行停车的可行路径；302.若目标车辆位于可行区域之外，则进入停车引导过程；所述可行区域，定义为停车位附近能够产生可行停车轨迹的区域；进一步规定，目标车辆若不位于可行区域，则需要通过驾驶员进行停车引导，使其位于可行区域，并且该引导应使目标车辆容易到达；停车引导过程结束后，将重新进入路径规划
过程；303.若目标车辆位于可行区域，则按照规划出的停车路径，进入停车过程；所述停车过程，定义为目标车辆自动沿轨迹运动至目标车位的过程；停车过程中，目标车辆根据停车控制算法沿规划路径运动，并且在所设计的线速度与角速度控制量下自动地完成停车过程；304.目标车辆在停车过程中，若传感器模块检测到干扰，则进入停车等待过程；所述干扰，定义为目标车辆在停车过程中，前进的停车轨迹与其他对象发生扰动的现象；所述停车等待过程，定义为干扰存在时，停车过程暂停，目标车辆保持当前位姿等待干扰消失的过程；干扰消失后，目标车辆按照先前的停车算法，继续进入停车过程；305.目标车辆到达目标位置后，停车结束；所述目标位置，即为停车位；目标车辆到达目标位置，必须同时满足两个条件：条件一，目标车辆垂直投影的几何中心与停车位的几何中心相重合(或在允许误差范围内)；条件二，目标车辆的车身朝向与停车位几何形状的长边相平行(或在允许误差范围内)。4.根据权利要求3所述的一种公路自动侧方停车路径规划及控制方法，其特征在于，针对前行停车和倒行停车两类轨迹，权利要求301所述路径规划算法包括末态确认，初态确认，轨迹解析，可行性分析等步骤；所述末态确认步骤，定义为在所述路径规划过程期间，计算决策模块建立环境模型，确认正常停车位置和姿态，并建立右手笛卡尔坐标系的步骤；正常停车位置和姿态即停车过程的末态，该状态是确定的，即权利要求2和权利要求305中所述目标位置；计算决策模块接收传感器模块的信息，并加以处理生成环境模型，以确定目标位置以及目标位置垂直投影的几何中心的位置；以目标位置垂直投影的几何中心为原点，以目标位置车头所指方向为横轴正方向，建立右手笛卡尔坐标系；所述初态确认步骤，定义为传感器模块在所述路径规划过程期间，获取目标车辆自身位置信息和姿态信息的步骤；所述轨迹解析步骤，定义为计算决策模块根据目标车辆的末态和初态，在当前坐标系下规划并计算停车路径的步骤；根据权利要求3，已知轨迹为一段线段与两段圆弧组合，并且第二段圆弧的终点目标位置垂直投影的几何中心即为停车位的几何中心；易知末态确认步骤中所建立的坐标系的横轴为第二段圆弧的一条切线，切点即为目标位置垂直投影的几何中心；则第二段圆弧轨迹的圆心一定在纵轴上运动；根据车辆的初末状态，判断目标车辆适合前行停车或倒行停车；目标车辆的轨迹形式为一段线段加两端圆弧，具体表现为：第一段圆弧位于x
″
o
″
y
″
坐标系下，圆心记为c
″
，半径为r
″
；第二段圆弧位于x
′
o
′
y
′
坐标系下，圆心记为c
′
，半径为r
′
；并记第一段圆弧与线段的切点为a
″
(x1″
，y1″
)，初始位置在x
″
o
″
y
″
坐标系下坐标为o(x0″
，y0″
)；此时目标车辆轨迹为：
其中，θ
′
为圆c
′
在o
″
处切线与x
′
轴夹角，θ
″
为圆c
″
在a
″
处切线与x
″
轴夹角，θ1为目标车辆在第一段圆弧上转过的角度，θ2为目标车辆在第一段圆弧上转过的角度；值得说明的是，目标车辆从开始运动至到达目标位置，其三维空间中任何一点不得与环境发生干扰；若上述方法无法找到可行路径，即目标车辆位于可行区域之外，则进入停车引导过程。5.根据权利要求1所述的一种公路自动侧方停车路径规划及控制方法，其特征在于，权利要求303所述路径规划算法，定义为设计目标车辆的线速度和角速度，使车辆能够精确地沿停车规划路径运动；在两类路径中，目标车辆将采用以下的切换控制模式或连贯控制模式：所述切换控制模式，指在线段轨迹与第一段圆弧轨迹交接处，第一段圆弧轨迹与第二段圆弧轨迹交接处分别切换控制律和末状态；(1)直线轨迹的控制模式：由于直线运动是最简单的运动形式，因此采取匀速直线运动的运动模式，线速度为v
l
，保证v
l
≤v
max
即可；(2)第一段圆弧轨迹的控制模式：为了便于计算车辆行驶于圆弧轨迹上的控制律，定义车辆在任意时刻的状态为：则末状态与实时状态之间的偏差表示为：其中，g
d
由第一段圆弧轨迹运动的末状态o
″
点的位姿构成；设计目标车辆具有饱和特性的线速度、角速度的控制律如下：v＝-k1tanh[x
e
(t)cosθ
e
(t) y
e
(t)sinθ
e
(t)]其中，k1≤v
m
，|k1 k2|≤ω
m
，|k
1-k2|≤ω
m
，以满足输入饱和约束的条件；通过构造李雅普诺夫函数：可以证明，目标车辆镇定于目标位置是稳定的；(3)第二段圆弧轨迹的控制模式：将末状态切换至目标车位；控制律模式与第一段圆弧轨迹完全相同，若g
e
满足
则说明目标车辆已经运行至第二段圆弧轨迹的末端，即目标停车位处，停车过程结束；所述切换控制模式能够提高停车过程的准确度，然而车辆在切换点处必须完全静止，导致停车过程不连贯；为了弥补上述不足，目标车辆也可以采用连贯控制模式：所述连贯控制模式，指目标车辆在线段和第一段圆弧轨迹处采用恒定线速度和角速度进行运动，行驶至第二段圆弧轨迹时在采用切换控制模式中所述圆弧轨迹的控制律实现停车入库；采用切换控制模式或连贯控制模式可根据环境要求调整；通过所设计的控制律，使自动停车车辆能够在无误差或误差很小的情况下实现自动停车任务。

技术总结
本发明公开了一种公路自动侧方停车路径规划及控制方法，属于自主智能停车技术领域。所述方法包括以下步骤：当无人驾驶车辆接收到自动停车指令后，进入路径规划过程，利用路径规划算法自动规划可行的停车路径；若目标车辆所在区域能够规划出可行的停车路径，则进入停车过程，利用停车控制算法沿所述停车路径执行自动停车任务；若目标车辆所在区域因环境约束无法规划出可行的停车路径，则进入停车引导过程，引导驾驶员前往可行区域；此外，本发明在停车过程中，若环境出现干扰，则进入停车等待过程，直到干扰消失。本发明所述方法无需进一步获取路径上的期望速度，所设计的停车控制算法提高了停车准确度，使车辆能够自主地完成停车任务，具有智能性、实用性和安全性。实用性和安全性。实用性和安全性。


技术研发人员：彭秀辉 解元博 王增茂 陈啸林 王从庆
受保护的技术使用者：南京航空航天大学
技术研发日：2022.03.03
技术公布日：2022/7/29