一种基于分层规划的自动泊车方法及辅助系统与流程

本发明涉及一种智能汽车自动泊车辅助系统，尤其是涉及一种基于分层规划的自动泊车方法及辅助系统。

背景技术：

泊车对于驾驶员来说从来都不是一件容易的事。由于驾驶员在驾驶舱中视角受限，对后方和侧方的车身周围情况无法直观把控，泊车过程需要经常采用后退、拐弯等难度较高的操作，稍有不慎便会有磕碰产生，造成财产损失，甚至安全事故。而随着城市地价的日益攀升，城市停车位也日益狭窄，对驾驶员来说，手动停车比以前更为困难。如果停车不够好，可能会扰乱公共停车资源的正常使用，甚至可能造成交通堵塞。此外，停车经验不足的司机可能不愿意使用狭窄的泊车位，从而不得不绕远路寻找泊车位，造成额外的能源损失、空气污染与交通拥堵。

为了减轻手动停车的负担，汽车制造商开发了自动停车辅助系统。自从自动停车辅助系统商用以来，众多汽车生产企业也纷纷将自动停车系统投放市场。尽管自动泊车技术蓬勃发展，该技术目前仍不成熟。

传统的自动泊车系统中的轨迹规划方法通常采用几何法，它通过获取车辆、周围障碍物以及目标库位的几何关系，求取车辆在当前环境中的可行路线。该方法对环境的要求很高，具体表现为车辆初始位置、初始航向角、障碍物等等。通常每一种几何运算方式只适合某一种或一类环境，适应性较差。基于搜索的a*算法非常适用于非结构化道路的无人车路径规划，在泊车工况下具有其独特的算法优势，然而由于泊车环境较狭窄，路径精度要求高，基于搜索的传统无人车路径规划方法搜索失败可能性较大，且需要对周围环境地图进行高精度离散，极大影响算法实时性，在泊车环境并不适用。

因此，如何解决传统泊车策略所带来的问题，有效利用搜索算法的优势，是本领域技术人员急需解决的问题。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于分层规划的自动泊车方法及辅助系统。

本发明的目的可以通过以下技术方案来实现：

一种基于分层规划的自动泊车方法，包括以下步骤：

s1：启动车辆自动泊车模式，开启自动泊车辅助hmi，通过环视摄像头获取周围环境信息并投放至车辆hmi屏幕上，同时以车辆为原点离散周围环境地图，通过激光雷达扫描障碍物，并将障碍物点云信息投影到离散地图上。

s2：驾驶员驾驶车辆缓慢行驶于泊车区域，搜寻目标库位，判断库位类型、库位大小以及库位内是否有障碍物。

驾驶员驾驶车辆缓慢搜寻目标库位过程中，地图随车辆移动，坐标系始终取车辆后轴中心为原点，建立分别以车辆行驶方向、车辆右方为正方向的二维平面栅格坐标系；库位检测同时检测库位尺寸、库位内部是否存在障碍物及库位类型，其中库位尺寸根据环视摄像头检测库位角点判断，库位内部障碍物通过激光雷达判断，库位类型根据车辆当前航向角与库位角点投影到离散地图上的几何关系进行判断，库位类型包含垂直库位、平行库位与斜库位。

s3：若库位尺寸满足要求，且库位内无障碍物，发送指令至车辆hmi，请求驾驶员判断是否以该库位泊车，若是，则进入步骤s4，否则返回步骤s2。

s4：根据选定泊车位的类型，判断泊车终点位置，获取终点车辆航向角。

s5：依据车辆当前位姿、终点位姿以及周围障碍物信息，在离散环境地图上规划起始点到目标点的路径；具体内容包括：

51)利用数值优化方法进行当前位姿到目标库位的初始规划，随后进行如下判断：

判断1：判断能否成功规划出路径，若能规划出初始路径，则进入判断2，否则hmi提示无法成功规划路径的信息后，返回步骤s2；

判断2：判断规划出的路径是否满足泊车需求，若能满足需求，则进入步骤s6，否则进入步骤52)；

52)根据当前信息，在离散地图里利用hybrida*方法获取一次规划路径。

满足需求指满足安全需求与轨迹可行性需求。安全需求指规划出的轨迹与障碍物距离需大于一定阈值。可行性需求指规划出的轨迹应平滑、曲率连续、转向角不宜过大。

优选地，步骤51)中，数值优化规划方法具体包括以下步骤：

511)根据离散地图上车辆当前位置与障碍物位置，构建环境地图上车辆方程障碍物方程无碰撞方程minx,y||x-y||2＞dmin，并依据优化理论建立对偶问题方程组

