CN115525047B - 一种具备多型避障方式的车辆局部轨迹规划方法及*** - Google Patents

一种具备多型避障方式的车辆局部轨迹规划方法及*** Download PDF

Info

Publication number
CN115525047B
CN115525047B CN202210328938.0A CN202210328938A CN115525047B CN 115525047 B CN115525047 B CN 115525047B CN 202210328938 A CN202210328938 A CN 202210328938A CN 115525047 B CN115525047 B CN 115525047B
Authority
CN
China
Prior art keywords
obstacle
height
vehicle
obs
local track
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
Application number
CN202210328938.0A
Other languages
English (en)
Other versions
CN115525047A (zh
Inventor
杨泽宇
王佳龙
徐彪
秦晓辉
王晓伟
秦洪懋
秦兆博
谢国涛
胡满江
边有钢
丁荣军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuxi Institute Of Intelligent Control Hunan University
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Original Assignee
Wuxi Institute Of Intelligent Control Hunan University
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuxi Institute Of Intelligent Control Hunan University, Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd filed Critical Wuxi Institute Of Intelligent Control Hunan University
Priority to PCT/CN2022/111967 priority Critical patent/WO2023178910A1/zh
Publication of CN115525047A publication Critical patent/CN115525047A/zh
Application granted granted Critical
Publication of CN115525047B publication Critical patent/CN115525047B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种具备多型避障方式的车辆局部轨迹规划方法及***，其包括：S1，获取当前自车位姿信息、障碍物信息、地图信息和参考路径；S2，从所有障碍物中选出当前车辆位置前方参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；S3，根据参考路径和当前自车位姿信息进行局部轨迹规划，生成一系列候选局部轨迹；S4，取出代价最低的候选局部轨迹,根据步骤S2生成的避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新执行步骤S4，直至获得代价最低的候选局部轨迹；S5，对步骤S4获得的无碰撞候选局部轨迹进行插值，得到局部轨迹。

Description

一种具备多型避障方式的车辆局部轨迹规划方法及***
技术领域
本发明涉及智能汽车轨迹规划技术领域，特别是关于一种具备多型避障方式的车辆局部轨迹规划方法及***。
背景技术
近几年来，由于城市化导致的交通拥堵、交通安全问题频发。而自动驾驶技术对于缓解道路拥堵、提高行车安全等方面有重大帮助。因此对车辆自动驾驶相关技术的研究成为各个领域专家学者关注的焦点。车辆自动驾驶***通常由五个模块组成，分别是环境感知模块、地图模块、定位模块、决策规划模块和跟踪控制模块。其中，决策规划模块又由全局规划、行为决策和局部规划三大功能组成。其中局部规划是自动驾驶避障功能实现的关键步骤，其根据全局路径、障碍物信息、车辆状态信息实时生成出不与障碍物碰撞的车辆安全行驶轨迹。目前常用的车辆局部规划算法包含动态窗口法(DWA，dynamic windowapproach)、时间弹性带法(TEB，Time Elastic Band)、人工势场法(APF，ArtificialPotential Field)、快速搜索随机树(RRT，Rapid-exploration Random Tree)和状态栅格(State-Lattice)等，它们能可靠地进行局部轨迹规划并避让车辆前方的障碍物。在车辆实际驾驶中会遇到路坑、碎石、它车、行人、路旁树枝等多种类型的障碍物。但现有局部轨迹规划算法在进行避障时，均采用绕行避让的方式，并未对不同类型的障碍物采用不同的避障方案。这并不符合人类的驾驶习惯，且会造成局部的曲率突变。同时，过多的、不必要的避障也会降低自动驾驶车辆的通行效率。
发明内容
本发明的目的在于提供一种具备多型避障方式的车辆局部轨迹规划方法，用于生成安全、平滑的局部轨迹。
为实现上述目的，本发明提供一种具备多型避障方式的车辆局部轨迹规划方法，其包括：
S1，获取当前自车位姿信息、当前车辆位姿下周边环境中的障碍物信息、地图信息和参考路径；
S2，从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；
S3，根据所述参考路径和当前自车位姿信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价；
S4，取出曲线坐标下代价最低的候选局部轨迹,将其转换至大地坐标下,根据所述步骤S2生成的所述避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新执行步骤S4，直至获得一条大地坐标下代价最低的无碰撞候选局部轨迹；
S5，对步骤S4获得的大地坐标系下代价最低的无碰撞候选局部轨迹进行插值，得到局部轨迹。
进一步地，所述步骤S2具体包括：
S21，依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物作为静态障碍物，速度vobs不为零的障碍物作为动态障碍物；
S22，为分类后的每一障碍物分配对应的避障决策，其具体包括：
对于所述步骤S21判定的对于动态障碍物，采用绕行避障决策。
进一步地，S22中，对于所述步骤S21判定的静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，在考虑安全裕量的情况下，具体包括：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b1；
b1，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c1；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策；
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策。
进一步地，S22中，对于所述步骤S21判定的静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，具体包括：
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2；
b2，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c2；若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策；
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第四预设值的大小关系，若wobs不小于第四预设值，或者，lobs不小于第四预设值，则对该静态障碍物采取绕行避障决策；若wobs小于第四预设值，并且，lobs小于第四预设值，则对该静态障碍物采取跨越避障决策，其中，第四预设值依据轮距Lb和车轮宽度d确定。
进一步地，所述步骤S3具体包括：
S31，将自车大地坐标、参考路径进行曲线坐标转换；
S32，采用多次多项式方法或多阶贝塞尔曲线方法生成横向候选局部轨迹；
S33，利用d(t)与s(t)为每一条横向候选局部轨迹与纵向候选局部轨迹分配相应的代价，d(t)、s(t)分别表示车辆在曲线坐标下横、纵向位移随时间的变化；
横向候选局部轨迹的代价计算公式表示为式(11)：
其中，kj、kt、kd、kΔd分别表示权重系数，T表示由采样生成的横向候选局部轨迹末端时刻，dp(T)表示上一个规划周期选择的最优局部轨迹对应的末端横向位移采样值，d(T)表示当前横向候选局部轨迹在末端时刻的横向位移，表示车辆横向加加速度；
纵向候选局部轨迹的代价计算公式表示为式(12)：
其中，kj、kt、分别表示权重系数，sd(T)表示在末端时刻期望达到的目标纵向位移，/>表示在末端时刻期望达到的目标纵向速度，s(T)表示当前纵向候选局部轨迹在末端时刻的纵向位移，/>表示当前纵向候选局部轨迹在末端时刻的纵向速度，/>表示车辆纵向加加速度；
S34，将采样时间相同的曲线坐标横、纵向候选局部轨迹合成为一条曲线坐标系下的候选局部轨迹，其代价函数表示为式(13)：
Ctotal＝klat·Cd+klon·Cs(13)
其中，klat、klon分别表示权重系数。
进一步地，所述步骤S4具体包括：
取出代价最低的候选局部轨迹，通过候选局部轨迹在曲线坐标系下各轨迹点(si，di)定位到其对应大地坐标下参考路径的路径点n＝)xn，yn，θn)，以点n作为曲线坐标系转换至大地坐标系的投影点，利用下式(14)计算该候选局部轨迹点在大地坐标系下的坐标(xi，yi)，再计算候选局部轨迹点角度θi以及曲率ki，如式(15)所示：
其中，(xi+1，yi+1，θi+1)、(xi-1，yi-1，θi-1)分别表示当前候选局部轨迹点的相邻前向、后向候选局部轨迹点，
并依据不同的避障决策，采用下面对应的碰撞检测方法进行障碍物与该候选局部轨迹的碰撞检测或不进行障碍物与该候选局部轨迹碰撞检测：
a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测，以判断车辆沿当前候选局部轨迹行驶时，车身与障碍物是否发生碰撞；
b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测，以判断车辆沿当前候选局部轨迹行驶时，车轮与障碍物是否发生碰撞；
c.对于采用穿越避障的障碍物，不进行碰撞检测；
进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条代价最低的候选局部轨迹重新执行以上操作，直至得到一条大地坐标系下最低代价的无碰撞候选局部轨迹。
进一步地，OBB碰撞检测具体包括：
去除障碍物三维边界框或三维包围盒的高度信息，得到障碍物二维矩形包络信息；
依据车身参数与定位模块获取的车辆位置信息与朝向信息，得到车辆二维矩形包络信息；
分别以障碍物二维矩形包络的中心点、车辆二维矩形包络的中心点和对应朝向角建立笛卡尔坐标系，得到4条投影轴：x(o)、y(o)、x(v)和y(v)；
将障碍物二维矩形包络半长Lo，障碍物二维矩形包络半宽Wo，车辆二维矩形包络半长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维矩形包络的中心点连线T向各投影轴分别做投影，如式(16)所示；
如果在4个投影轴中存在任一个投影都满足式(17)，则障碍物不与车身发生碰撞，否则发生碰撞：
|Proj(T)|＞|Proj(Obstacle)||Proj(Vehicle)| (17)
式中，Proj()为投影函数，|Proj()|为投影长度。
本发明还提供一种具备多型避障方式的车辆局部轨迹规划***，其包括：
全局规划功能模块，其用于根据获取当前自车位姿信息、当前车辆位姿下周边环境中的障碍物信息和地图信息，规划一条连接当前车辆位姿与目标终点的全局路径，基于全局路径构造一条参考路径；
行为决策功能模块，其用于从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；
局部规划功能模块，其用于根据所述参考路径和当前自车信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价,根据所述行为决策功能模块生成的所述避障决策选择碰撞检测方法，取出代价最低的候选局部轨迹进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新取出代价最低的候选局部轨迹，直至取出的代价最低的无碰撞候选局部轨迹，并进行插值，得到局部轨迹。
进一步地，所述行为决策功能模块具体包括：
筛选单元，其用于保留障碍物中当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，并剔除其他障碍物；
分类单元，其用于依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物作为静态障碍物，速度vobs不为零的障碍物作为动态障碍物；
决策分配单元，其用于为分类后的每一障碍物分配对应的避障决策，其具体包括：
第一决策子单元，其用于对于动态障碍物，采用绕行避障决策；
第二决策子单元，其用于对于静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，具体包括a1至c1、或者，a2至c2：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b1；
b1，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c1；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策；
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策；
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2；
b2，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c2；若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策；
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第四预设值的大小关系，若wobs不小于第四预设值，或者，lobs不小于第四预设值，则对该静态障碍物采取绕行避障决策；若wobs小于第四预设值，并且，lobs小于第四预设值，则对该静态障碍物采取跨越避障决策，其中，第四预设值依据轮距Lb和车轮宽度d确定。
进一步地，所述局部规划功能模块具体包括：
轨迹生成单元，其基于参考路径构建曲线坐标，并使用多次多项式方法或多阶贝塞尔曲线方法生成横、纵向候选局部轨迹，再为横、纵向候选局部轨迹分配代价，并将曲线坐标下的横、纵向候选局部轨迹合成为曲线坐标下的候选局部轨迹；
碰撞检测单元，其用于取出曲线坐标下代价最低的候选局部轨迹，将其转换至大地坐标系下，并依据不同的避障决策，采用下面不同的碰撞检测方法或不进行障碍物与该候选局部轨迹碰撞检测：
a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
c.对于采用穿越避障的障碍物，不进行碰撞检测；
进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条代价最低的候选局部轨迹重新执行以上操作，直至得到一条大地坐标系下最低代价的无碰撞候选局部轨迹；
轨迹插值单元，其用于对大地坐标系下最低代价的无碰撞候选局部轨迹进行插值，生成局部轨迹。
本发明由于采取以上技术方案，其具有以下优点：针对不同类型的障碍物，设计绕行、跨越、穿越多种类型的避障决策，提出了一种基于采样的车辆局部轨迹规划算法，以保证局部轨迹的平滑性、可靠性、高效性；依据不同避障决策使用不同的碰撞检测方法或不进行障碍物与候选局部轨迹碰撞检测，则可在一次局部规划中同时实现对不同类型障碍物的合理避障。例如对于道路中车辆、行人等需要避开的障碍物，可采用绕行的避障方式；对于小碎石、路坑等车辆可跨越的障碍物，可采用跨越障碍物的避障方式；针对路旁树枝等底部有可通行空间的障碍物，可采用从底部穿越行驶的避障方式。
附图说明
图1是本发明实施例提供的自动驾驶***流程图。
图2是本发明实施例提供的具备多型避障方式的车辆局部轨迹规划方法流程图。
图3是本发明实施例提供的车辆外形图。
图4是本发明实施例提供的局部轨迹规划算法流程图。
图5是本发明实施例采用五次多项式方法生成的曲线坐标横向候选局部轨迹示意图。
图6是本发明实施例采用五阶贝塞尔曲线方法生成的曲线坐标横向候选局部轨迹示意图。
图7是本发明实施例采用四次多项式方法生成的曲线坐标纵向候选局部轨迹示意图。
图8是本发明实施例采用五阶贝塞尔曲线方法生成的曲线坐标纵向候选局部轨迹示意图。
图9是本发明实施例采用的车辆包络碰撞检测示意图。
图10是本发明实施例采用的车轮包络碰撞检测示意图。
具体实施方式
在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。
在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。
本发明实施例提供的具备多型避障方式的车辆局部轨迹规划方法主要包括图1中的行为决策功能模块与局部规划功能模块，其流程图如图2所示，具体包括：
S1，获取当前自车位姿信息、当前车辆位姿下周边环境中的障碍物信息、地图信息和参考路径。其中：由地图模块获取地图信息。
由定位模块得到当前状态下自车位姿信息其中，(xo，yo)表示车辆大地坐标，vo表示车辆速度，ao表示车辆加速度，/>表示车辆车身的朝向角。
由环境感知模块收集环境中与障碍物相关的数据，得到每个障碍物信息其中：(xobs，yobs，zobs)表示障碍物三维边界框或三维包围盒的几何中心点大地坐标，lobs，wobs，hobs)分别表示障碍物三维边界框或三维包围盒的长度、宽度、高度，vobs表示障碍物速度，/>表示障碍物速度方向。
由HMI人机接口得到规划的目标终点其中(xf，yf)为终点的大地坐标，/>为车辆到达终点处时应保持的车辆车身朝向角。目标终点信息输入决策规划模块。决策规划模块中，全局规划功能模块依据上述信息进行全局路径规划，生成一条连接当前自车位姿与目标终点的全局路径，并基于此全局路径构建参考路径。参考路径点可以被描述为/>(x，y为参考路径点大地坐标，(lleft，lright)分别为参考路径左右两侧的道路宽度，/>为参考路径点朝向。
S2，从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策。
在一个实施例中，分类的主要依据是障碍物三维边界框或三维包围盒的中心点大地坐标、大小参数及速度。避障决策包括但不限于绕行、跨越和穿越。步骤S2具体包括：
S21，依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物为静态障碍物，速度vobs不为零的障碍物为动态障碍物。
S22，为分类后的每一障碍物分配对应的避障决策，其具体包括：
对于S21判定的动态障碍物，均采用绕行避障决策。
对于S21判定的静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策。本实施例采用的车辆参数通过结合图3进行说明：如图3示出的车辆外形，图中，1为整车高度Hh，2为底盘高度Hl，3为轮距Lb，4为车轮宽度d。
在一个实施例中，在进行避障决策时，还需考虑安全裕量σ。其中，安全裕量σ的具体数值需要依据轮距Lb、宽度d及应用需求进行设置，例如在需要考虑较大安全裕量时，可以σ＝0.1×(Lb-d)。
作为依据对比结果获取避障决策的方法的一种实现方式，在考虑安全裕量的情况下，其具体包括：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物三维边界框或三维包围盒的最高点与最低点，并进入步骤b1；
其中，最高点的高度可以但不限于设置为zobs+0.5hobs，最低点的高度可以但不限于设置为zobs-0.5hobs。
b1，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c1；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策。
其中，第一预设值和第二预设值依据车辆底盘高度Hl和安全裕量σ确定，例如：第一预设值可以但不限于设置为Hl-σ，第二预设值可以但不限于设置为Hh+σ。若zobs+0.5hobs＜Hl-σ，则进入c1；若zobs-0.5hobs＞Hh+σ，则对该静态障碍物采取穿越避障决策；若Hh+σ＞zobs+0.5hobs＞Hl-σ或Hh+σ＞zobs-0.5hobs＞Hl-σ，或者，zobs+0.5hobs＞Hh+σ且zobs-0.5hobs＜Hl-σ则对该静态障碍物采取绕行避障决策。
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策。
其中，第三预设值依据轮距Lb、和安全裕量σ确定，例如：第三预设值可以但不限于设置为Lb-2σ-d，那么，若wobs≥Lb-2σ-d或lobs≥Lb-2σ-d，则对该静态障碍物采取绕行避障决策；若wobs＜Lb-2σ-d且lobs＜Lb-2σ-d，则对该静态障碍物采取跨越避障决策。
在另一个实施例中，作为依据对比结果获取避障决策的方法的另一种实现方式，其具体包括：
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2。
其中，最高点的高度可以但不限于设置为zobs+0.5hobs，最低点的高度可以但不限于设置为zobs-0.5hobs。
b2，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c2；若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策。
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第四预设值的大小关系，若wobs不小于第四预设值，或者，lobs不小于第四预设值，则对该静态障碍物采取绕行避障决策；若wobs小于第四预设值，并且，lobs小于第四预设值，则对该静态障碍物采取跨越避障决策。
其中，第四预设值依据轮距Lb和车轮宽度d确定，例如但不限于选用Lb-2d那么，步骤c1为：判断该静态障碍物三维边界框或三维包围盒宽度wobs或长度lobs与Lb-d的大小关系，若wobs≥Lb-d或lobs≥Lb-d，则对该静态障碍物采取绕行避障决策；若wobs＜Lb-d且lobs＜Lb-d，则对该静态障碍物采取跨越避障决策。
当然，也可以采用现有技术中其他来获取避障决策的方法，在此不再一一列举。
S3，根据所述参考路径和当前自车位姿信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价。
在一个实施例中，行为决策功能模块生成对于各种类型障碍物的避障决策后，局部规划功能模块可以但不限于使用基于采样的轨迹规划算法进行局部轨迹规划。该局部规划方法首先基于参考路径构建曲线坐标系，在曲线坐标系下以多项式或贝塞尔曲线的形式完成横向候选局部轨迹与纵向候选局部轨迹规划并进行曲线合成、代价分配后，得到一组曲线坐标下的候选局部轨迹，再将曲线坐标系下的候选局部轨迹转换至大地坐标系下。最终，得到一组大地坐标系下的候选局部轨迹。如图4所示，包括：
S31，将自车大地坐标、参考路径进行曲线坐标转换，将大地坐标(x，y)转换为曲线坐标(d，s，其中，s代表沿着参考路径方向的纵向位移；d代表与s轴横向偏离的位移。
S32，在曲线坐标下可以采用多次多项式方法或多阶贝塞尔曲线方法生成横向候选局部轨迹。
S32a，在采用五次多项式方法生成曲线坐标系下的横向候选局部轨迹时，可得d关于时间t的横向候选局部轨迹五次多项式函数d(t)，如式(1)所示：
d(t)＝ad·t5+bd·t4+cd·t3+dd·t2+ed·t+fd(1)
车辆横向运动的初始状态包括曲线坐标下初始横向位移d(0)、初始横向速度和初始横向加速度/>这些量均可以直接从当前自车的车辆状态得到。
对末端时刻T与末端横向位移d(T)进行采样，令末端横向速度和横向加速度为0。其中，末端横向位移d(T)的采样范围不可超过(-lright，lleft)。
由此，可得到6个已知条件，并以此确定五次多项式的六个参数，即可确定曲线坐标系下横向候选局部轨迹表达式d(t)：
其中：
例如：如图5所示，为根据采样生成的五次多项式形式的曲线坐标横向候选局部轨迹示意图。图中使用的样例d(0)＝0，采样末端时刻T∈[4，6，间隔为0.5s，采样末端横向位移d(T)∈[-4，4，间隔为1m。因此，末端时刻采样点个数为5，末端横向位移采样点个数为9，共生成45条横向候选局部轨迹。
S32b，在采用五阶贝塞尔曲线方法生成曲线坐标系下的横向候选局部轨迹时，可得到d关于时间t的横向候选局部轨迹五阶贝塞尔曲线函数d(t)，如式(4)所示：
其中，Pd0，Pd1，Pd2，Pd3，Pd4，Pd5分别为横向候选局部轨迹五阶贝塞尔曲线的控制点，每个点包含一个d值，Pd0为初始状态横向位移，Pd5为末状态横向位移。
与五次多项式方法生成横向候选局部轨迹相同地，可以从当前自车的车辆状态得到曲线坐标下的横向运动初始状态包括初始横向位移d(0)、初始横向速度和初始横向加速度/>再对末端时刻T与末端横向位移d(T)进行采样，令末端横向速度/>和末端横向加速度/>为0。由此可知，d(0)即为Pd0，d(T)即为Pd5。
依据上述信息，可确定Pd0，Pd1，Pd2，Pd3，Pd4，Pd5表示如下式(5)：
在一个实施例中，如图6所示，为根据采样生成的五阶贝塞尔曲线形式的曲线坐标横向候选局部轨迹示意图。图中使用的样例d(0)＝0， 采样末端时刻T∈[4，6，间隔为0.5s，采样末端横向位移d(T)∈[-4，4，间隔为1m。因此，末端时刻采样点个数为5，末端横向位移采样点个数为9，共生成45条横向候选局部轨迹。
S32c，在采用四次多项式方法生成曲线坐标系下的纵向候选局部轨迹时，可得到s关于t的纵向候选局部轨迹四次多项式函数s(t)，如式(6)所示：
s(t)＝as·t4+bs·t3+cs·t2+ds·t+es(6)
其纵向运动的初始状态包括初始纵向位移s(0)、初始纵向速度和初始纵向加速度/>这些量可以直接从当前自车的车辆状态得到。在与横向采样同样的末端时刻T下，对末端纵向速度/>进行采样，并设定末端纵向加速度/>为0。以上可得到5个已知条件，并以此确定四次多项式的五个参数，即可确定曲线坐标系下纵向轨迹表达式s(t)，如式(7)所示：
其中：
在一个实施例中，如图7所示，其为根据采样生成的四次多项式形式的曲线坐标纵向候选局部轨迹示意图。图中使用的样例s(0)＝0，采样末端时刻T∈[4，6，间隔为0.5s，末端采样纵向速度/> 间隔为2.5/3.6m/s。因此，末端时刻采样点个数为5，末端纵向速度采样点个数为5，共生成25条纵向候选局部轨迹。
S32d，在采用五阶贝塞尔曲线方法生成曲线坐标系下的纵向候选局部轨迹时，可得到s关于时间t的纵向候选局部轨迹五阶贝塞尔曲线函数s(t)，如式(9)所示：
其中，Ps0，Ps1，Ps2，Ps3，Ps4，Ps5为纵向轨迹五阶贝塞尔曲线的控制点，每个点包含一个s值，Ps0为初始状态纵向位移，Ps5为末状态纵向位移。与四次多项式方法生成纵向局部轨迹相同，可以从当前自车的车辆状态得到曲线坐标下的纵向运动初始状态包括初始纵向位移s(0)、初始纵向速度初始纵向加速度/>在与横向采样同样的末端时刻T下，对末端纵向速度/>进行采样，再令末端纵向位移/>末端纵向加速度/>为0。由此可知，s(0)即为Ps0，s(T)即为Ps4。依据上述信息，可确定Ps0，Ps1，Ps2，Ps3，Ps4，Ps5如表示为下式(10)：
在一个实施例中，如图8所示，其为根据采样生成的五阶贝塞尔曲线形式的曲线坐标纵向候选局部轨迹示意图。图中使用的样例s(0)＝0， 采样末端时刻T∈[4，6，间隔为0.5s，末端采样纵向速度/>间隔为2.5/3.6m/s。因此，末端时刻采样点个数为5，末端纵向速度采样点个数为5，共生成25条横向候选局部轨迹。
S33，在曲线坐标下，利用d(t)与s(t)为每一条横向候选局部轨迹与纵向候选局部轨迹分配相应的代价，d(t)为车辆在曲线坐标下横向位移随时间的变化，s(t)为车辆在曲线坐标下纵向位移随时间的变化。
横向候选局部轨迹代价计算公式(11)表示为：
其中，kj，kt，kd，kΔd为各项权重系数，对于各项权重系数具体数值的确定原则为：欲更倾向于选择横向局部轨迹舒适性更佳的候选局部轨迹，则可适当提高kj值；欲更倾向于选择横向局部轨迹效率更高的候选局部轨迹，则可适当提高kt值；欲车辆更倾向于行驶在道路中心，则可适当提高kd值；欲局部轨迹在横向运动上连续两帧不发生太大变化，则可适当提高kΔd值。依据一般的驾驶习惯，kj、kt、kΔd项的取值相对kd项较小。常用的一组数值为kj＝0.1，kt＝0.1，kd＝1，kΔd＝0.2。dp(T)表示上一个规划周期选择的最优局部轨迹对应的末端横向位移采样值。代价函数第一项为横向加加速度的积分和，其反映了轨迹的舒适性；第二项考虑了轨迹的效率，即对末端采样时间进行惩罚；第三项对采样的末端横向位移进行惩罚，使选择的最终轨迹尽可能靠近参考线；第三项对轨迹的切换进行惩罚，避免前后两个规划周期的轨迹在末端横向位移上频繁切换。
纵向轨迹代价计算公式表示为式(12)：
其中，kj，kt，为各项权重系数；/>表示期望达到的目标速度。代价函数第一项为纵向加加速度的积分和，其同样反映了轨迹的舒适性；第二项考虑了轨迹的效率，即对末端采样时间进行惩罚；第三项对采样末端速度与期望末端速度差值的平方进行惩罚，使选择的局部轨迹对于末端速度靠近目标速度。纵向局部轨迹的代价计算也采用与横向局部轨迹代价计算相同的kj与kt，若希望最优局部轨迹末速度更接近目标速度，则可适当提高值。为保证车辆在行驶过程中的整体的行驶效率，kj、kt项的取值相对/>项较小。常用的一组数值为kj＝0.1，kt＝0.1，/>
S34，将采样时间相同的曲线坐标横、纵向候选局部轨迹合成为一条曲线坐标系下的候选局部轨迹。其代价函数表示为式(13)：
Ctotal＝klatCd+klonCs (13)
其中，klat，klon为各项权重系数。
对于klat和klon的具体数值，可以通过适当提高klat的数值，来获得更倾向于选择横向局部轨迹质量高的候选局部轨迹，而通过适当提高klon的数值，则可以更倾向于选择纵向局部轨迹质量高的候选局部轨迹。一般情况下，klat与klon的值不应相差太大。常用的一组数值为klat＝1，klon＝1。
除了上述实施例给出的获取候选局部轨迹的方法之外，也可以采用动态窗口算法(DWA,dynamic window approach)，通过在速度空间(v，w)中采样得到未来一定时间内的候选局部轨迹，其中v为速度，w为角速度，设置评价函数为候选局部轨迹分配代价，选取最优轨迹为局部轨迹。还可以采用人工势场算法(APF，Artificial Potential Field)，设置距离当前位置一定距离的全局路径点为目标位置，并基于目标位置设置引力场，基于障碍物位置设置斥力场，将引力场与斥力场叠加生成势场函数，依据势场函数即可得到连接当前车辆位置与目标位置的局部路径，在此基础上进行速度规划便可以获得局部轨迹。甚至还可以采用现有的其他方法获得，在此不再一一列举。
S4，取出曲线坐标下代价最低的候选局部轨迹,将其转换至大地坐标下,根据所述步骤S2生成的所述避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新执行步骤S4，直至获得一条大地坐标下代价最低的无碰撞候选局部轨迹。
在一个实施例中，取出曲线坐标系下代价最低的候选局部轨迹，将其转换至大地坐标系下，即已知候选局部轨迹在曲线坐标系下各轨迹点的坐标(si，di)，通过si可定位到其对应大地坐标下参考路径的路径点n＝(xn，yn，θn)，以点n作为曲线坐标系转换至大地坐标系的投影点，利用下式(14)计算该候选局部轨迹点在大地坐标系下的坐标(xi，yi)：
当求得大地坐标系下候选局部轨迹的坐标(xi，yi)后，通过相邻轨迹点的相对位置及角度计算候选局部轨迹点角度θi以及曲率ki，如式(15)所示：
其中，(xi+1，yi+1，θi+1)、(xi-1，yi-1，θi-1)分别表示大地坐标系下当前候选局部轨迹点的相邻前向、后向候选局部轨迹点。
将其与障碍物进行碰撞检测，若当前轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条候选局部轨迹重新执行以上操作，直至得到一条最低代价的无碰撞轨迹。依据行为决策针对不同类型障碍物生成的绕行、跨越、穿越避障决策，采用不同的碰撞检测方法或不进行障碍物与候选局部轨迹碰撞检测。对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；对于采用穿越避障的障碍物，不进行碰撞检测。
所述步骤S8所采用的OBB碰撞检测算法利用了分离轴定理。分离轴定理其几何原理为：对于两个任意的凸多边形，当存在一条轴，使得两个凸多边形在该轴上的投影不相交，则这两个凸多边形不相交，该轴即为分离轴。如图9所示，为车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测的示意图。图中Lo，Wo，Lv，Wv分别为障碍物二维矩形包络和车身二维矩形包络的半长与半宽；T为两矩形中心连线。分别以障碍物二维矩形包络的中心点、车辆二维矩形包络的中心点和对应朝向角建立笛卡尔坐标系，得到4条投影轴：x(o)、y(o)、x(v)和y(v)；然后，将障碍物二维矩形包络半长Lo，障碍物二维矩形包络半宽Wo，车辆二维矩形包络半长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维矩形包络的中心点连线T向各投影轴分别做投影，如式(16)所示。
如果在4个投影轴中存在任一个投影都满足式(17)：
|Proj(T)|＞|Proj(Obstacle)|+|Proj(Vehicle)| (17)
则障碍物不与车身发生碰撞，否则发生碰撞。
图10为车轮二维矩形包络与障碍物二维矩形包络进行碰撞检测的示意图。使用四个车轮二维矩形包络分别与障碍物二维矩形包络进行OBB碰撞检测。
S5，对步骤S4获得的大地坐标系下代价最低的无碰撞候选局部轨迹进行插值，得到局部轨迹。
需要说明的是，以上步骤S1至S5并非严格的先后顺序，也可以有适当的调整，只要能够保证算法对于所有种类的障碍物都可得到有且只有一个的避障决策均可。
本发明实施例还包括具备多型避障方式的车辆局部轨迹规划***，其包括环境感知设备、决策规划设备和跟踪控制设备，其中：
环境感知设备通过激光雷达、摄像头等设备获取当前车辆位姿下周边环境中的障碍物信息。
地图模块拥有离线生成的高精地图，向决策规划模块提供地图信息。
定位模块利用IMU、轮速计、GNSS天线等设备获取当前自车信息。
HMI人机接口向决策规划模块提供目标终点信息。
决策规划设备用于接收环境感知设备获取的障碍物信息、地图模块获取的地图信息、定位模块获取的自车信息、HMI人机接口获取的目标终点信息，并基于上述信息进行决策规划，最终生成一条局部轨迹。
决策规划设备划分为三个功能模块，分别为全局规划功能模块、行为决策功能模块、局部规划功能模块；
其中，全局规划功能模块基于车辆自车信息、地图信息、目标终点信息规划一条连接当前车辆位姿与目标终点的全局路径，基于全局路径构造一条参考路径；
其中，行为决策功能模块用于从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；
其中，局部规划功能模块根据所述参考路径和当前自车信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价,根据所述行为决策模块生成的所述避障决策选择碰撞检测方法，取出代价最低的候选局部轨迹进行障碍物与局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该轨迹并重新取出代价最低的候选局部轨迹，直至取出的代价最低的无碰撞候选局部轨迹，并进行插值。
跟踪控制模块，接收决策规划模块生成的局部轨迹、定位模块获取的自车信息，依据上述信息并结合车辆参数进行横向控制和纵向控制，输出对车辆方向盘、油门、刹车等执行机构的控制信号。
在一个实施例中，所述行为决策功能模块具体包括：
筛选单元，保留障碍物中当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，并剔除其他障碍物；
分类单元，其用于依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物作为静态障碍物，速度vobs不为零的障碍物作为动态障碍物。
决策分配单元，其用于为分类后的每一障碍物分配对应的避障决策，其具体包括：
第一决策子单元，其用于对于动态障碍物，采用绕行避障决策；
第二决策子单元，其用于对于静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，具体包括步骤a1至步骤c1、或者步骤a2至步骤c2：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物三维边界框或三维包围盒的最高点与最低点，并进入步骤b1。
b1，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c1。若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策。
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与Lb-2d的大小关系，若wobs≥Lb-d或lobs≥Lb-d，则对该静态障碍物采取绕行避障决策；若wobs＜Lb-d且lobs＜Lb-d，则对该静态障碍物采取跨越避障决策。
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2；
b2，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c2；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策。
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策。
在一个实施例中，所述局部规划功能模块具体包括轨迹生成单元，其基于参考路径构建曲线坐标，并使用多次多项式方法或多阶贝塞尔曲线方法生成横、纵向候选局部轨迹，再为横、纵向候选局部轨迹分配代价，并将曲线坐标下的横、纵向候选局部轨迹合成为曲线坐标下的候选局部轨迹；
碰撞检测单元，其用于取出曲线坐标下代价最低的候选局部轨迹，将其转换至大地坐标系下，并依据不同的避障决策，采用下面不同的碰撞检测方法或不进行障碍物与该候选局部轨迹碰撞检测：
a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
c.对于采用穿越避障的障碍物，不进行碰撞检测；
进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条代价最低的候选局部轨迹重新执行以上操作，直至得到一条大地坐标系下最低代价的无碰撞候选局部轨迹。
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种具备多型避障方式的车辆局部轨迹规划方法，其特征在于，包括：
S1，获取当前自车位姿信息、当前车辆位姿下周边环境中的障碍物信息、地图信息和参考路径；
S2，从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；
S3，根据所述参考路径和当前自车位姿信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价；
S4，取出曲线坐标下代价最低的候选局部轨迹，将其转换至大地坐标下，根据所述步骤S2生成的所述避障决策选择碰撞检测方法，进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新执行步骤S4，直至获得一条大地坐标下代价最低的无碰撞候选局部轨迹；
S5，对步骤S4获得的大地坐标系下代价最低的无碰撞候选局部轨迹进行插值，得到局部轨迹，
所述步骤S2具体包括：
S21，依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物作为静态障碍物，速度vobs不为零的障碍物作为动态障碍物；
S22，为分类后的每一障碍物分配对应的避障决策，其具体包括：对于所述步骤S21判定的对于动态障碍物，采用绕行避障决策，
S22中，对于所述步骤S21判定的静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，在考虑安全裕量的情况下，具体包括：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b1；
b1，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c 1；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策；
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策；或者
S22中，对于所述步骤S21判定的静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，具体包括：
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2；
b2，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c2；若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策；
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第四预设值的大小关系，若wobs不小于第四预设值，或者，lobs不小于第四预设值，则对该静态障碍物采取绕行避障决策；若wobs小于第四预设值，并且，lobs小于第四预设值，则对该静态障碍物采取跨越避障决策，其中，第四预设值依据轮距Lb和车轮宽度d确定。
2.如权利要求1所述的具备多型避障方式的车辆局部轨迹规划方法，其特征在于，所述步骤S3具体包括：
S31，将自车大地坐标、参考路径进行曲线坐标转换；
S32，采用多次多项式方法或多阶贝塞尔曲线方法生成横向候选局部轨迹；
S33，利用d(t)与s(t)为每一条横向候选局部轨迹与纵向候选局部轨迹分配相应的代价，d(t)、s(t)分别表示车辆在曲线坐标下横、纵向位移随时间的变化；
横向候选局部轨迹的代价计算公式表示为式(11)：
其中，kj、kt、kd、kΔd分别表示权重系数，T表示由采样生成的横向候选局部轨迹末端时刻，dp(T)表示上一个规划周期选择的最优局部轨迹对应的末端横向位移采样值，d(T)表示当前横向候选局部轨迹在末端时刻的横向位移，表示车辆横向加速度；
纵向候选局部轨迹的代价计算公式表示为式(12)：
其中，kj、kt、分别表示权重系数，sd(T)表示在末端时刻期望达到的目标纵向位移，表示在末端时刻期望达到的目标纵向速度，s(T)表示当前纵向候选局部轨迹在末端时刻的纵向位移，/>表示当前纵向候选局部轨迹在末端时刻的纵向速度，/>表示车辆纵向加速度；
S34，将采样时间相同的曲线坐标横、纵向候选局部轨迹合成为一条曲线坐标系下的候选局部轨迹，其代价函数表示为式(13)：
Ctotal＝klat·Cd+klon·Cs (13)
其中，klat、klon分别表示权重系数。
3.如权利要求1所述的具备多型避障方式的车辆局部轨迹规划方法，其特征在于，所述步骤S4具体包括：
取出代价最低的候选局部轨迹，通过候选局部轨迹在曲线坐标系下各轨迹点(si，di)定位到其对应大地坐标下参考路径的路径点n＝(xn，yn，θn)，以点n作为曲线坐标系转换至大地坐标系的投影点，利用下式(14)计算该候选局部轨迹点在大地坐标系下的坐标(xi，yi)，再计算候选局部轨迹点角度θi以及曲率ki，如式(15)所示：
其中，(xi+1，yi+1，θi+1)、(xi-1，yi-1，θi-1)分别表示当前候选局部轨迹点的相邻前向、后向候选局部轨迹点，
并依据不同的避障决策，采用下面对应的碰撞检测方法进行障碍物与该候选局部轨迹的碰撞检测或不进行障碍物与该候选局部轨迹碰撞检测：
a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测，以判断车辆沿当前候选局部轨迹行驶时，车身与障碍物是否发生碰撞；
b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测，以判断车辆沿当前候选局部轨迹行驶时，车轮与障碍物是否发生碰撞；
c.对于采用穿越避障的障碍物，不进行碰撞检测；
进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条代价最低的候选局部轨迹重新执行以上操作，直至得到一条大地坐标系下最低代价的无碰撞候选局部轨迹。
4.如权利要求3所述的具备多型避障方式的车辆局部轨迹规划方法，其特征在于，OBB碰撞检测具体包括：
去除障碍物三维边界框或三维包围盒的高度信息，得到障碍物二维矩形包络信息；
依据车身参数与定位模块获取的车辆位置信息与朝向信息，得到车辆二维矩形包络信息；
分别以障碍物二维矩形包络的中心点、车辆二维矩形包络的中心点和对应朝向角建立笛卡尔坐标系，得到4条投影轴：x(o)、y(o)、x(v)和y(v)；
将障碍物二维矩形包络半长Lo，障碍物二维矩形包络半宽Wo，车辆二维矩形包络半长Lv，车辆二维矩形包络半宽Wv和障碍物二维矩形包络的中心点与车辆二维矩形包络的中心点连线T向各投影轴分别做投影，如式(16)所示；
如果在4个投影轴中存在任一个投影都满足式(17)，则障碍物不与车身发生碰撞，否则发生碰撞：
|Proj(T)|>|Proj(Obstacle)|+|Proj(Vehicle)| (17)
式中，Proj()为投影函数，|Proj()|为投影长度。
5.一种具备多型避障方式的车辆局部轨迹规划***，其特征在于，包括：
全局规划功能模块，其用于根据获取当前自车位姿信息、当前车辆位姿下周边环境中的障碍物信息和地图信息，规划一条连接当前车辆位姿与目标终点的全局路径，基于全局路径构造一条参考路径；
行为决策功能模块，其用于从所有障碍物中选择出当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，进行分类，并为分类后的每一障碍物分配对应的避障决策；
局部规划功能模块，其用于根据所述参考路径和当前自车信息进行局部轨迹规划，生成一系列候选局部轨迹，并依据评价函数为每一条候选局部轨迹分配代价，根据所述行为决策功能模块生成的所述避障决策选择碰撞检测方法，取出代价最低的候选局部轨迹进行障碍物与候选局部轨迹的碰撞检测，若该候选局部轨迹发生碰撞，则剔除该候选局部轨迹并重新取出代价最低的候选局部轨迹，直至取出的代价最低的无碰撞候选局部轨迹，并进行插值，得到局部轨迹，
所述行为决策功能模块具体包括：
筛选单元，其用于保留障碍物中当前车辆位置前方所述参考路径的道路宽度范围内的障碍物，并剔除其他障碍物；
分类单元，其用于依据障碍物速度vobs是否为零进行分类，速度vobs为零的障碍物作为静态障碍物，速度vobs不为零的障碍物作为动态障碍物；
决策分配单元，其用于为分类后的每一障碍物分配对应的避障决策，其具体包括：
第一决策子单元，其用于对于动态障碍物，采用绕行避障决策；
第二决策子单元，其用于对于静态障碍物，将其三维边界框或三维包围盒参数与车辆参数对比，依据对比结果获取避障决策，具体包括a1至c1、或者，a2至c2：
a1，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b1；
b1，判断最高点的高度是否低于第一预设值、以及判断最低点的高度是否高于第二预设值，若最高点的高度低于第一预设值，则进入步骤c 1；若最低点的高度高于第二预设值，则对该静态障碍物采取穿越避障决策；若最高点的高度高于第一预设值且低于第二预设值，或者，最低点的高度高于第一预设值且低于第二预设值，或者，最低点的高度低于第一预设值且最高点的高度高于第二预设值，则对该静态障碍物采取绕行避障决策；
c1，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第三预设值的大小关系，若wobs或lobs大于等于第三预设值，则对该静态障碍物采取绕行避障决策；若wobs与lobs同时小于第三预设值，则对该静态障碍物采取跨越避障决策；
a2，依据大地坐标系中静态障碍物三维边界框或三维包围盒中心点的高度参数zobs与静态障碍物三维边界框或三维包围盒的高度参数hobs，计算静态障碍物的最高点与最低点，并进入步骤b2；
b2，判断最高点的高度是否低于车辆底盘高度Hl、以及判断最低点的高度是否高于车辆整车高度Hh，若最高点的高度低于车辆底盘高度Hl，则进入步骤c2；若最低点的高度高于车辆整车高度Hh，则对该静态障碍物采取穿越避障决策；若最高点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度高于车辆底盘高度Hl且低于车辆整车高度Hh，或者，最低点的高度低于车辆底盘高度Hl且最高点的高度高于车辆整车高度Hh，则对该静态障碍物采取绕行避障决策；
c2，判断该静态障碍物三维边界框或三维包围盒宽度wobs和长度lobs与第四预设值的大小关系，若wobs不小于第四预设值，或者，lobs不小于第四预设值，则对该静态障碍物采取绕行避障决策；若wobs小于第四预设值，并且，lobs小于第四预设值，则对该静态障碍物采取跨越避障决策，其中，第四预设值依据轮距Lb和车轮宽度d确定。
6.如权利要求5所述的具备多型避障方式的车辆局部轨迹规划***，其特征在于，所述局部规划功能模块具体包括：
轨迹生成单元，其基于参考路径构建曲线坐标，并使用多次多项式方法或多阶贝塞尔曲线方法生成横、纵向候选局部轨迹，再为横、纵向候选局部轨迹分配代价，并将曲线坐标下的横、纵向候选局部轨迹合成为曲线坐标下的候选局部轨迹；
碰撞检测单元，其用于取出曲线坐标下代价最低的候选局部轨迹，将其转换至大地坐标系下，并依据不同的避障决策，采用下面不同的碰撞检测方法或不进行障碍物与该候选局部轨迹碰撞检测：
a.对于采用绕行避障的障碍物，使用车身二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
b.对于采用跨越避障的障碍物，使用车轮二维矩形包络与障碍物二维矩形包络进行OBB碰撞检测；
c.对于采用穿越避障的障碍物，不进行碰撞检测；
进行碰撞检测时，若当前候选局部轨迹与障碍物发生碰撞，则将其从候选局部轨迹序列中剔除并选择下一条代价最低的候选局部轨迹重新执行以上操作，直至得到一条大地坐标系下最低代价的无碰撞候选局部轨迹；
轨迹插值单元，其用于对大地坐标系下最低代价的无碰撞候选局部轨迹进行插值，生成局部轨迹。
CN202210328938.0A 2022-03-21 2022-03-30 一种具备多型避障方式的车辆局部轨迹规划方法及*** Active CN115525047B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/CN2022/111967 WO2023178910A1 (zh) 2022-03-21 2022-08-12 一种具备多型避障方式的车辆局部轨迹规划方法及***

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210279682 2022-03-21
CN2022102796829 2022-03-21

Publications (2)

Publication Number Publication Date
CN115525047A CN115525047A (zh) 2022-12-27
CN115525047B true CN115525047B (zh) 2023-07-25

Family

ID=84695723

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210328938.0A Active CN115525047B (zh) 2022-03-21 2022-03-30 一种具备多型避障方式的车辆局部轨迹规划方法及***

Country Status (2)

Country Link
CN (1) CN115525047B (zh)
WO (1) WO2023178910A1 (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116088538B (zh) * 2023-04-06 2023-06-13 禾多科技（北京）有限公司 车辆轨迹信息生成方法、装置、设备和计算机可读介质
CN117093005B (zh) * 2023-10-16 2024-01-30 华东交通大学 一种智能汽车自主避障方法
CN117075619B (zh) * 2023-10-17 2024-01-16 之江实验室 一种局部路径规划方法、装置和介质
CN117406755B (zh) * 2023-12-01 2024-04-19 北京极智嘉科技股份有限公司 机器人避障方法、装置、设备和可读存储介质
CN117804476B (zh) * 2023-12-07 2024-06-18 无锡物联网创新中心有限公司 基于改进dwa和prm算法的移动机器人动态路径规划方法
CN117826819B (zh) * 2024-01-02 2024-06-21 中国矿业大学 一种基于自适应动态窗口法的uuv三维避障规划方法

Family Cites Families (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106774347A (zh) * 2017-02-24 2017-05-31 安科智慧城市技术（中国）有限公司 室内动态环境下的机器人路径规划方法、装置和机器人
CN109709945B (zh) * 2017-10-26 2022-04-15 深圳市优必选科技有限公司 一种基于障碍物分类的路径规划方法、装置及机器人
US20190286145A1 (en) * 2018-03-14 2019-09-19 Omron Adept Technologies, Inc. Method and Apparatus for Dynamic Obstacle Avoidance by Mobile Robots
CN111368607B (zh) * 2018-12-26 2023-06-02 北京欣奕华科技有限公司 一种机器人、障碍物的检测方法及检测装置
CN109960261B (zh) * 2019-03-22 2020-07-03 北京理工大学 一种基于碰撞检测的动态障碍物避让方法
CN110597260A (zh) * 2019-09-20 2019-12-20 深圳乐动机器人有限公司 一种机器人避障的方法及机器人
CN110766220A (zh) * 2019-10-21 2020-02-07 湖南大学 一种结构化道路局部路径规划方法
CN111338335B (zh) * 2019-12-31 2021-02-26 清华大学 一种结构化道路场景下的车辆局部轨迹规划方法
CN111469836B (zh) * 2020-02-28 2022-12-20 广东中科臻恒信息技术有限公司 基于车载单元和路侧单元的避障方法、设备、存储介质
CN111806433B (zh) * 2020-06-09 2022-07-12 宁波吉利汽车研究开发有限公司 一种自动驾驶车辆避障方法、装置及设备
CN111694356B (zh) * 2020-06-10 2023-06-27 上海高仙自动化科技发展有限公司 一种行驶控制方法、装置、电子设备及存储介质
CN111679678B (zh) * 2020-06-30 2022-04-08 安徽海博智能科技有限责任公司 一种横纵向分离的轨迹规划方法、***及计算机设备
CN111845774B (zh) * 2020-07-20 2021-12-03 上海大学 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
CN111856507B (zh) * 2020-07-28 2023-03-28 上海木木聚枞机器人科技有限公司 一种环境感测实现方法、智能移动设备和存储介质
CN112415998B (zh) * 2020-10-26 2024-06-18 珠海一微半导体股份有限公司 一种基于tof摄像头的障碍物分类避障控制***
CN112362074B (zh) * 2020-10-30 2024-03-19 重庆邮电大学 一种结构化环境下的智能车辆局部路径规划方法
CN112947419B (zh) * 2021-01-27 2023-03-21 河北工业职业技术学院 避障方法、装置及设备
CN113386795B (zh) * 2021-07-05 2022-07-01 西安电子科技大学芜湖研究院 一种自动驾驶车辆智能决策及局部轨迹规划方法及其决策***
CN114047753B (zh) * 2021-11-03 2023-02-03 哈尔滨鹏路智能科技有限公司 一种基于深度视觉的扫地机器人障碍物识别和避障方法

Also Published As

Publication number Publication date
WO2023178910A1 (zh) 2023-09-28
CN115525047A (zh) 2022-12-27

Similar Documents

Publication Publication Date Title
CN115525047B (zh) 一种具备多型避障方式的车辆局部轨迹规划方法及***
CN107702716B (zh) 一种无人驾驶路径规划方法、***和装置
CN112964271B (zh) 一种面向多场景的自动驾驶规划方法及***
Banzhaf et al. The future of parking: A survey on automated valet parking with an outlook on high density parking
CN110262508B (zh) 应用于封闭场地无人驾驶货运车辆上的自动引导***及方法
US11698638B2 (en) System and method for predictive path planning in autonomous vehicles
CN111006667B (zh) 高速场景下的自动驾驶轨迹生成***
CN111845774A (zh) 一种基于横纵向协调的自动驾驶汽车动态轨迹规划及跟踪方法
CN114005280B (zh) 一种基于不确定性估计的车辆轨迹预测方法
CN113031583A (zh) 一种结构化道路避障方法
CN109084798A (zh) 网络下发带有道路属性的控制点的路径规划方法
CN109945885B (zh) 无人摩托的动态障碍物避障路径规划计算方法
Ben-Messaoud et al. Smooth obstacle avoidance path planning for autonomous vehicles
CN111896004A (zh) 一种狭窄通道车辆轨迹规划方法及***
CN113291318B (zh) 基于部分可观测马尔科夫模型的无人车盲区转弯规划方法
CN113720346B (zh) 基于势能场和隐马尔可夫模型的车辆路径规划方法及***
CN118235180A (zh) 预测可行驶车道的方法和装置
CN115257746A (zh) 一种考虑不确定性的自动驾驶汽车换道决策控制方法
CN113515111B (zh) 一种车辆避障路径规划方法及装置
CN114063615A (zh) 棚内垄间农药喷洒智能车的倒车导航控制方法及***
CN114637292A (zh) 一种兼顾避障的车辆轨迹跟踪鲁棒控制方法和***
CN113375672B (zh) 一种无人飞行器的高实时航迹避让方法及***
Nagasaka et al. Towards safe, smooth, and stable path planning for on-road autonomous driving under uncertainty
CN211427151U (zh) 一种应用于封闭场地无人驾驶货运车辆上的自动引导***
CN116465427A (zh) 一种基于时空风险量化的智能车辆换道避障路径规划方法

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