CN109606354B - 一种基于分层规划的自动泊车方法及辅助*** - Google Patents
一种基于分层规划的自动泊车方法及辅助*** Download PDFInfo
- Publication number
- CN109606354B CN109606354B CN201811217362.0A CN201811217362A CN109606354B CN 109606354 B CN109606354 B CN 109606354B CN 201811217362 A CN201811217362 A CN 201811217362A CN 109606354 B CN109606354 B CN 109606354B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- parking
- path
- planning
- module
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 238000005457 optimization Methods 0.000 claims abstract description 38
- 230000004888 barrier function Effects 0.000 claims abstract description 16
- 230000007613 environmental effect Effects 0.000 claims abstract description 9
- 230000008569 process Effects 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 14
- 230000009977 dual effect Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 5
- 244000273256 Phragmites communis Species 0.000 claims description 4
- 235000014676 Phragmites communis Nutrition 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 239000013643 reference control Substances 0.000 claims description 3
- 230000003111 delayed effect Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000003915 air pollution Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
- B60W30/06—Automatic manoeuvring for parking
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W50/08—Interaction between the driver and the control system
Landscapes
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Human Computer Interaction (AREA)
- Traffic Control Systems (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明涉及一种基于分层规划的自动泊车方法,通过设于车辆四周的感知模块获取周围环境障碍物信息,推算泊车位大小、类型以及库位内是否存在障碍物,当库位尺寸符合且库位内部不存在障碍物时,进行基于数值优化的初始规划,当初始规划不满足泊车需求时,根据当前自车位姿、库位信息与环境障碍物信息,进行一次A*搜索规划和二次数值优化规划,规划成功后,将轨迹控制点发送车载控制器,车载控制器控制车辆方向盘、油门踏板和制动踏板,将车辆泊入目标库位;本发明还涉及一种自动泊车辅助***,包括感知模块、HMI显示模块、路径规划模块、车辆路径跟踪模块。与现有技术相比,本发明具有较强的环境适应性,且轨迹计算更精确。
Description
技术领域
本发明涉及一种智能汽车自动泊车辅助***,尤其是涉及一种基于分层规划的自动泊车方法及辅助***。
背景技术
泊车对于驾驶员来说从来都不是一件容易的事。由于驾驶员在驾驶舱中视角受限,对后方和侧方的车身周围情况无法直观把控,泊车过程需要经常采用后退、拐弯等难度较高的操作,稍有不慎便会有磕碰产生,造成财产损失,甚至安全事故。而随着城市地价的日益攀升,城市停车位也日益狭窄,对驾驶员来说,手动停车比以前更为困难。如果停车不够好,可能会扰乱公共停车资源的正常使用,甚至可能造成交通堵塞。此外,停车经验不足的司机可能不愿意使用狭窄的泊车位,从而不得不绕远路寻找泊车位,造成额外的能源损失、空气污染与交通拥堵。
为了减轻手动停车的负担,汽车制造商开发了自动停车辅助***。自从自动停车辅助***商用以来,众多汽车生产企业也纷纷将自动停车***投放市场。尽管自动泊车技术蓬勃发展,该技术目前仍不成熟。
传统的自动泊车***中的轨迹规划方法通常采用几何法,它通过获取车辆、周围障碍物以及目标库位的几何关系,求取车辆在当前环境中的可行路线。该方法对环境的要求很高,具体表现为车辆初始位置、初始航向角、障碍物等等。通常每一种几何运算方式只适合某一种或一类环境,适应性较差。基于搜索的A*算法非常适用于非结构化道路的无人车路径规划,在泊车工况下具有其独特的算法优势,然而由于泊车环境较狭窄,路径精度要求高,基于搜索的传统无人车路径规划方法搜索失败可能性较大,且需要对周围环境地图进行高精度离散,极大影响算法实时性,在泊车环境并不适用。
因此,如何解决传统泊车策略所带来的问题,有效利用搜索算法的优势,是本领域技术人员急需解决的问题。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于分层规划的自动泊车方法及辅助***。
本发明的目的可以通过以下技术方案来实现:
一种基于分层规划的自动泊车方法,包括以下步骤:
S1:启动车辆自动泊车模式,开启自动泊车辅助HMI,通过环视摄像头获取周围环境信息并投放至车辆HMI屏幕上,同时以车辆为原点离散周围环境地图,通过激光雷达扫描障碍物,并将障碍物点云信息投影到离散地图上。
S2:驾驶员驾驶车辆缓慢行驶于泊车区域,搜寻目标库位,判断库位类型、库位大小以及库位内是否有障碍物。
驾驶员驾驶车辆缓慢搜寻目标库位过程中,地图随车辆移动,坐标系始终取车辆后轴中心为原点,建立分别以车辆行驶方向、车辆右方为正方向的二维平面栅格坐标系;库位检测同时检测库位尺寸、库位内部是否存在障碍物及库位类型,其中库位尺寸根据环视摄像头检测库位角点判断,库位内部障碍物通过激光雷达判断,库位类型根据车辆当前航向角与库位角点投影到离散地图上的几何关系进行判断,库位类型包含垂直库位、平行库位与斜库位。
S3:若库位尺寸满足要求,且库位内无障碍物,发送指令至车辆HMI,请求驾驶员判断是否以该库位泊车,若是,则进入步骤S4,否则返回步骤S2。
S4:根据选定泊车位的类型,判断泊车终点位置,获取终点车辆航向角。
S5:依据车辆当前位姿、终点位姿以及周围障碍物信息,在离散环境地图上规划起始点到目标点的路径;具体内容包括:
51)利用数值优化方法进行当前位姿到目标库位的初始规划,随后进行如下判断:
判断1:判断能否成功规划出路径,若能规划出初始路径,则进入判断2,否则HMI提示无法成功规划路径的信息后,返回步骤S2;
判断2:判断规划出的路径是否满足泊车需求,若能满足需求,则进入步骤S6,否则进入步骤52);
52)根据当前信息,在离散地图里利用Hybrid A*方法获取一次规划路径。
满足需求指满足安全需求与轨迹可行性需求。安全需求指规划出的轨迹与障碍物距离需大于一定阈值。可行性需求指规划出的轨迹应平滑、曲率连续、转向角不宜过大。
优选地,步骤51)中,数值优化规划方法具体包括以下步骤:
式中,x、y为车辆后轴中心坐标;Et、Om均为定义在R2上的多边形;A、b为车辆位置矩阵,A∈Rl·n,b∈Rl;Cm、dm为障碍物位置矩阵,n为空间维度;l、k为组成凸集的超平面个数,λ、μ均为对偶优化问题拉格朗日变量。
512)根据障碍物角点坐标建立超平面方程组,求取Cm、dm,根据车辆当前位姿求取A、b:
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)中,利用Hybrid A*方法进行规划具体包括下列内容:
521)根据离散地图上障碍物与目标点的位置,获取离散地图里所有格子在考虑障碍物的情况下的A*heuristic1值;
522)基于离散地图上障碍物与目标点的位置,根据Reeds Shepp曲线原理求取格子所有的点到目标点的Reeds Shepp线的最短长度,其长度值作为格子的heuristic2值,栅格地图中每一个格子的heuristic值为heuristic1值与heuristic2值之和。
523)从起点开始向四周格子进行扩展,扩展格子cost的值为父格子与子格子的长度差加上角度差;
524)拓展所有从起点至终点cost+heuristic值最小的格子并将其连接起来,生成Hybrid A*路径。
优选地,Hybrid A*一次规划的结果包含路径点的控制点,即车辆后轴中心坐标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。
其中,数值优化规划方法具体步骤包括:
式中,Et,Om均为定义在R2上的多边形,A、b为描述车辆位置的矩阵,Cm、dm为描述障碍物位置的矩阵;A∈Rl·n,b∈Rl,n为空间维度;l、k为组成凸集的超平面个数,λ、μ均为对偶优化问题拉格朗日变量。A、b需要通过约束计算。
步骤512:根据障碍物角点坐标建立超平面方程组求取Cm、dm,根据车辆当前位姿求取A、b:
步骤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:根据当前信息,在离散地图里利用Hybrid A*方法求出一次规划路径。
具体内容为:
步骤521:根据离散地图上障碍物与目标点的位置,求取离散地图里所有格子在考虑障碍物的情况下的A*heuristic1值;
步骤522:根据离散地图上障碍物与目标点的位置,根据Reeds Shepp曲线原理求取格子所有的点到目标点的Reeds Shepp线的最短长度,其长度值作为格子的heuristic2值。栅格地图中每一个格子的heuristic值为heuristic1值与heuristic2值之和;
步骤523:从起点开始向四周格子进行扩展,扩展格子cost的值为父格子与子格子的长度差加上角度差,角度差值越大,cost权重越大;
步骤524:拓展所有从起点至终点cost+heuristic值最小的格子,连接起来生成Hybrid A*路径。
步骤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*搜索规划和二次数值优化规划。规划成功后,将包含速度、加速度信息的轨迹控制点发送车载控制器,车载控制器控制车辆方向盘、油门踏板和制动踏板,将车辆泊入目标库位。泊车过程中激光雷达实时进行碰撞检测,若车辆行进轨迹上检测出障碍物,车辆停止行驶,等待障碍物离开或者原路返回至泊车起始点。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的工作人员在本发明揭露的技术范围内,可轻易想到各种等效的修改或替换,这些修改或替换都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应以权利要求的保护范围为准。
Claims (5)
1.一种基于分层规划的自动泊车方法,其特征在于,该方法包括以下步骤:
1)启动车辆自动泊车模式,开启自动泊车辅助HMI,通过环视摄像头获取周围环境信息并投放至车辆HMI屏幕上,同时以车辆为原点离散周围环境地图,通过激光雷达扫描障碍物,并将障碍物点云信息投影到离散地图上;
2)驾驶员驾驶车辆缓慢行驶于泊车区域,搜寻目标库位,判断库位类型、库位大小以及库位内是否有障碍物,驾驶员驾驶车辆缓慢搜寻目标库位过程中,地图随车辆移动,坐标系始终取车辆后轴中心为原点,建立分别以车辆行驶方向、车辆右方为正方向的二维平面栅格坐标系;库位检测同时检测库位尺寸、库位内部是否存在障碍物及库位类型,其中库位尺寸根据环视摄像头检测库位角点判断,库位内部障碍物通过激光雷达判断,库位类型根据车辆当前航向角与库位角点投影到离散地图上的几何关系进行判断,库位类型包含垂直库位、平行库位与斜库位;
3)若库位尺寸满足要求,且库位内无障碍物,发送指令至车辆HMI,请求驾驶员判断是否以该库位泊车,若是,则进入步骤4),否则返回步骤2);
4)根据选定泊车位的类型,判断泊车终点位置,获取终点车辆航向角;
5)依据车辆当前位姿、终点位姿以及周围障碍物信息,在离散环境地图上规划起始点到目标点的路径;具体包括以下步骤:
51)利用数值优化方法进行当前位姿到目标库位的初始规划,随后进行如下判断:
判断1:判断能否成功规划出路径,若能规划出初始路径,则进入判断2,否则HMI提示无法成功规划路径的信息后,返回步骤2;
判断2:判断规划出的路径是否满足泊车需求,若能满足需求,则进入步骤6,否则进入步骤52);
52)根据当前信息,在离散地图里利用Hybrid A*方法获取一次规划路径;
6)规划模块将规划路径传入HMI显示模块,驾驶员根据已规划出的路径对是否采用该规划路径进行决策,若是,则进入步骤7),否则,返回步骤2);
7)控制模块根据规划模块获取的轨迹与车速信息控制车辆入库,开启激光雷达,实时检测车辆轨迹周围是否存在障碍物,若是,进入步骤8),否则,进入步骤9);
8)车辆HMI显示模块提示已检测到障碍物,驾驶员决定是否等待障碍物离开,若是,延时若干时间,返回步骤7),若否,车辆沿走过的规划路径原路返回,且返回步骤2);
9)车辆到达泊车终点位置,控制模块跟踪完成所规划出的路径,退出自主泊车模式,泊车结束;
当前车辆是否能顺利泊车由基于数值优化的初始规划决定,若初始规划失败,放弃当前车位,搜寻下一个可行泊车位;
步骤51)中,数值优化规划方法具体包括以下步骤:
511)根据离散地图上车辆当前位置与障碍物位置,构建环境地图上车辆方程Et={x|Ax≤b}、障碍物方程Om={y|Cmy≤dm}、无碰撞方程minx,y||x-y||2>dmin,并依据优化理论建立对偶问题方程组
式中,x、y为车辆后轴中心坐标;Et、Om均为定义在R2上的多边形;A、b为车辆位置矩阵,A∈Rl·n,b∈R1;Cm、dm为障碍物位置矩阵,n为空间维度;l、k为组成凸集的超平面个数,λ、μ均为对偶优化问题拉格朗日变量;
512)根据障碍物角点坐标建立超平面方程组,求取Cm、dm,根据车辆当前位姿求取A、b:
513)建立状态迭代方程式:
其中,为***在t(t∈{t0,t1,...,tN})时刻的所有状态变量,ut为***在t(t∈{t0,t1,...,tN})时刻的所有输入变量,包括加速度a,方向盘转角δ,zs表示***初始状态,zf表示***目标状态,TF为整个过程所需时间,N为离散状态个数,top为相邻两个状态间的时间差,p、q分别为时间与状态输入量的优化目标权重;
514)将上述方程组代入开源数值优化求解器中,求解泊车路径及其控制点参数,包括控制点x、y、速度v、加速度a、转向角δ及车辆航向角θ;
步骤52)中,利用Hybrid A*方法进行规划具体步骤如下:
521)根据离散地图上障碍物与目标点的位置,获取离散地图里所有格子在考虑障碍物的情况下的A*heuristic1值;
522)基于离散地图上障碍物与目标点的位置,根据Reeds Shepp曲线原理求取格子所有的点到目标点的Reeds Shepp线的最短长度,其长度值作为格子的heuristic2值,栅格地图中每一个格子的heuristic值为heuristic1值与heuristic2值之和;
523)从起点开始向四周格子进行扩展,扩展格子cost的值为父格子与子格子的长度差加上角度差;
524)拓展所有从起点至终点cost+heuristic值最小的格子并将其连接起来,生成Hybrid A*路径。
2.根据权利要求1所述的一种基于分层规划的自动泊车方法,其特征在于,Hybrid A*一次规划的结果包含路径点的控制点,即车辆后轴中心坐标x,y,将此x,y值作为优化初值,代入数值优化二次规划,输出最终包含速度v、加速度a、转向角信息δ的控制点x,y;成功规划出路径后,若驾驶员采用,则自动泊车控制模块接收规划模块发出的路径点信息,接管车辆行驶。
3.根据权利要求2所述的一种基于分层规划的自动泊车方法,其特征在于,控制模块接管车辆驾驶过程中,环境地图不再随车辆移动,原点为车辆泊车起点,坐标轴正方向为泊车起点航向,即泊车起点车辆航向角θ设为0。
4.一种实现权利要求1-3任一项所述的基于分层规划的自动泊车方法的自动泊车辅助***,其特征在于,该***包括:
感知模块,包括环视摄像头和激光雷达,所述的环视摄像头用于识别库位,并投影于离散地图上;所述的激光雷达用于在泊车过程中检测环境障碍物,并将障碍物点云信息投影于离散地图上,同时用于驾驶过程中的实时碰撞检测;
HMI显示模块,用于显示所检测到库位信息、离散地图、所规划路径及控制模块跟踪情况;驾驶员通过HMI模块确认库位,确认规划路径,遇到障碍物是否等待;
路径规划模块,该模块设有基于数值优化的规划方法单元与基于A*搜索的规划方法的处理单元,路径规划模块的输入信息为车辆起始点以及目标点的坐标值与车辆航向角,输出信息为泊车路径,且包含车辆在每个离散控制点的车辆坐标、车辆航向角、速度、加速度、车辆转向角;
车辆控制跟踪模块,包括控制器与ECU,用于接受规划模块输出的离散轨迹点信息以及参考控制量,通过控制器控制车辆的油门踏板与方向盘转角,控制车辆行驶。
5.根据权利要求4所述的自动泊车辅助***,其特征在于,所述的感知模块包括四个环视摄像头和两个激光雷达,四个环视摄像头分别装配于车辆右后视镜下方、右尾灯处、左后视镜下方及左尾灯处,两个激光雷达分别装配于车辆顶部右端与车辆顶部左端。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811217362.0A CN109606354B (zh) | 2018-10-18 | 2018-10-18 | 一种基于分层规划的自动泊车方法及辅助*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811217362.0A CN109606354B (zh) | 2018-10-18 | 2018-10-18 | 一种基于分层规划的自动泊车方法及辅助*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109606354A CN109606354A (zh) | 2019-04-12 |
CN109606354B true CN109606354B (zh) | 2020-07-28 |
Family
ID=66002025
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811217362.0A Active CN109606354B (zh) | 2018-10-18 | 2018-10-18 | 一种基于分层规划的自动泊车方法及辅助*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109606354B (zh) |
Families Citing this family (32)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110136254B (zh) * | 2019-06-13 | 2019-12-13 | 吉林大学 | 基于动态概率驾驶地图的驾驶辅助信息显示方法 |
CN112506176B (zh) * | 2019-08-26 | 2024-05-28 | 上海汽车集团股份有限公司 | 路径规划方法及装置 |
CN110444044B (zh) * | 2019-08-27 | 2022-07-12 | 纵目科技(上海)股份有限公司 | 基于超声波传感器的车辆位姿检测***、终端和存储介质 |
CN110861639B (zh) * | 2019-11-28 | 2021-06-15 | 安徽江淮汽车集团股份有限公司 | 泊车信息融合方法、装置、电子设备及存储介质 |
EP4067821A4 (en) * | 2019-11-29 | 2023-04-05 | Huawei Technologies Co., Ltd. | PATH-PLANNING METHOD FOR A VEHICLE AND PATH-PLANNING DEVICE FOR A VEHICLE |
CN110979313B (zh) * | 2019-12-13 | 2021-02-09 | 武汉乐庭软件技术有限公司 | 一种基于空间地图的自动泊车定位方法及*** |
CN111169470B (zh) * | 2020-01-06 | 2021-07-27 | 上海汽车集团股份有限公司 | 一种自动泊车方法、装置及电子设备 |
CN111197987A (zh) | 2020-02-26 | 2020-05-26 | 深圳市银星智能科技股份有限公司 | 一种困境识别方法、装置以及计算机存储介质 |
CN111301409A (zh) * | 2020-03-11 | 2020-06-19 | 中国第一汽车股份有限公司 | 一种泊车路径规划方法、装置、车辆和存储介质 |
CN111319615B (zh) * | 2020-03-16 | 2021-02-26 | 湖北亿咖通科技有限公司 | 智能代客泊车方法、计算机可读存储介质及电子设备 |
CN111459160B (zh) * | 2020-04-02 | 2021-10-15 | 中国矿业大学 | 一种开放道路无人洗扫车大规模轨迹平滑方法 |
CN111427366B (zh) * | 2020-05-13 | 2023-08-29 | 合肥卓科智能技术有限公司 | 一种用于物流agv小车的控制*** |
CN111552764B (zh) * | 2020-05-15 | 2023-07-25 | 汇智机器人科技(深圳)有限公司 | 一种车位检测方法、装置、***及机器人和存储介质 |
CN111619552A (zh) * | 2020-05-26 | 2020-09-04 | 三一专用汽车有限责任公司 | 适于施工区域的工程车辆自主泊车***和自主泊车方法 |
CN111578894B (zh) * | 2020-06-02 | 2021-10-15 | 北京经纬恒润科技股份有限公司 | 一种确定障碍物航向角的方法及装置 |
CN111891119A (zh) * | 2020-06-28 | 2020-11-06 | 东风汽车集团有限公司 | 一种自动泊车控制方法及*** |
CN111845719B (zh) * | 2020-06-30 | 2021-10-12 | 江苏大学 | 自动泊车过程中的自动退回重选*** |
CN112180373B (zh) * | 2020-09-18 | 2024-04-19 | 纵目科技(上海)股份有限公司 | 一种多传感器融合的智能泊车***和方法 |
CN112092810B (zh) * | 2020-09-24 | 2022-06-14 | 上海汽车集团股份有限公司 | 一种车辆泊出方法、装置及电子设备 |
CN112092802A (zh) * | 2020-09-25 | 2020-12-18 | 北京罗克维尔斯科技有限公司 | 车辆控制方法、装置及车辆 |
CN112389465B (zh) * | 2020-11-17 | 2022-03-18 | 湖南三一智能控制设备有限公司 | 工程车辆的控制方法、控制***和工程车辆 |
CN112562411B (zh) * | 2020-12-04 | 2022-04-22 | 中国联合网络通信集团有限公司 | 获取自主代客泊车视频的方法和*** |
CN112721704B (zh) * | 2021-01-20 | 2022-08-19 | 安洁无线科技(苏州)有限公司 | 基于无线充电对齐技术的电动汽车自动泊车方法及*** |
CN113044029B (zh) * | 2021-03-19 | 2022-03-15 | 北京理工大学 | 一种保证无人车在三维地形上安全行驶的运动规划方法 |
CN113085838A (zh) * | 2021-04-02 | 2021-07-09 | 的卢技术有限公司 | 一种基于多传感器融合的车位检测方法和*** |
CN113353064B (zh) * | 2021-04-30 | 2024-03-29 | 惠州华阳通用电子有限公司 | 一种自动泊车行车控制方法 |
CN113830079B (zh) * | 2021-10-19 | 2023-09-01 | 同济大学 | 任意起始位姿的连续曲率泊车路径在线规划方法及*** |
CN114030463B (zh) * | 2021-11-23 | 2024-05-14 | 上海汽车集团股份有限公司 | 一种自动泊车***的路径规划方法及装置 |
CN114296452B (zh) * | 2021-12-16 | 2024-05-28 | 华人运通(上海)自动驾驶科技有限公司 | 自动泊车路径规划方法、设备及计算机可读存储介质 |
CN114940164B (zh) * | 2022-05-20 | 2023-05-23 | 重庆邮电大学 | 一种面向泊车场景的无人驾驶车辆行驶轨迹优化方法及*** |
CN115407355B (zh) * | 2022-11-01 | 2023-01-10 | 小米汽车科技有限公司 | 库位地图的验证方法、装置及终端设备 |
CN116796210B (zh) * | 2023-08-25 | 2023-11-28 | 山东莱恩光电科技股份有限公司 | 基于激光雷达的障碍物检测方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101327736B1 (ko) * | 2011-12-23 | 2013-11-11 | 현대자동차주식회사 | Avm탑뷰 기반 주차지원 시스템 |
CN105197010B (zh) * | 2014-06-04 | 2018-03-27 | 长春孔辉汽车科技股份有限公司 | 辅助泊车***以及辅助泊车控制方法 |
CN104843066B (zh) * | 2015-05-12 | 2018-02-06 | 上海寅喆计算机科技有限公司 | 一种自动泊车方法及*** |
CN107563405A (zh) * | 2017-07-19 | 2018-01-09 | 同济大学 | 基于多分辨率神经网络的车库自动驾驶语义目标识别方法 |
CN108121205B (zh) * | 2017-12-13 | 2021-02-26 | 深圳市航盛电子股份有限公司 | 一种用于多种泊车场景的路径规划方法、***及介质 |
-
2018
- 2018-10-18 CN CN201811217362.0A patent/CN109606354B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109606354A (zh) | 2019-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109606354B (zh) | 一种基于分层规划的自动泊车方法及辅助*** | |
US11932284B2 (en) | Trajectory setting device and trajectory setting method | |
JP6601696B2 (ja) | 予測装置、予測方法、およびプログラム | |
KR20210050925A (ko) | 차량 충돌 회피 장치 및 방법 | |
US9141109B1 (en) | Automated driving safety system | |
JP6710710B2 (ja) | 車両制御装置、車両制御方法、およびプログラム | |
JP6843819B2 (ja) | 交通案内物認識装置、交通案内物認識方法、およびプログラム | |
JP6641583B2 (ja) | 車両制御装置、車両制御方法、およびプログラム | |
CN113160547B (zh) | 一种自动驾驶方法及相关设备 | |
CN112068574A (zh) | 一种无人车在动态复杂环境中的控制方法及*** | |
US11535271B2 (en) | Methods and systems for monitoring vehicle motion with driver safety alerts | |
JP2019159611A (ja) | 車両制御装置、車両制御方法、およびプログラム | |
US20220366175A1 (en) | Long-range object detection, localization, tracking and classification for autonomous vehicles | |
JP7376682B2 (ja) | 視覚追跡および画像再投影による自律運転のための物***置特定 | |
US20240124030A1 (en) | Responding to emergency vehicles for autonomous vehicles | |
JP2020083106A (ja) | 車両制御装置、車両制御方法、およびプログラム | |
JP2019156266A (ja) | 車両制御装置、車両制御方法、及びプログラム | |
EP3679441B1 (en) | Mobile robot having collision avoidance system for crossing a road from a pedestrian pathway | |
WO2021005392A1 (ja) | 運転制御方法及び運転制御装置 | |
US20220176987A1 (en) | Trajectory limiting for autonomous vehicles | |
WO2023102445A1 (en) | Automatic bootstrap for autonomous vehicle localization | |
US20220219678A1 (en) | Parking Assist Method and Parking Assist Apparatus | |
US20240017738A1 (en) | Planning trajectories for controlling autonomous vehicles | |
WO2023067793A1 (ja) | 運転支援方法及び運転支援装置 | |
US12049236B2 (en) | Complementary control system detecting imminent collision of autonomous vehicle in fallback monitoring region |
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 |