式中，x、y为车辆后轴中心坐标；et、om均为定义在r²上的多边形；a、b为车辆位置矩阵，a∈r^l·n，b∈r^l；cm、dm为障碍物位置矩阵，n为空间维度；l、k为组成凸集的超平面个数，λ、μ均为对偶优化问题拉格朗日变量。

512)根据障碍物角点坐标建立超平面方程组，求取cm、dm，根据车辆当前位姿求取a、b：

其中，为车辆航向角，xt、yt为t时刻车辆后轴中心坐标，e1、e2、e3为车辆几何尺寸。

513)建立状态迭代方程式：

设定优化目标函数：

其中，为系统在t(t∈{t0,t1,…,tn})时刻的所有状态变量，ut为系统在t(t∈{t0,t1,…,tn})时刻的所有输入变量，包括加速度a，方向盘转角δ，zs表示系统初始状态，zf表示系统目标状态，tf为整个过程所需时间，n为离散状态个数，top为相邻两个状态间的时间差，p、q分别为时间与状态输入量的优化目标权重。

514)将上述方程组代入开源数值优化求解器中，求解泊车路径及其控制点参数，即基于数值优化的规划方法规划出泊车起点至目标点的n个中间状态点，包含车辆六个状态，包括车辆后轴坐标x、y，速度v、加速度a、转向角δ、车辆航向角θ。

基于数值优化的初始规划成功与否，直接决定当前车辆是否能顺利泊车。初始规划失败，放弃当前车位，搜寻下一个可行泊车位。

优选地，步骤52)中，利用hybrida*方法进行规划具体包括下列内容：

521)根据离散地图上障碍物与目标点的位置，获取离散地图里所有格子在考虑障碍物的情况下的a*heuristic1值；

522)基于离散地图上障碍物与目标点的位置，根据reedsshepp曲线原理求取格子所有的点到目标点的reedsshepp线的最短长度，其长度值作为格子的heuristic2值，栅格地图中每一个格子的heuristic值为heuristic1值与heuristic2值之和。

523)从起点开始向四周格子进行扩展，扩展格子cost的值为父格子与子格子的长度差加上角度差；

524)拓展所有从起点至终点cost+heuristic值最小的格子并将其连接起来，生成hybrida*路径。

优选地，hybrida*一次规划的结果包含路径点的控制点，即车辆后轴中心坐标x，y，将此x，y值作为优化初值，代入数值优化二次规划，输出最终包含速度v、加速度a、转向角信息δ的控制点x，y；成功规划出路径后，若驾驶员采用，则自动泊车控制模块接收规划模块发出的路径点信息，接管车辆行驶。

优选地，控制模块接管车辆驾驶过程中，环境地图不再随车辆移动，原点为车辆泊车起点，坐标轴正方向为泊车起点航向，即泊车起点车辆航向角θ设为0。

s6：规划模块将规划路径传入hmi显示模块，驾驶员根据已规划出的路径对是否采用该规划路径进行决策，若是，则进入步骤s7，否则，返回步骤s2；

s7：控制模块根据规划模块获取的轨迹与车速信息控制车辆入库，开启激光雷达，实时检测车辆轨迹周围是否存在障碍物，若是，进入步骤s8，否则，进入步骤s9；

s8：车辆hmi显示模块提示已检测到障碍物，驾驶员决定是否等待障碍物离开，若是，延时若干时间，返回步骤s7，若否，车辆沿走过的规划路径原路返回，且返回步骤s2；

s9：车辆到达泊车终点位置，控制模块跟踪完成所规划出的路径，退出自主泊车模式，泊车结束。

一种自动泊车辅助系统，该系统包括：

感知模块，包括环视摄像头和激光雷达，所述的环视摄像头用于识别库位，并投影于离散地图上；所述的激光雷达用于泊车过程中检测环境障碍物，并将障碍物点云信息投影于离散地图上，同时接管驾驶过程中的实时碰撞检测；

hmi显示模块，用于显示所检测到库位信息、离散地图、所规划路径及控制模块跟踪情况；驾驶员通过hmi模块确认库位，确认规划路径，遇到障碍物是否等待；

路径规划模块，该模块设有基于数值优化的规划方法单元与基于a*搜索的规划方法的处理单元，路径规划模块的输入信息为车辆起始点以及目标点的坐标值与航向角，输出信息为泊车路径，且包含车辆在每个离散控制点的车辆坐标、航向角、速度、加速度、车辆转向角；

