CN109557912B - 一种自动驾驶特种作业车辆的决策规划方法 - Google Patents
一种自动驾驶特种作业车辆的决策规划方法 Download PDFInfo
- Publication number
- CN109557912B CN109557912B CN201811183181.0A CN201811183181A CN109557912B CN 109557912 B CN109557912 B CN 109557912B CN 201811183181 A CN201811183181 A CN 201811183181A CN 109557912 B CN109557912 B CN 109557912B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- track
- path
- decision
- mode
- 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 31
- 230000007613 environmental effect Effects 0.000 claims abstract description 13
- 230000008447 perception Effects 0.000 claims abstract description 9
- 238000000354 decomposition reaction Methods 0.000 claims abstract description 7
- 238000005457 optimization Methods 0.000 claims abstract description 5
- 235000019580 granularity Nutrition 0.000 claims description 18
- 239000006185 dispersion Substances 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 3
- 230000009191 jumping Effects 0.000 description 21
- 238000010586 diagram Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 230000006872 improvement Effects 0.000 description 2
- 208000027418 Wounds and injury Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 230000006378 damage Effects 0.000 description 1
- 230000002498 deadly effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000012634 fragment Substances 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 208000014674 injury Diseases 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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
-
- 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/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
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)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
- Traffic Control Systems (AREA)
- Steering Control In Accordance With Driving Conditions (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种自动驾驶特种作业车辆的决策规划方法。包括以步骤:1)自动驾驶作业模块获取自车的当前定位位姿;2)将感知***发送的环境信息投影到栅格地图,并生成环境地图;3)自动驾驶作业模块获取当前作业执行器的控制指令并下发;4)自动驾驶作业模块获取任务参考路径,采用路径‑速度分解的轨迹规划方法结合车辆动力学约束进行轨迹簇规划,获取车辆可执行的基础轨迹簇,将基础轨迹簇和任务参考路径融合得到可执行轨迹簇;5)对规划的可执行轨迹簇进行安全性和高效性的择优,最终生成高收益轨迹。与现有技术相比,本发明具有提高避障成功率、自动决策、多模式的轨迹决策策略、实现自动驾驶安全性等优点。
Description
技术领域
本发明涉及车辆轨迹规划领域,尤其是涉及一种自动驾驶特种作业车辆的决策规划方法。
背景技术
近年来,人工智能技术的飞跃发展、计算机硬件运算能力大幅提升、感知***的不断完善以及车辆电动化、线控化的日趋成熟使自动驾驶技术的落地成为可能。无人驾驶乘用车是目前自动驾驶技术应用最为激烈的方向,包括像谷歌Waymo、百度Apollo等,然而,类似矿山车、清扫车、渣土车等的特种作业车辆有着比乘用车对自动驾驶技术更为急切的需求,这不仅仅是出于行车安全的考虑,同等重要的还有驾驶员劳动负荷的降低、用工需求增加与熟练工人不足矛盾的缓解。因而,特种作业车辆的自动驾驶技将会是市场竞争的另一个焦点。
现有自动驾驶特种作业车辆,轨迹规划往往是考虑一次规划一条曲线,为了能够规避障碍物,必须在距离障碍物较远的地方就开始规划绕开障碍物;轨迹决策要么采用保守的策略,遇到障碍物就停车,要么采取激进策略,遇到障碍物直接绕行。这都将极大的降低特种作业车辆的作业效率。此外,由于感知***的FOV、分辨率和测量精度导致的测量误差、栅格化后的***误差以及障碍物的遮挡,传递给决策规划的环境地图存在严重的不确定性,出于对自动驾驶特种作业车辆的作业安全考虑,规划决策需要能兼容这些不确定性。
因此,如何提供一种自动驾驶特种作业车辆的决策规划方法解决上述问题的自动驾驶特种车辆决策规划策略是本领域技术人员亟待解决的问题。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种自动驾驶特种作业车辆的决策规划方法自动驾驶特种作业车辆的轨迹决策规划方法。
本发明的目的可以通过以下技术方案来实现:
一种自动驾驶特种作业车辆的决策规划方法,包括以下步骤:
1)特种作业车辆的自动驾驶作业模块通过GPS/IMU获取自车的当前定位位姿,包括经度、纬度、航向以及当前的定位状态;
2)以自车为中心将感知***发送的环境信息投影到栅格地图,并在栅格地图中标注静态和动态障碍物生成环境地图;
3)根据自车的当前定位位姿,自动驾驶作业模块通过存储在本地或远程发送的任务文件,获取当前作业执行器的控制指令下发到运动控制***;
4)自动驾驶作业模块获取存储在本地或远程发送的任务参考路径,并将任务参考路径投影到环境地图,采用路径-速度分解的轨迹规划方法结合车辆动力学约束进行轨迹簇规划,获取车辆可执行的基础轨迹簇,将基础轨迹簇和任务参考路径融合得到可执行轨迹簇;
轨迹融合的过程可以看作是轨迹片段的衔接与曲率平滑过程对于避障规划的轨迹,轨迹片段依次由任务参考轨迹、避障轨迹、任务参考路径平移轨迹衔接而成,在衔接点用三次B样条拟合实现曲率的连续,对于并道/返回规划的轨迹,轨迹片段依次由任务参考轨迹平移轨迹、并道/返回轨迹、任务参考轨迹迹衔接而成,在衔接点用三次B样条拟合实现曲率的连续,基础轨迹簇完成的是避障轨迹或者并道/返回轨迹的规划;
5)考虑环境感知的不确定性,对规划的可执行轨迹簇进行安全性和高效性的择优,最终生成一条车辆可执行的高效作业且低碰撞风险的高收益轨迹;
6)根据高收益轨迹与自车当前的定位位姿,获取航向角偏差和横向偏差,并下发给运动控制***进行实时路径控制。
所述的自动驾驶作业模块包括三个功能区,分别为决策规划、作业控制和行车控制。
在路径-速度分解的轨迹规划方法中,将轨迹规划解耦为路径规划和速度规划,可先进行速度规划,再进行路径规划,也可先进行路径规划,再进行速度规划。
所述的步骤4)中,车辆动力学约束包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度和路面附着系数约束。
所述的步骤4)中,车辆可执行的基础轨迹簇的起点为车辆当前的位姿点,或车辆当前执行轨迹安全距离内的一点;
车辆可执行的基础轨迹簇的终点具有目标偏向性,即:
当车辆在任务参考路径上时,为规避障碍物,在任务参考路径所在的可行驶区域内,对任务参考路径进行横纵向粒度不一致的离散得到目标点集,当车辆在避障执行轨迹上时,为返回任务参考路径进行作业,在避障执行轨迹和任务参考路径所在的可行驶区域内,对避障执行轨迹进行横纵向粒度不一致的离散以及对任务参考路径进行纵向粒度一致的离散得到目标点集。
所述的步骤4)中,可执行轨迹簇由四种模式的轨迹片段组成,包括任务参考轨迹、避障轨迹、任务参考轨迹平移轨迹和并道/返回轨迹。
所述的步骤5)中,考虑环境感知的不确定性包括感知***的FOV、分辨率以及测量精度导致测量误差和栅格化后的***误差。
所述的步骤5)中,轨迹择优根据车辆当前所处模式,划分为避障决策、并道/返回决策以及并道/返回循迹三种模式。
对任务参考路径进行横纵向粒度不一致的离散,具体为:
横向越偏离参考路径,离散的目标点越密集,纵向距规划起点越远,离散的目标点越稀疏;
对任务参考路径进行纵向粒度一致的离散得到目标点集,具体为:
沿参考路径的纵向,等间距离散出多个目标点,所有目标点作为自动驾驶特种作业车辆的并道点。
所述的避障决策模式根据自车相对任务参考路径上的障碍物的距离分为由有限状态机实现功能切换的6个子模式,包括自由循迹子模式、规划调整子模式、最优决策子模式、轨迹执行子模式、紧急制动子模式和紧急避障转向子模式;
所述的并道/返回决策模式根据自车相对最优并道/返回点的距离分为由有限状态机实现功能切换的3个子模式,包括避障循迹子模式、并道/返回决策子模式、紧急制动子模式;
所述的并道/返回循迹模式根据自车相对障碍物的距离和所处轨迹片段分为由有限状态机实现功能切换的2个模式,包括循迹子模式和紧急制动子模式。
所述的避障决策模式具体为:
当处在自由循迹子模式时,表明障碍物非常远或者没有障碍物,此时车辆继续执行当前作业轨迹;当处在规划调整模式时,表明当前作业轨迹上存在较近的障碍物,需要调用规划模块,规划避障的基础路径簇,在规划完成后,以路径簇终点偏离当前作业轨迹的横向距离为指标,进行由大到小排序,然后由小到大依次与障碍物进行碰撞检测,第一条不发生碰撞的路径作为候选路径,若都有碰撞,选择终点离作业轨迹横向最远那一条作为候选路径,将候选路径存入序列Q1中;当处在最优决策子模式时,调取序列Q1中所有的候选路径,若无候选路径,则执行轨迹不改变,若有候选路径,选择候选路径中终点离作业轨迹横向最远的那一条作为候选执行路径,并进行速度规划;当处在轨迹执行子模式时,将候选轨迹调取,并执行;当处在紧急制动子模式时,表明无有效避障的执行轨迹,此时进行紧急制动,以免发生碰撞;当处在紧急制动子模式时,表明紧急制动还是不能避免发生碰撞,只能通过将方向盘打死,尽可能避免碰撞带来的损失。
所述的并道/返回决策模式具体为:
当处在避障循迹子模式时,表明当前还不能返回作业轨迹,需要先平移部分作业轨迹到当前执行轨迹上,以便车辆还有后续参考轨迹可执行,并且不断规划返回作业轨迹的路径簇,按返回作业轨迹的切换点离车辆的距离由近及远排序,依次进行碰撞检测,第一条不碰撞的路径作为候选路径,存入序列Q2中;当处在并道/返回决策子模式时,调取序列Q2中所有的候选路径,选择候选路径中切换点最近的那一条路径作为候选路径,并进行速度规划,将该轨迹作为并道/返回决策轨迹;当处在紧急制动子模式时,表明无有效的执行轨迹,此时进行紧急制动,以免发生碰撞。
所述的并道/返回循迹模式具体为:
当处在循迹子模式时,执行并道/返回决策模式决策出来的并道/返回决策轨迹;当处在紧急制动子模式时,表明无有效的执行轨迹,此时进行紧急制动,以免发生碰撞。
与现有技术相比,本发明具有如下优点:
一、本发明中作业人员仅需要在作业区域附近打开自动驾驶作业模式开关,***能够自动调取作业任务与作业路线等信息,规划此后动作,无需人类参与判断;
二、本发明中的决策规划模块在获取定位信息、环境信息、任务模式和任务参考路径后能够完全自动地进行特种作业,将作业过程和行车过程程序化和自动化,提高特种作业效率,降低作业的劳动负荷。
三、本发明提供了一种目标偏向性规划方法,主要包括横纵向粒度不一致和纵向粒度一致的基础轨迹簇规划方法,以及融合任务参考路径和基础轨迹簇的规划策略,提高了避障的成功率,同时增加了快速返回任务参考路径的可能,提升作业效率。
四、本发明使用多模式的轨迹决策策略,通过对最优轨迹的不断检测,以及紧急避障与紧急转向的模式接入,可以实现轨迹对感知环境的不确定性进行兼容,实现自动驾驶的安全性。
附图说明
图1为本发明的方法流程图。
图2为本发明的避障规划的起点和横纵向粒度不一致的目标点离散示意图。
图3为本发明的并道/返回规划的起点和横纵向粒度不一致与纵向粒度一致的目标点离散示意图。
图4为本发明的轨迹组成片段示意图。
图5为本发明的轨迹决策的有限状态机切换示意图。
图6为本发明的避障决策细分的六个子模式示意图。
图7为本发明的并道/返回决策细分的三个子模式示意图。
图8为本发明的并道/返回循迹细分的两个子模式示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。注意,以下的实施方式的说明只是实质上的例示,本发明并不意在对其适用物或其用途进行限定,且本发明并不限定于以下的实施方式。
实施例
如图1所示,本发明提供一种自动驾驶特种作业车辆的决策规划方法,具体步骤包括:
步骤1:由驾驶员控制至自动驾驶作业区附近,开启自动驾驶作业模式,自动驾驶作业模块接管整车控制,包括作业控制和行车控制;
步骤2:自动驾驶作业模块通过GPS/IMU确定自车的当前位姿,主要包括经度、纬度、航向以及当前的定位状态;
步骤3:以自车为中心,将感知***发送的环境信息投影到一张栅格地图当中,无障碍物的栅格统一标注为0;对于有障碍物的栅格,可以根据速度信息标注为静态或者动态;对于障碍物的类型,同样可以根据感知的识别结果,标注为水泥路沿、灌木等等。
步骤4:根据自车的当前定位位姿,自动驾驶作业模块调用存储在本地或者远程发送的任务文件,决策出当前作业执行器的控制指令,并下发;
步骤5:根据自车的当前定位位姿,自动驾驶作业模块调用存储在本地或者远程发送的任务参考路径;
步骤6:将步骤5获得的任务参考路径投影到步骤3中获得的环境地图,并进行轨迹簇规划,采用路径-速度分解的策略,结合车辆动力学约束,规划出车辆可执行的基础轨迹簇,然后将基础轨迹簇和任务参考路径融合得到可执行轨迹簇,具体实施步骤:
步骤61:采用路径-速度分解的轨迹规划策略,将轨迹规划解耦为路径规划和速度规划两部分。
步骤62:如图2和图3所示,以车辆当前点为规划起点;
步骤63:如图2所示,当车辆在任务参考路径上时,在任务参考路径所在的可行驶区域内,对任务参考路径进行横纵向粒度不一致的离散得到终点目标点集;如图3所示,当车辆在避障执行轨迹上时,在避障执行轨迹和任务参考路径所在的可行驶区域内,对避障执行轨迹进行横纵向粒度不一致的离散和对任务参考路径进行纵向粒度一致的离散得到终点目标点集;
步骤64:结合车辆动力学约束,包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度、路面附着系数等等与车辆动力学相关的约束,计算出路径的曲率约束,通过最优化方法,规划出N个离散路径点,并通过三次B样条平滑;
步骤65:将任务参考路径进行裁剪,剩余部分横向偏移到规划的基础路径目标点,并结合车辆动力学约束,包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度、路面附着系数等等与车辆动力学相关的约束,计算出路径的曲率约束,通过最优化方法,规划出对应数量的离散速度点,并通过三次B样条平滑;
步骤6:如图4所示,融合任务参考路径和基础轨迹的轨迹簇进行片段划分,来自于任务参考路径的轨迹片段划分为任务参考轨迹(ID1),来自于避障规划的轨迹片段划分为避障轨迹(ID2),来自于任务参考路径平移的轨迹划分为任务参考轨迹平移轨迹(ID3),来自于并道/返回轨迹规划的轨迹片段划分为并道/返回轨迹(ID4);
步骤7:从步骤6规划出的轨迹簇中,考虑环境感知的不确定性,不断地对规划的可执行轨迹簇进行安全性和高效性的择优,最终决策输出一条车辆可执行的高效作业且低碰撞风险的高收益轨迹。如图5所示,轨迹决策分位避障决策、并道/返回决策、并道/返回循迹三大块,其中,如图6所示,避障决策依次是自由循迹模式(S1)、规划调整模式(S2)、最优决策模式(S3)、轨迹执行模式(S4)、紧急制动模式(S5)和紧急避障转向模式(S6);如图7所示,并道/返回决策依次是避障循迹模式(B1)、并道/返回决策模式(B2)、紧急制动模式(B3);如图8所示,并道/返回循迹依次是循迹(C1)、紧急制动(C2),具体实施步骤:
步骤71:沿任务参考路径或当前执行轨迹,以车宽加上一定的横向安全裕度,往前搜索障碍物,若障碍物不存在,执行任务参考路径或当前执行轨迹的循迹,若存在障碍物,按照当前所处模式,分别跳转到步骤72、步骤73或者步骤74。
步骤72:如图5的避障决策状态机所示,若是避障决策,具体步骤为:
步骤721:如图5所示,默认从自由循迹模式(S1)区开始,若无障碍物或障碍物距离落在S1区间,则执行任务参考路径或当前执行轨迹的循迹;若有障碍物,距离落在S2区间(跳转条件E1),则跳转到规划调整模式(S2)区;若有障碍物,距离落在S5区(跳转条件E5),则跳转到紧急制动模式(S5)区;若有障碍物,距离落在S6区(跳转条件E6),则跳转到紧急避障转向模式(S6)区。
步骤722:如图5所示,若车辆与障碍物的距离落在规划调整模式(S2)区,则执行步骤6所述的轨迹簇规划,并继续执行任务参考路径或当前执行轨迹的循迹,在每次规划完成后,按照目标点的横向距离由远及近逐条进行碰撞检测,若无碰轨迹,则最优点Best_Point_queue队列记为无效,若存在无碰轨迹,则更新Best_Point_queue队列为最靠近障碍物的那条轨迹序号;若障碍物消失,或障碍物的距离落在S1区(跳转条件E2),则执行自由循迹模式(S1);若障碍物的距离落在S3区(跳转条件E3),则跳转到最优决策模式(S3)区;若有障碍物,距离落在S5区(跳转条件E5),则跳转到紧急制动模式(S5)区;若有障碍物,距离落在S6区(跳转条件E6),则跳转到紧急避障转向模式(S6)区。
步骤723:如图5所示,若车辆与障碍物的距离落在最优决策模式(S3)区,则将步骤722队列Best_Point_queue所有有效点取出,选择偏离横向障碍物最远的那个点作为最优点Best_Point;若障碍物的距离落在S4区(跳转条件E4),则跳转到轨迹执行模式(S4)区;若有障碍物,距离落在S5区(跳转条件E5),则跳转到紧急制动模式(S5)区;若有障碍物,距离落在S6区(跳转条件E6),则跳转到紧急避障转向模式(S6)区。
步骤724:如图5所示,若车辆与障碍物的距离落在轨迹执行模式(S4)区,以步骤723的Best_Point为终点,规划出一条完整的轨迹,并以此作为执行轨迹(跳转条件H1),跳转到并道/返回决策;若有障碍物,距离落在S5区(跳转条件E5),则跳转到紧急制动模式(S5)区;若有障碍物,距离落在S6区(跳转条件E6),则跳转到紧急避障转向模式(S6)区。
步骤725:如图5所示,若车辆与障碍物的距离落在紧急制动模式(S5)区,落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大,覆盖了作业车可绕行的区间),为避免碰撞,必须采取紧急制动;若有障碍物,距离落在S6区(跳转条件E6),则跳转到紧急避障转向模式(S6)区;若无障碍物,或障碍物的距离落在S1~S4区间(跳转条件E7),则跳转到规划调整模式(S2)区。
步骤726:如图5所示,若车辆与障碍物的距离落在紧急避障转向模式(S6)区,落在该区表明障碍物是突然出现的(动态障碍物,极有可能是人),紧急制动已难以避免碰撞,为尽可能降低伤害,将方向盘按避障方向打死,紧急避开障碍物;若无障碍物,或障碍物的距离落在S1~S5区间(跳转条件E8),则跳转到紧急制动模式(S5)区。
步骤73:如图5所示,若是并道/返回决策,具体步骤为:
步骤731:如图5所示,默认进入避障循迹模式(B1)区,执行避障轨迹的循迹,并且不断规划并道/返回的轨迹,更新Best_Point_queue队列为最先并道/返回的轨迹序号;在若车辆与障碍物的距离落在并道/返回决策模式(B2)区或者无障碍物且车辆到达ID3轨迹片段(跳转条件F1),则跳转到并道/返回决策模式(B2)区;若车辆与障碍物的距离落在B3区(跳转条件F3),则跳转到紧急制动模式(B3)区。
步骤732:如图5所示,若车辆与障碍物的距离落在并道/返回决策模式(B2)区或者无障碍物且车辆到达ID3轨迹片段,将步骤731队列Best_Point_queue所有有效点取出,选择序号最大的那个点作为最优点Best_Point,规划出一条完整的轨迹,并以此作为执行轨迹(跳转条件H2),跳转到并道/返回循迹;若无障碍物且车辆到达ID3轨迹片段(跳转条件F2),则跳转到避障循迹模式(B1)区;若车辆与障碍物的距离落在B3区(跳转条件F3),则跳转到紧急制动模式(B3)区。
步骤733:如图7所示,若车辆与障碍物的距离落在紧急制动模式(B3)区,落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大,覆盖了作业车可绕行的区间),为避免碰撞,必须采取紧急制动;若无障碍物或者障碍物距离落在B1~B2区(跳转条件F4),则跳转到避障循迹模式(B1)区。
步骤74:如图5所示,若是并道/返回循迹,具体步骤为:
步骤731:如图5所示默认进入循迹(C1)区,执行并道/返回轨迹的循迹,如车辆当前位置在ID1轨迹片段(跳转条件H3),则跳转到避障决策;若有障碍物且距离落在C2区(跳转条件G1),则跳转到紧急制动(C2)区。
步骤732:如图5所示,若车辆与障碍物的距离落在紧急制动(C2)区,落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大,覆盖了作业车可绕行的区间),为避免碰撞,必须采取紧急制动;若无障碍物或障碍物距离落在C1区(跳转条件G2),则跳转到循迹(C1)区。
步骤8:根据执行轨迹与自车当前的位姿,计算出航向角偏差和横向偏差,并下发给运动控制***。
本发明依次执行获取自车的定位信息;获取自车周围的环境信息;根据自车的定位信息确定作业任务模式;根据自车的定位信息获取任务参考路径;根据作业任务模式确定作业执行器控制指令;结合任务参考路径和自车周围的环境信息,进行轨迹规划,得到可执行轨迹簇;轨迹决策从可执行轨迹簇当中决策出一条满足安全性与高效性的执行轨迹;根据执行轨迹与自车当前的位姿,计算出航向角和横向偏差,并下发给运动控制***。其中,轨迹规划模块采用路径-速度分解的策略,结合车辆动力学约束,规划出车辆可执行的基础轨迹簇,然后将基础轨迹簇和任务参考路径融合得到可执行轨迹簇;轨迹决策模块考虑规划对环境不确定性的兼容,通过不断地对规划的可执行轨迹簇进行安全性和高效性的择优,最终决策输出一条车辆可执行的高效作业且低碰撞风险的高收益轨迹。
上述实施方式仅为例举,不表示对本发明范围的限定。这些实施方式还能以其它各种方式来实施,且能在不脱离本发明技术思想的范围内作各种省略、置换、变更。
Claims (8)
1.一种自动驾驶特种作业车辆的决策规划方法,其特征在于,包括以下步骤:
1)特种作业车辆的自动驾驶作业模块通过GPS/IMU获取自车的当前定位位姿,包括经度、纬度、航向以及当前的定位状态;
2)以自车为中心将感知***发送的环境信息投影到栅格地图,并在栅格地图中标注静态和动态障碍物生成环境地图;
3)根据自车的当前定位位姿,自动驾驶作业模块通过存储在本地或远程发送的任务文件,获取当前作业执行器的控制指令下发到运动控制***;
4)自动驾驶作业模块获取存储在本地或远程发送的任务参考路径,并将任务参考路径投影到环境地图,采用路径-速度分解的轨迹规划方法结合车辆动力学约束进行轨迹簇规划,获取车辆可执行的基础轨迹簇,将基础轨迹簇和任务参考路径融合得到可执行轨迹簇,车辆可执行的基础轨迹簇的起点为车辆当前的位姿点,或车辆当前执行轨迹安全距离内的一点;
车辆可执行的基础轨迹簇的终点具有目标偏向性,即:
当车辆在任务参考路径上时,为规避障碍物,在任务参考路径所在的可行驶区域内,对任务参考路径进行横纵向粒度不一致的离散得到目标点集,当车辆在避障执行轨迹上时,为返回任务参考路径进行作业,在避障执行轨迹和任务参考路径所在的可行驶区域内,对避障执行轨迹进行横纵向粒度不一致的离散以及对任务参考路径进行纵向粒度一致的离散得到目标点集,对任务参考路径进行横纵向粒度不一致的离散,具体为:
横向越偏离参考路径,离散的目标点越密集,纵向距规划起点越远,离散的目标点越稀疏;
对任务参考路径进行纵向粒度一致的离散得到目标点集,具体为:
沿参考路径的纵向,等间距离散出多个目标点,所有目标点作为自动驾驶特种作业车辆的并道点;
5)考虑环境感知的不确定性,对规划的可执行轨迹簇进行安全性和高效性的择优,最终生成一条车辆可执行的高效作业且低碰撞风险的高收益轨迹;
6)根据高收益轨迹与自车当前的定位位姿,获取航向角偏差和横向偏差,并下发给运动控制***进行实时路径控制。
2.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的自动驾驶作业模块包括三个功能区,分别为决策规划、作业控制和行车控制。
3.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,在路径-速度分解的轨迹规划方法中,将轨迹规划解耦为路径规划和速度规划,可先进行速度规划,再进行路径规划,也可先进行路径规划,再进行速度规划。
4.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的步骤4)中,车辆动力学约束包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度和路面附着系数约束。
5.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的步骤4)中,可执行轨迹簇由四种模式的轨迹片段组成,包括任务参考轨迹、避障轨迹、任务参考轨迹平移轨迹和并道/返回轨迹。
6.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的步骤5)中,考虑环境感知的不确定性包括感知***的FOV、分辨率以及测量精度导致测量误差和栅格化后的***误差。
7.根据权利要求1所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的步骤5)中,轨迹择优根据车辆当前所处模式,划分为避障决策、并道/返回决策以及并道/返回循迹三种模式。
8.根据权利要求6所述的一种自动驾驶特种作业车辆的决策规划方法,其特征在于,所述的避障决策模式根据自车相对任务参考路径上的障碍物的距离分为由有限状态机实现功能切换的6个子模式,包括自由循迹子模式、规划调整子模式、最优决策子模式、轨迹执行子模式、紧急制动子模式和紧急避障转向子模式;
所述的并道/返回决策模式根据自车相对最优并道/返回点的距离分为由有限状态机实现功能切换的3个子模式,包括避障循迹子模式、并道/返回决策子模式、紧急制动子模式;
所述的并道/返回循迹模式根据自车相对障碍物的距离和所处轨迹片段分为由有限状态机实现功能切换的2个模式,包括循迹子模式和紧急制动子模式。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811183181.0A CN109557912B (zh) | 2018-10-11 | 2018-10-11 | 一种自动驾驶特种作业车辆的决策规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811183181.0A CN109557912B (zh) | 2018-10-11 | 2018-10-11 | 一种自动驾驶特种作业车辆的决策规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109557912A CN109557912A (zh) | 2019-04-02 |
CN109557912B true CN109557912B (zh) | 2020-07-28 |
Family
ID=65864885
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811183181.0A Active CN109557912B (zh) | 2018-10-11 | 2018-10-11 | 一种自动驾驶特种作业车辆的决策规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109557912B (zh) |
Families Citing this family (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110196592B (zh) * | 2019-04-26 | 2023-07-28 | 纵目科技(上海)股份有限公司 | 一种轨迹线的更新方法、***、终端和存储介质 |
CN109976387B (zh) * | 2019-05-07 | 2022-05-27 | 福建盛海智能科技有限公司 | 一种无人车探测轨迹的方法及终端 |
CN110502004B (zh) * | 2019-07-01 | 2022-05-17 | 江苏大学 | 一种面向智能车辆激光雷达数据处理的行驶区域重要性权值分布建模方法 |
CN110568841A (zh) * | 2019-08-05 | 2019-12-13 | 西藏宁算科技集团有限公司 | 一种自动驾驶决策方法及*** |
CN112578788B (zh) * | 2019-09-30 | 2023-05-02 | 北京百度网讯科技有限公司 | 车辆避障二次规划方法、装置、设备和可读存储介质 |
CN112859829B (zh) * | 2019-11-28 | 2023-01-20 | 北京百度网讯科技有限公司 | 一种车辆控制方法、装置、电子设备和介质 |
CN111142532B (zh) * | 2020-01-03 | 2023-04-28 | 三一汽车制造有限公司 | 基于5g网络通讯的无人驾驶压路机群避障方法和*** |
CN111473795B (zh) * | 2020-04-08 | 2022-05-17 | 江苏盛海智能科技有限公司 | 一种无人驾驶车辆避障方法及终端 |
CN111766870B (zh) * | 2020-05-29 | 2021-11-05 | 广州极飞科技股份有限公司 | 过渡路径和作业路径的规划方法及相关装置 |
CN111679678B (zh) * | 2020-06-30 | 2022-04-08 | 安徽海博智能科技有限责任公司 | 一种横纵向分离的轨迹规划方法、***及计算机设备 |
US11835958B2 (en) | 2020-07-28 | 2023-12-05 | Huawei Technologies Co., Ltd. | Predictive motion planning system and method |
CN112232558B (zh) * | 2020-10-09 | 2024-02-20 | 中国铁建重工集团股份有限公司 | 一种tbm喷混***作业路径规划方法 |
CN112721914B (zh) * | 2020-12-23 | 2022-04-05 | 同济大学 | 带有监督机制的智能电动汽车漂移入库分段式控制方法 |
CN112829765A (zh) * | 2021-01-23 | 2021-05-25 | 湖南喜宝达信息科技有限公司 | 具备无人驾驶功能的电单车控制方法、***及存储介质 |
CN113050633B (zh) * | 2021-03-12 | 2024-05-31 | 苏州挚途科技有限公司 | 清扫轨迹的确定方法、装置和自动清扫设备 |
CN113465590B (zh) * | 2021-06-29 | 2024-03-15 | 三一专用汽车有限责任公司 | 路径规划方法和装置、自动驾驶方法和装置及作业机械 |
CN113204242B (zh) * | 2021-07-05 | 2021-09-07 | 北京理工大学 | 可重构无人车三段式对接控制方法 |
CN113963027B (zh) * | 2021-10-28 | 2022-09-09 | 广州文远知行科技有限公司 | 不确定性检测模型的训练、不确定性的检测方法及装置 |
CN114056351B (zh) * | 2021-11-26 | 2024-02-02 | 文远苏行(江苏)科技有限公司 | 自动驾驶方法及装置 |
CN114115283B (zh) * | 2021-12-02 | 2023-09-19 | 深圳市证通电子股份有限公司 | 基于改进的对向并行算法的机器人路径规划方法和*** |
CN113879339A (zh) * | 2021-12-07 | 2022-01-04 | 阿里巴巴达摩院(杭州)科技有限公司 | 自动驾驶的决策规划方法、电子设备及计算机存储介质 |
CN114200939B (zh) * | 2021-12-10 | 2024-02-27 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人防碰撞路径规划方法 |
CN116381748B (zh) * | 2023-04-06 | 2023-11-21 | 黑龙江惠达科技股份有限公司 | 农机直线自动驾驶轨迹对比方法及装置 |
CN117622227B (zh) * | 2024-01-24 | 2024-04-16 | 广汽埃安新能源汽车股份有限公司 | 一种车辆换道避障控制方法及装置 |
CN117698769B (zh) * | 2024-02-05 | 2024-04-26 | 上海鉴智其迹科技有限公司 | 自动驾驶的轨迹规划方法、装置、电子设备及存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107168305A (zh) * | 2017-04-01 | 2017-09-15 | 西安交通大学 | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 |
CN107340772A (zh) * | 2017-07-11 | 2017-11-10 | 清华大学 | 一种面向无人驾驶的拟人化参考轨迹规划方法 |
CN107817509A (zh) * | 2017-09-07 | 2018-03-20 | 上海电力学院 | 基于rtk北斗和激光雷达的巡检机器人导航***及方法 |
CN107992050A (zh) * | 2017-12-20 | 2018-05-04 | 广州汽车集团股份有限公司 | 无人驾驶汽车局部路径运动规划方法和装置 |
CN108088456A (zh) * | 2017-12-21 | 2018-05-29 | 北京工业大学 | 一种具有时间一致性的无人驾驶车辆局部路径规划方法 |
CN108501911A (zh) * | 2018-03-09 | 2018-09-07 | 同济大学 | 一种车辆集成式电子液压制动***及稳定性控制方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018173729A (ja) * | 2017-03-31 | 2018-11-08 | パナソニックIpマネジメント株式会社 | 自動運転制御方法およびそれを利用した自動運転制御装置、プログラム |
-
2018
- 2018-10-11 CN CN201811183181.0A patent/CN109557912B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107168305A (zh) * | 2017-04-01 | 2017-09-15 | 西安交通大学 | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 |
CN107340772A (zh) * | 2017-07-11 | 2017-11-10 | 清华大学 | 一种面向无人驾驶的拟人化参考轨迹规划方法 |
CN107817509A (zh) * | 2017-09-07 | 2018-03-20 | 上海电力学院 | 基于rtk北斗和激光雷达的巡检机器人导航***及方法 |
CN107992050A (zh) * | 2017-12-20 | 2018-05-04 | 广州汽车集团股份有限公司 | 无人驾驶汽车局部路径运动规划方法和装置 |
CN108088456A (zh) * | 2017-12-21 | 2018-05-29 | 北京工业大学 | 一种具有时间一致性的无人驾驶车辆局部路径规划方法 |
CN108501911A (zh) * | 2018-03-09 | 2018-09-07 | 同济大学 | 一种车辆集成式电子液压制动***及稳定性控制方法 |
Non-Patent Citations (2)
Title |
---|
Design of Unmanned Park Vehicle Decision-making System Based on Uncertainty Information;Yu, Zhuoping等;《 5th International-Federation-of-Automatic-Control (IFAC) Conference on Engine and Powertrain Control, Simulation and Modeling (E-COSM)》;20180922;第51卷(第31期);第924-929页 * |
一种新的自动驾驶轨迹规划方法;梁广民;《电子科技大学学报》;20170807;第46卷(第4期);第600-606页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109557912A (zh) | 2019-04-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109557912B (zh) | 一种自动驾驶特种作业车辆的决策规划方法 | |
US11231286B2 (en) | Dynamic routing for self-driving vehicles | |
US20230041319A1 (en) | Data processing method and apparatus, device, and storage medium | |
US10379533B2 (en) | System and method for autonomous vehicle fleet routing | |
CN111426326A (zh) | 一种导航方法、装置、设备、***及存储介质 | |
US20210009119A1 (en) | Method and control device for a system for the control of a motor vehicle | |
CN113916246A (zh) | 一种无人驾驶避障路径规划方法和*** | |
US11453410B2 (en) | Reducing processing requirements for vehicle control | |
US11586209B2 (en) | Differential dynamic programming (DDP) based planning architecture for autonomous driving vehicles | |
JP2021054393A (ja) | 車両のuターン経路を決定する方法、装置、デバイスおよび媒体 | |
CN111006667A (zh) | 高速场景下的自动驾驶轨迹生成*** | |
KR20150084143A (ko) | 다양한 주행 환경 기반의 차량 자율 주행 장치 및 그 경로 계획 방법 | |
CN106840169B (zh) | 用于机器人路径规划的改进方法 | |
JP2019530608A (ja) | 物体レベル融合の自律走行車両 | |
WO2019160447A1 (ru) | Способ создания трека пути движения для автономного движения подвижного объекта и способ осуществления автономного движения подвижного объекта по треку пути движения | |
CN109933068A (zh) | 驾驶路径规划方法、装置、设备和存储介质 | |
CN113670305A (zh) | 泊车轨迹生成方法、装置、计算机设备和存储介质 | |
CN111998858B (zh) | 一种基于改进a*算法的无人机航路规划方法 | |
CN114137948A (zh) | 编队车辆行驶方法、装置、计算机设备和存储介质 | |
CN115042820A (zh) | 自动驾驶车辆控制方法、装置、设备及存储介质 | |
CN117885764B (zh) | 车辆的轨迹规划方法、装置、车辆及存储介质 | |
CN115810268A (zh) | 一种车辆避撞方法及装置 | |
AU2021334408B2 (en) | Method and apparatus for coordinating multiple cooperative vehicle trajectories on shared road networks | |
CN112622924B (zh) | 一种驾驶规划方法及装置、车辆 | |
CN116653963B (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 |