车辆控制跟踪模块，包括控制器与ecu，用于接受规划模块输出的离散轨迹点信息以及参考控制量，通过控制器控制车辆的油门踏板与方向盘转角，控制车辆行驶。车辆控制模块跟踪完成最后一个控制点后，或者跟踪失败返回车辆泊车起点后，车辆控制跟踪模块不再接管车辆行驶。

优选地，所述的感知模块包括4个环视摄像头与2个激光雷达。环视摄像头装配于车辆右后视镜下方、右尾灯旁、左后视镜下方、左尾灯旁四个地方；2个激光雷达分别装配于车辆顶部右端与顶部左端。优选地，环视摄像头采用双目摄像头匹配的方法推算库位大小，并投影于离散地图上。

与现有技术相比，本发明具有以下优点：

1、本发明提供了一种结合搜索和数值优化的自动泊车规划方法，适用于城市停车场环境下库位线明显的条件，环境适应性强，轨迹计算更精确，与传统自动泊车规划方法相比，在库位狭小、库位角度不规则、环境障碍物较复杂等条件下表现出更为显著的优势；

2、本发明根据当前环境信息的描述，规划出的轨迹包含车辆所处状态点的速度与加速度，无需再对车辆进行速度加速度规划；

3、本发明能够很好处理环境障碍物发生变化的情况，车辆泊车过程中，若探测到车辆规划路径上存在障碍物且障碍物不再移动，车辆自动回到泊车起始位置，搜寻下一个泊车位，有效保障了自动泊车系统的安全性以及过程连续性。

附图说明

图1为本发明自动泊车方法的过程流程图；

图2为本发明感知模块生成的离散地图示意图；

图3为本发明的规划路径示意图；

图4为本发明的车辆位姿示意图；

图5为本发明自动泊车辅助系统的模块框架示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

本发明涉及一种基于分层规划的自动泊车方法，如图1所示，该方法包括下列步骤：

步骤1：驾驶员启动车辆自动泊车模式，启动自动泊车辅助hmi，开启车辆环视摄像头，开启激光雷达，将环视摄像头所拍摄的周围环境投放至车辆hmi屏幕上，并以车辆为原点离散周围环境地图，并将激光雷达所扫描到的障碍物点云信息投影到离散地图上。

如图2所示，离散环境地图以车辆后轴中心为原点，以车辆前进方向和后轴向右方向为坐标轴正方向，该离散地图应包含目标库位信息与环境障碍物信息，并随车辆移动。

步骤2：驾驶员驾驶车辆缓慢行驶于泊车区域，搜寻目标库位。布置于车辆四周的环视摄像头以库位角点为特征点，搜索可泊车库位，并获取目标库位的两个角点坐标值，判断库位类型(平行库位、垂直库位、斜库位)、库位大小以及库位内是否有障碍物。

步骤3：若库位尺寸满足要求，且库位内无障碍物，发送车辆hmi，请求驾驶员判断是否以该库位泊车，若是，进入步骤4；若否，返回步骤2。

步骤4：根据选定泊车位的类型，判断泊车终点位置，推算终点车辆航向角。

步骤5：在离散环境地图上，依据车辆当前位姿、终点位姿以及周围障碍物信息，规划起始点到目标点的路径，具体步骤包括：

步骤51：利用数值优化方法，做当前位姿到目标库位的初始规划，随后进行如下判断：

a)判断1：能否成功规划出路径？若能规划出初始路径，进入判断2，否则hmi提示信息“无法成功规划路径”，返回步骤2。

b)判断2：规划出的路径是否满足泊车安全需求？若能满足需求，进入步骤6，否则进入步骤52。

其中，数值优化规划方法具体步骤包括：

步骤511：根据离散地图上车辆当前位置与障碍物位置，构建环境地图上车辆方程：以及障碍物方程无碰撞方程minx,y||x-y||2＞dmin，并依据优化理论建立对偶问题方程组‖a^tλ‖*≤1。

式中，et，om均为定义在r²上的多边形，a、b为描述车辆位置的矩阵，cm、dm为描述障碍物位置的矩阵；a∈r^l·n，b∈r^l，n为空间维度；l、k为组成凸集的超平面个数，λ、μ均为对偶优化问题拉格朗日变量。a、b需要通过约束计算。

步骤512：根据障碍物角点坐标建立超平面方程组求取cm、dm，根据车辆当前位姿求取a、b：

其中，各项车辆参数如图4所示，为车辆航向角，xt、yt为车辆后轴中心坐标，e1、e2、e3为车辆几何尺寸。

步骤513：建立状态迭代方程式：

设定优化目标函数：

其中，为系统在t(t∈{t0,t1,…,tn})时刻的所有状态变量，ut为系统在t(t∈{t0,t1,…,tn})时刻的所有输入变量，包括加速度a，方向盘转角δ，zs表示系统初始状态，zf表示系统目标状态，f为整个过程所需时间，n为离散状态个数，top为相邻两个状态间的时间差，p、q分别为时间与状态输入量的优化目标权重。

步骤514：将上述方程组代入开源数值优化求解器ipopt中，求解泊车路径及其控制点参数：车辆后轴中心坐标x、y，速度v、加速度a、转角δ、车辆航向角θ。

步骤52：根据当前信息，在离散地图里利用hybrida*方法求出一次规划路径。

具体内容为：

步骤521：根据离散地图上障碍物与目标点的位置，求取离散地图里所有格子在考虑障碍物的情况下的a*heuristic1值；

步骤522：根据离散地图上障碍物与目标点的位置，根据reedsshepp曲线原理求取格子所有的点到目标点的reedsshepp线的最短长度，其长度值作为格子的heuristic2值。栅格地图中每一个格子的heuristic值为heuristic1值与heuristic2值之和；

步骤523：从起点开始向四周格子进行扩展，扩展格子cost的值为父格子与子格子的长度差加上角度差，角度差值越大，cost权重越大；

步骤524：拓展所有从起点至终点cost+heuristic值最小的格子，连接起来生成hybrida*路径。

步骤53：将步骤52所求得一次规划路径作为初值，代入数值优化求解器，求出二次规划路径。

如图3所示，初始规划出的轨迹(虚线)并不符合需求，该轨迹曲率变化率大，轨迹平滑度不够，需要进行再次规划，通过a*搜索与数值优化共同算出的泊车轨迹(实线)更平稳、安全、可靠。

步骤6：规划模块将成功规划出的路径传入hmi显示模块，驾驶员根据已规划出的路径进行决策：“是否采用该规划路径”。若是，进入步骤7，若否，返回步骤2。

步骤7：控制模块根据规划模块求得的轨迹与车速信息控制车辆入库，该过程激光雷达开启，实时检测车辆轨迹周围是否存在障碍物。若是，进入步骤8；若否，进入步骤9。

步骤8：车辆hmi显示模块提示已检测到障碍物，驾驶员决定是否等待障碍物离开。若是，延时10秒，返回步骤7，若否，车辆沿走过的规划路径原路返回，且返回步骤2。

步骤9：车辆到达泊车终点位置，控制模块跟踪完成所规划出的路径，退出自主泊车模式，泊车结束。

本发明还提供一种自动泊车辅助系统，该系统基于数值优化和搜索规划方法，用于实现上述方法，如图5所示，该系统包括感知模块、hmi显示模块、路径规划模块、跟踪控制模块。

所述感知模块包含环视摄像头与激光雷达。环视摄像头用于识别库位，并采用双目摄像头匹配的方法推算库位大小，并投影于离散地图上；激光雷达用于检测环境障碍物，并将障碍物点云信息投影于离散地图上，并用于控制模块接管驾驶过程中实时碰撞检测。

所述hmi显示模块为驾驶员与当前自动泊车辅助系统交互模块，hmi屏幕显示所检测到库位信息，显示离散地图，显示所规划路径，显示控制模块跟踪情况；驾驶员通过hmi模块确认库位，确认规划路径，遇到障碍物是否等待。

所述路径规划模块包含基于数值优化的规划方法与基于a*搜索的规划方法，输入信息为车辆起始点以及目标点的坐标值与航向角，输出信息为泊车路径，并且包含车辆在每个离散控制点的车辆坐标、航向角、速度、加速度、车辆转向角。

所述车辆控制跟踪模块包含控制器与ecu，接受规划模块输出的离散轨迹点信息以及参考控制量，通过控制器控制车辆的油门踏板与方向盘转角，控制车辆行驶。

本发明系统通过布置在车辆四周的环视摄像头识别车辆附近的泊车位，以及车顶激光雷达获取周围环境障碍物信息，推算泊车位大小、类型以及库位内是否存在障碍物。当库位尺寸符合且库位内部不存在障碍物时，进行基于数值优化的初始规划。当初始优化不满足泊车需求时，如安全性不足，平滑度不够等，需根据当前自车位姿、库位信息与环境障碍物信息，进行一次a*搜索规划和二次数值优化规划。规划成功后，将包含速度、加速度信息的轨迹控制点发送车载控制器，车载控制器控制车辆方向盘、油门踏板和制动踏板，将车辆泊入目标库位。泊车过程中激光雷达实时进行碰撞检测，若车辆行进轨迹上检测出障碍物，车辆停止行驶，等待障碍物离开或者原路返回至泊车起始点。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。