CN104590259B - 一种轨迹搜寻方法和系统 - Google Patents

一种轨迹搜寻方法和系统 Download PDF

Info

Publication number
CN104590259B
CN104590259B CN201510005390.6A CN201510005390A CN104590259B CN 104590259 B CN104590259 B CN 104590259B CN 201510005390 A CN201510005390 A CN 201510005390A CN 104590259 B CN104590259 B CN 104590259B
Authority
CN
China
Prior art keywords
track
path
motion
alternative
motor device
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
CN201510005390.6A
Other languages
English (en)
Other versions
CN104590259A (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.)
Foochow Hua Ying Heavy Industry Machinery Co Ltd
Original Assignee
Foochow Hua Ying Heavy Industry Machinery 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 Foochow Hua Ying Heavy Industry Machinery Co Ltd filed Critical Foochow Hua Ying Heavy Industry Machinery Co Ltd
Priority to CN201510005390.6A priority Critical patent/CN104590259B/zh
Publication of CN104590259A publication Critical patent/CN104590259A/zh
Application granted granted Critical
Publication of CN104590259B publication Critical patent/CN104590259B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/10—Path keeping
    • 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
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • 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
    • B60W2555/00—Input parameters relating to exterior conditions, not covered by groups B60W2552/00, B60W2554/00

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开一种轨迹搜寻方法和系统，其中方法包括如下步骤：区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；根据不同的驾驶环境生成不同的运动轨迹。本发明对轨迹搜寻的效果好，使得机动设备的控制能达到较好的效果。

Description

一种轨迹搜寻方法和系统
技术领域
本发明涉及轨迹搜寻技术领域，尤其涉及一种轨迹搜寻方法和系统，具体地可以是一种汽车自动驾驶的轨迹搜寻方法和系统。
背景技术
自动驾驶汽车(Autonomous vehicles或者Self-piloting automobile)又称无人驾驶汽车、电脑驾驶汽车、或轮式移动机器人，是一种通过电脑系统实现无人驾驶的智能汽车。自动驾驶汽车技术的研发，在20世纪也已经有数十年的历史，于21世纪初呈现出接近实用化的趋势，比如，谷歌自动驾驶汽车于2012年5月获得了美国首个自动驾驶车辆许可证，预计于2015年至2017年进入市场销售。
汽车自动驾驶技术包括视频摄像头、雷达传感器以及激光测距器来了解周围的交通状况，并通过一个详尽的地图(通过有人驾驶汽车采集的地图)对前方的道路进行导航。这一切都通过服务器的数据中心来实现，服务器的数据中心能处理汽车收集的有关周围地形的大量信息。就这点而言，自动驾驶汽车相当于服务器数据中心的遥控汽车或者智能汽车。汽车自动驾驶技术物联网技术应用之一。
自动驾驶过程中对汽车到目标点的运动轨迹的预测和搜寻至关重要，直接关系到效率和能耗的问题，与轨迹搜寻相关的专利有如下：
公开时间为：2011年5月18日，公开号为CN102060018A的中国发明专利，公开了《车辆特别是载货车的方向控制方法以及方向控制系统》，其主要车辆根据测得的车道参数预先给定轨迹，方向控制系统针对分别预先给定的车道求得和/或测得至少一个规定的车辙参数作为车辙实际值，并将其与至少一个相应地预先给定的车辙给定值比较，从而在至少一个车辙实际值与配设的车辙给定值有规定的偏差时，改变预定的轨迹，致使车辆的车轮基本上在相应车道的车辙内行驶。
这些专利都是将轨迹局限于汽车当前的状态的采集和判断，没有结合汽车目标点的位置情况，搜寻的效果不理想。
发明内容
为此，需要提供一种轨迹搜寻方案，解决现有汽车轨迹搜寻效果不佳的问题。
为实现上述目的，发明人提供了一种轨迹搜寻方法，包括如下步骤：
区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；
根据不同的驾驶环境生成不同的运动轨迹。
进一步地，所述结构化区域包括具有车道的道路。
进一步地，所述生成运动轨迹具体包括如下步骤：
生成一条平滑的沿车道中线的运动轨迹；
则本方法还包括如下步骤：
调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；
选择最佳的备选轨迹；
输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
进一步地，还包括成本计算步骤：在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
进一步地，所述成本计算时包括如下计算条件：
备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反比；
或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的倒数呈反比；
或者备选轨迹与障碍物相交，则成本无穷大；
或者成本与备选轨迹和车道中线的平均距离成正比；
或者成本与备选轨迹平滑度成反比；
或者成本与车辆完成轨迹所需要的总时间成正比。
进一步地，所述根据不同的驾驶环境生成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运动轨迹。
进一步地，还包括如下步骤：间隔固定时间段或距离后重新生成运动轨迹。
进一步地，当驾驶环境是非结构化区域时，所述生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
进一步地，还包括对运动轨迹进行成本计算步骤：使用点阵多维搜索算法对运动轨迹的成本进行计算。
进一步地，还包括如下步骤：对生成的运动轨迹进行平滑化操作。
以及发明人还提供一种轨迹搜寻系统，包括如下模块：
区域区分模块：用于区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；
轨迹生成模块：根据不同的驾驶环境生成不同的运动轨迹。
进一步地，所述结构化区域包括具有车道的道路。
进一步地，所述轨迹生成模块具体包括如下模块：
中线轨迹生成模块：用于生成一条平滑的沿车道中线的运动轨迹；
则本系统还包括如下模块：
备选轨迹计算模块：用于调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；
备选轨迹选择模块：用于选择最佳的备选轨迹；
控制参数输出模块：用于输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
进一步地，还包括成本计算模块：用于在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
进一步地，所述成本计算模块在成本计算时包括如下计算条件：
备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反比；
或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的倒数呈反比；
或者备选轨迹与障碍物相交，则成本无穷大；
或者成本与备选轨迹和车道中线的平均距离成正比；
或者成本与备选轨迹平滑度成反比；
或者成本与车辆完成轨迹所需要的总时间成正比。
进一步地，所述根据不同的驾驶环境生成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运动轨迹。
进一步地，还包括如下模块：轨迹重生成模块：用于间隔固定时间段或距离后重新生成运动轨迹。
进一步地，当驾驶环境是非结构化区域时，所述轨迹生成模块生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
进一步地，成本计算模块还用于使用点阵多维搜索算法对运动轨迹的成本进行计算。
进一步地，还包括如下轨迹平滑化模块：用于对生成的运动轨迹进行平滑化操作。
区别于现有技术，上述技术方案对轨迹搜寻的效果好，使得机动设备的控制能达到较好的效果。
附图说明
图1为行动目标指令的示意图；
图2为一实施方式的场景示意图；
图3为另一实施方式的场景示意图；
图4为又一实施方式的场景示意图；
图5为再一实施方式的场景示意图；。
具体实施方式
为详细说明技术方案的技术内容、构造特征、所实现目的及效果，以下结合具体实施例并配合附图详予说明。
请参阅图1到图5，本实施例提供一种轨迹搜寻方法，可用于机动设备自动驾驶过程的轨迹搜寻，机动设备包括但不限于民用车辆、军事车辆甚至特种车辆(如火星月球探测车)等，本方法可以运行于机动设备的动作控制模块上。
机动设备上可以安装有多个用于感知机动设备状态的传感器，每当发生新的事件(事件可以由感知模块、定时器或者其它模块产生的信号触发)，机动设备需要马上分析处理这些新的事件，然后搜寻新的运动轨迹。在机动设备自动驾驶过程中，为了实现对机动设备的控制，每个运动轨迹包含有一个或者多个行动目标指令，这些行动目标的具体实现，可以由机动设备的动作控制模块完成，动作控制模块用于控制机动设备动作。
如图1所示，行动目标指令包括了行动目标和行动参数。根据当前驾驶环境的区别，分为结构化区域内的行动目标指令和非结构化区域内的行动目标指令。行动目标指令精确地定义了动作控制模块和行为决策模块之间的界面，让这两个模块之间的分工非常明确。在得到行为决策模块输出的行动目标指令后，如何具体地履行这些指令，是动作控制模块的任务。为了完成这个任务，动作控制模块还须感知模块输出的虚拟世界模型，虚拟世界模型即对当前环境进行虚拟化，使得机动设备可以获取环境状态。
行动目标的内容包含了目标点坐标(称为点B)，同时蕴含了机动设备的当前坐标点(称为点A)。这个目标简单而抽象，而机动设备从点A到点B的可行运动轨迹(以下简称轨迹)却有无穷多种。所以动作控制模块必须设计出一条具体的轨迹，同时找到实现这一条轨迹的控制参数，才能在现实中控制机动设备实现由点A到点B的运动。
很显然，一条简单地连接AB的直线是行不通的。因为一、机动设备在点A或者点B的车头指向很可能不是二、机动设备在点A的速度很可能不是零。如果要强行执行这个直线轨迹的话，则机动设备在点A必须能够瞬间把速度降到零，原地调转车头指向然后再加速前进。这样的机动要求对于绝大部分的机动设备而言明显是不现实的。即便有机动设备能实现这样的机动，这条轨迹在时间和能耗方面的效率也存在很大的改进的空间。
由此可见轨迹的生成需要受到各种因素的制约，这些因素包括：1.机动设备起始点状态，起始点状态包括：位置坐标(x，y)，航向(θ)和速度(v)。2.机动设备目标点状态。3.机动设备自身的驾驶性能，这个因素可以由机动设备模型模拟，以保证生成的轨迹最接近机动设备真实的机动能力。4.行动参数中的最大速度和理想加速度。5.路上的静态/动态障碍物(可以由虚拟世界模型提供)。
本方法具体地包括如下步骤：
首先区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；而后根据不同的驾驶环境生成不同的运动轨迹。结构化区域可以是有规则的区域，如道路，机动设备辆的运动要受到区域结构化的限制，如图1所示，具有上下两个车道，机动设备A的运行要在两个车道中进行。非结构化区域可以是没有规则的区域，如停车场或者操场等，机动设备可以往任意方向或者路径行进，如图2所示。对于不同的区域，对区域进行区分并生成不同的运动轨迹，而后在该运动轨迹上完成机动设备的行进，有利于节省机动设备能耗和减少行进时间。
因为在不同行驶环境下，合理可行的轨迹的形状会有很大的不同，所以动作控制模块用不同的方法区别对待这些环境：
1.结构化的区域。即可行驶车道有明确界定的道路。在这种环境下机动设备的运动受限于车道和道路的形状，因此动作控制模块生成的轨迹和车道在方向和走向上相似(如图2所示)。在机动设备沿道路行驶的情况下，以下两种动作的组合足以满足正常情况下的机动要求：a.沿当前车道行驶；b.换车道行驶。
2.非结构化的区域。例如没有规划车道的停车场。如图3所示，这类环境开放而且没有道路结构，因此机动设备的运动轨迹相对自由很多。在这种环境下，行动目标指令仅仅要求机动设备达到某一位置的同时保持某一朝向，而具体的轨迹灵活性很大。
进一步地，所述生成运动轨迹具体包括如下步骤：
生成一条平滑的沿车道中线的运动轨迹；
则本方法还包括如下步骤：
调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；选择最佳的备选轨迹；输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
因为在沿道路行驶的背景下，行动目标点通常位于某一车道内。这个时候，道路的走向和是否变道决定了合理的轨迹的形状。如果目标是顺着车道行驶，那么合理的运动轨迹应该和道路中线基本平行，如图2机动设备A正前方的轨迹曲线。如果目标是变道行驶，那么合理的运动轨迹会应该一开始和原车道中线基本平行，接着平滑过渡到和目标车道中线基本平行，如图2机动设备A上面车道的曲线。由此可见，道路中线曲线提供了一条很好的参照轨迹，这条轨迹可以作为轨迹搜索的起始点。
搜索轨迹的最终目的，是要找到能实现这么一条轨迹的电传操控控制参数，控制参数用于控制机动设备运动。本实施例中采取的办法是，用一组微分方程来模拟机动设备状态受控制参数变化而变化的过程。同时，将上述中所提到的一些制约因素以约束模型的形式加入这一运动模型。从而用求微分方程组的边界值问题来解决轨迹搜索的问题。
这个微分方程组的两个边界值便是车辆在起始点和目标点的状态矢量(位置(x，y)，航向(θ)和速度(v))。驱动机动设备起始点状态变化至目标点状态的，是电传操纵模块的一系列输入参数的连续函数，这也是微分方程组最终要搜寻的解。
算法的具体步骤为：
1.生成一条平滑的中线轨迹。从起始点起，沿着目标车道的中线，直到目标点，从道路几何地图(虚拟世界模型中包含)中提取一系列道路中线点。然后从这些点生成一条平滑的曲线，即中线轨迹(如图4的中线f)。
2.以中线轨迹为基础，通过一定的数值计算法，一个一个地寻找满足微分方程组的解，即不同的机动设备控制参数，获得一系列逐渐侧向平移的轨迹(如图4中线f周围的轨迹曲线)即是备选轨迹。这种方法能够极大地缩小搜索空间，确保动作控制模块能够以较高的频率输出电传操纵指令。
3.对第2步生成的轨迹进行筛选，过滤掉不可行的轨迹，然后选出最佳轨迹。最佳轨迹所代表的解，即为电传操纵模块的参数函数，可以直接输出至电传操纵模块，实现对机动设备e的控制。
具体地，可以使用以下这一组微分方程模型，来模拟由机动设备电传控制参数的改变，而导致机动设备运动状态的改变，使得机动设备由从起始点状态变化到目标点状态：
x＝[x y θ v…]T  (1)
x · ( x , p ) = f ( x , u ( p , x ) ) - - - ( 2 )
其中，矢量x代表机动设备的各种状态(位置(x，y)，航向(θ)和速度(v))。机动设备状态的微分函数由电传操控参数函数p和机动设备状态函数x决定，这是因为机动设备状态对于特定的电传操控参数的反应受当前状态的影响。这个方程组的解p，即为电传操控的一系列输入参数的连续函数。
在上述(1)(2)这一通用框架下，运动轨迹具有多种多样，如图5所示的多条轨迹曲线，则可以进一步细化状态约束模型和机动设备模型。首先是状态约束模型，所有的轨迹必须满足起始点和目标点的状态约束。状态约束方程C(x,p)定义为目标点边界约束状态(xC)和运动方程(公式(2))的积分之间的差：C(x,p)＝xC-xF(p,x)；其中：
x F ( p , x ) = x I + ∫ 0 t f x · ( x , p ) dt - - - ( 3 )
为运动和控制方程从起始点到某一时间点的积分。而：
xC＝[xC+yC+θC]T
为目标点状态。
轨迹生成算法就是要通过数值运算法，找到一个电传操控参数函数p，使得状态约束方程尽可能地趋近于0。
对于机动设备模型，用来模拟理想化的机动设备操控和现实机动设备操控的区别。在现实中，有几个重要因素，使得实际操控和理想有较大偏差：1.电传操控的控制延迟；2.最小曲率，即机动设备最小转弯半径的限制；以及3.曲率变化速率，即方向盘的旋转速度的限制。机动设备模型的逼真度直接影响到模型生成的电传操控参数函数p在执行过程中，实际产生的轨迹和模型预测的轨迹能否吻合。因此，对于以上几个因素的模拟，动作控制模块用一组参数化的方程式表达。参数的确定根据实际的测量数据。数据的收集由人为驾驶完成。以驾驶过程中电传操控的信号数据为方程的输入数值，而机动设备硬件的实际数据为方程的输出值，可以推算出方程参数。
为了实现对不同轨迹成本的计算，本方法，还包括成本计算步骤：在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
具体的轨迹成本计算包括机动设备和障碍物之间的轨迹计算。在轨迹的每一点上，根据此时车辆的朝向，确定车辆的边界。然后分析车辆附近所有的障碍物，并计算出距离车辆边界最近的障碍物的距离。障碍物距离成本方程分为几种情况：a.在一定的最小安全距离之外，成本和距离呈反比；b.在一定的最小安全距离之内，成本和距离-1呈反比；c.如果静态或者动态的障碍物和轨迹相交，则成本无穷大成本。
还包括：轨迹和车道中线的平均距离。距离中线近的轨迹成本低。反之则高。轨迹平滑度。越平滑的轨迹成本越低。以及车辆完成轨迹所需要的总时间。时间越短成本越低。
最后选出最低成本的轨迹，由电传操控模块执行。如此筛选出的最低成本轨迹有几个优点：1.轨迹上不存在障碍物，2在速度和机动设备动作的激烈程度上达到一定的平衡。
上述中的非结构化区域包括指无明显道路结构，可自由行驶的空间。如某些停车场，操场，和未划定车道的宽阔路段等。非结构化区域和结构化区域连接于若干关口，机动设备一旦进入通过这些关口驶入非结构化区域，则动作控制模块立即切换到非结构化模式。反之，机动设备一旦通过这些关口离开非结构化区域，则动作控制模块马上切换回结构化模式。
非结构化的区域内的运动轨迹生成由行为决策模块输出的指令，有时会以停车场的一个停车位为目标，如图3所示的三角形d，为目标点的位置和朝向。有时会要求从一个节点进入一个开放的区域内，然后由另一个节点离开。在非结构化区域内行驶，因为没有车道和方向的限制，所以不存在一条显而易见的，由目标车道和道路走向决定的轨迹。因此在轨迹的生成上，动作控制模块就需要运用一种不同于结构化区域内轨迹生成的算法，在自由的空间里内避开动态和静态的障碍物，寻找到一条到达目标点的路径。
具体地，当驾驶环境是非结构化区域时，所述生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。在非结构区域搜寻运动轨迹是一个在一定空间内的搜索最佳路径的问题。这个问题的解决经常会使用到A*搜寻算法或称A星算法，或者由其衍生的搜寻算法。A*将二维空间抽象为一幅由节点组成的图，两节点之间若有线段相连，则代表两节点之间可通行。线段同时还包括了两节点之间距离的信息。在搜寻过程中，A*从起始点开始，不断地把最优节点(成本最低的节点)添加到搜索树上，直到搜索树的某一枝碰到终点节点。
A*搜寻算法本身衍生于Dijkstra于1959年公布的搜寻算法，但在应变能力，前瞻性上优于后者。A*和Dijkstra算法都采取了一种贪婪的探索策略搜寻节点空间，但主要区别在于成本函数f(x)的构成。成本函数决定了节点选择的优先顺序，A*的成本函数如下：
f(x)＝g(x)+h(x)
其中，g(x)为已知的，从起始点到节点x的距离。这点A*和Dijkstra算法相似。
而h(x)是A*独有的启发式函数。A*的启发式函数通常计算从节点x到终点的估算距离。利用这个启发，A*经常能“聪明地”避开一些不可行的路径。
同样地，对于非结构化区域的运动路径也要进行成本计算步骤：可以使用点阵多维搜索算法对运动轨迹的成本进行计算。这里使用的点阵多维搜索算法，就是一种基于A*，经过拓展后充分考虑机动设备运动特性的启发式搜寻算法。
首先，点阵多维搜索算法搜寻的是4维的节点图。每个节点代表着机动设备在某一时间点上的状态，包括位置(x,y)，指向θ，速度v，一共四个参数四个维度。
其次，在每一个节点(x,y，θ,v)，毗邻节点包括从当前节点出发，在一段时间内能够到达的所有状态(x′,y′,θ′,v′)。毗邻节点的生成可以使用机动设备模型。通过向机动设备模型里输入一系列控制参数以及当前节点状态(x,y,θ,v)，得出高准确度的新的节点状态。如此生成的节点所组成的路径能够最大程度地保证可被机动设备执行。
点阵地图的分辨率和自动驾驶动作的细致程度成正比，也和搜寻所需要的时间成正比。为了平衡这两个相互矛盾的要求，点阵地图采用多分辨率。
1.在机动设备周围的区域，方向分辨率可使用5.625度，速度分辨率可使用0.05米/秒。这样每个节点的毗邻节点最多有64个不同的方向(360/5.625度)，和400个不同的速度(假定最大速度20米/秒)。机动设备周边的高分辨率让机动设备能够在障碍物多或者车况复杂的情况下，做出精细的转向，前进/后退的机动，尽量避免由于分辨率不足而陷入一种进退两难的状态。
2.在目标点周围，同样采用高分辨率。这样能够让机动设备尽可能精确地到达目标点。例如在停车场停车的时候，精确地保持和左右邻车的车距。
3.在其他的区域，采取较低的分辨率。比如最多有32个不同的方向，和200个不同的速度。这样做的好处是在不影响路径准确性的情况下，显著地减少了搜索时间。
点阵多维搜索算法的成本函数g(x)有以下几个特点：
1.行驶所需要的时间和成本成正比，距离长的行驶时间长，速度快的行驶时间短；2.前进比后退成本低，以鼓励机动设备尽量往前开；3.转向有额外的成本，以模拟方向盘来回转动所需要的时间；4.对于一些靠近不宜接近的区域(道路边缘和人行道交界处，动态障碍物的周围)的节点，成本会提高，从而可以避开这些区域。
启发函数包含两个独立的函数：1.不考虑机动设备的朝向和速度的方向，考虑障碍物，计算到达目标点的距离；2.考虑机动设备的朝向和速度的方向，不考虑障碍物，计算达到目标的具体。
为了减少搜索时间，搜索从目标点出发。在搜索的每一循环，在目前搜索树末端节点的所有毗邻接点中，用成本函数计算出最小成本节点，加入搜索树中。同时避开存在障碍物的节点，并且在情况允许下，尽可能地远离属于危险区域的节点，从而保证行驶的安全。
在搜索找到最佳路径后，还须将最佳路径平滑化。最佳路径是由线段的集合组成。虽然每条线段两端的节点都是由高仿真的机动设备模型推算出的，代表机动设备确实可实现的机动状态，但两个节点之间的车辆状态由一条直线模拟过于粗糙。所以为了改善最佳路径的可行驶性，须要采用上述的轨迹生成算法将这一条最佳路径转化为一条精确的可执行轨迹，即对生成的运动轨迹进行平滑化操作。
根据非结构化区域特点，在运用轨迹生成算法有几个区别：用最佳路径作为轨迹生成的起点，而不是道路中线轨迹；生成的待筛选的轨迹的数量减少一个数量级；生成轨迹的目标点的状态不允许和实际目标点有偏差。
在生成完一条轨迹后，如果在短时间(由速度决定，行驶距离不超过3-5米)内没有新的指令，那么动作控制模块会利用空闲时间重新生成一次轨迹，以保证实际执行时轨迹的偏差的积累在一定距离内能够得到纠正。即可以间隔固定时间段或距离后重新生成运动轨迹。保证运动轨迹的实时性。
同时对于实际应用中，运动轨迹的生成在不同驾驶环境切换时应该保留一定的提前时间量，这样可以保证机动设备行驶的流畅性，在进入一个非结构化区域内之前，如果行为决策模块给的行动目标列表中有目标在非结构化区域之内，则动作控制模块会启动一个新的进程，提早开始计算从不同驾驶环境之间关口到达目标点的轨迹。
以及基于上述方法发明人还提供一种轨迹搜寻系统，包括如下模块：
区域区分模块：用于区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域；
轨迹生成模块：根据不同的驾驶环境生成不同的运动轨迹。
进一步地，所述结构化区域包括具有车道的道路。
进一步地，所述轨迹生成模块具体包括如下模块：
中线轨迹生成模块：用于生成一条平滑的沿车道中线的运动轨迹；
则本系统还包括如下模块：
备选轨迹计算模块：用于调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；
备选轨迹选择模块：用于选择最佳的备选轨迹；
控制参数输出模块：用于输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
进一步地，还包括成本计算模块：用于在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
进一步地，所述成本计算模块在成本计算时包括如下计算条件：
备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反比；
或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的倒数呈反比；
或者备选轨迹与障碍物相交，则成本无穷大；
或者成本与备选轨迹和车道中线的平均距离成正比；
或者成本与备选轨迹平滑度成反比；
或者成本与车辆完成轨迹所需要的总时间成正比。
进一步地，所述根据不同的驾驶环境生成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运动轨迹。
进一步地，还包括如下模块：轨迹重生成模块：用于间隔固定时间段或距离后重新生成运动轨迹。
进一步地，当驾驶环境是非结构化区域时，所述轨迹生成模块生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
进一步地，成本计算模块还用于使用点阵多维搜索算法对运动轨迹的成本进行计算。
进一步地，还包括如下轨迹平滑化模块：用于对生成的运动轨迹进行平滑化操作。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括……”或“包含……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的要素。此外，在本文中，“大于”、“小于”、“超过”等理解为不包括本数；“以上”、“以下”、“以内”等理解为包括本数。
本领域内的技术人员应明白，上述各实施例可提供为方法、装置、或计算机程序产品。这些实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。上述各实施例涉及的方法中的全部或部分步骤可以通过程序来指令相关的硬件来完成，所述的程序可以存储于计算机设备可读取的存储介质中，用于执行上述各实施例方法所述的全部或部分步骤。所述计算机设备，包括但不限于：个人计算机、服务器、通用计算机、专用计算机、网络设备、嵌入式设备、可编程设备、智能移动终端、智能家居设备、穿戴式智能设备、车载智能设备等；所述的存储介质，包括但不限于：RAM、ROM、磁碟、磁带、光盘、闪存、U盘、移动硬盘、存储卡、记忆棒、网络服务器存储、网络云存储等。
上述各实施例是参照根据实施例所述的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到计算机设备的处理器以产生一个机器，使得通过计算机设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机设备以特定方式工作的计算机设备可读存储器中，使得存储在该计算机设备可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机设备上，使得在计算机设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已经对上述各实施例进行了描述，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改，所以以上所述仅为本发明的实施例，并非因此限制本发明的专利保护范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围之内。

Claims (16)

1.一种轨迹搜寻方法，其特征在于，包括如下步骤：
区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域，所述结构化区域包括具有车道的道路；
根据不同的驾驶环境生成不同的运动轨迹，所述生成运动轨迹具体包括：生成一条平滑的沿车道中线的运动轨迹；
调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；
选择最佳的备选轨迹；
输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
2.根据权利要求1所述的轨迹搜寻方法，其特征在于：还包括成本计算步骤：在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
3.根据权利要求2所述的轨迹搜寻方法，其特征在于：所述成本计算时包括如下计算条件：
备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反比；
或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的倒数呈反比；
或者备选轨迹与障碍物相交，则成本无穷大；
或者成本与备选轨迹和车道中线的平均距离成正比；
或者成本与备选轨迹平滑度成反比；
或者成本与车辆完成轨迹所需要的总时间成正比。
4.根据权利要求1所述的轨迹搜寻方法，其特征在于：所述根据不同的驾驶环境生成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运动轨迹。
5.根据权利要求1所述的轨迹搜寻方法，其特征在于，还包括如下步骤：间隔固定时间段或距离后重新生成运动轨迹。
6.根据权利要求1所述的轨迹搜寻方法，其特征在于，当驾驶环境是非结构化区域时，所述生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
7.根据权利要求6所述的轨迹搜寻方法，其特征在于，还包括对运动轨迹进行成本计算步骤：使用点阵多维搜索算法对运动轨迹的成本进行计算。
8.根据权利要求6所述的轨迹搜寻方法，其特征在于，还包括如下步骤：对生成的运动轨迹进行平滑化操作。
9.一种轨迹搜寻系统，其特征在于，包括如下模块：
区域区分模块：用于区分机动设备当前所属的驾驶环境为结构化区域还是非结构化区域，所述结构化区域包括具有车道的道路；
轨迹生成模块：根据不同的驾驶环境生成不同的运动轨迹；
轨迹生成模块包括如下模块：中线轨迹生成模块：用于生成一条平滑的沿车道中线的运动轨迹；
备选轨迹计算模块：用于调整不同的机动设备控制参数，根据不同的机动设备控制参数算出多条接近运动轨迹的备选轨迹；
备选轨迹选择模块：用于选择最佳的备选轨迹；
控制参数输出模块：用于输出最佳的备选轨迹对应的机动设备控制参数实现对机动设备控制。
10.根据权利要求9所述的轨迹搜寻系统，其特征在于：还包括成本计算模块：用于在算出多条备选轨迹时并计算备选轨迹的成本，所述最佳的备选轨迹为成本最低的备选轨迹。
11.根据权利要求10所述的轨迹搜寻系统，其特征在于：所述成本计算模块在成本计算时包括如下计算条件：
备选轨迹与障碍物在一定的最小安全距离之外，成本和备选轨迹与障碍物距离呈反比；
或者备选轨迹与障碍物在一定的最小安全距离之内，成本和备选轨迹与障碍物距离的倒数呈反比；
或者备选轨迹与障碍物相交，则成本无穷大；
或者成本与备选轨迹和车道中线的平均距离成正比；
或者成本与备选轨迹平滑度成反比；
或者成本与车辆完成轨迹所需要的总时间成正比。
12.根据权利要求9所述的轨迹搜寻系统，其特征在于：所述根据不同的驾驶环境生成不同的运动轨迹还包括根据机动设备当前位置、航向、目标点位置或机动设备性能生成运动轨迹。
13.根据权利要求9所述的轨迹搜寻系统，其特征在于，还包括如下模块：轨迹重生成模块：用于间隔固定时间段或距离后重新生成运动轨迹。
14.根据权利要求9所述的轨迹搜寻系统，其特征在于，当驾驶环境是非结构化区域时，所述轨迹生成模块生成运动轨迹包括使用A星算法对运动轨迹进行搜寻并生成运动轨迹。
15.根据权利要求14所述的轨迹搜寻系统，其特征在于，成本计算模块还用于使用点阵多维搜索算法对运动轨迹的成本进行计算。
16.根据权利要求14所述的轨迹搜寻系统，其特征在于，还包括如下轨迹平滑化模块：用于对生成的运动轨迹进行平滑化操作。
CN201510005390.6A 2015-01-07 2015-01-07 一种轨迹搜寻方法和系统 Active CN104590259B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510005390.6A CN104590259B (zh) 2015-01-07 2015-01-07 一种轨迹搜寻方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510005390.6A CN104590259B (zh) 2015-01-07 2015-01-07 一种轨迹搜寻方法和系统

Publications (2)

Publication Number Publication Date
CN104590259A CN104590259A (zh) 2015-05-06
CN104590259B true CN104590259B (zh) 2015-08-26

Family

ID=53116416

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510005390.6A Active CN104590259B (zh) 2015-01-07 2015-01-07 一种轨迹搜寻方法和系统

Country Status (1)

Country Link
CN (1) CN104590259B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105487537B (zh) * 2015-11-06 2018-01-26 福州华鹰重工机械有限公司 一种车辆运动规划方法
US9645577B1 (en) * 2016-03-23 2017-05-09 nuTonomy Inc. Facilitating vehicle driving and self-driving
CN109643496B (zh) * 2016-09-21 2021-08-20 苹果公司 车辆控制系统
CN115343947A (zh) * 2016-09-23 2022-11-15 苹果公司 自主车辆的运动控制决策
JP6714714B2 (ja) * 2016-10-18 2020-06-24 本田技研工業株式会社 車両制御装置
US10146224B2 (en) * 2016-11-09 2018-12-04 GM Global Technology Operations LLC Processor-implemented systems and methods for automated driving
US10345812B2 (en) * 2017-01-10 2019-07-09 GM Global Technology Operations LLC Methods and apparatus for optimizing a trajectory for an autonomous vehicle
CN110015290B (zh) * 2018-01-08 2020-12-01 湖南中车时代电动汽车股份有限公司 一种用于智能驾驶系统的控制方法
US20180164822A1 (en) * 2018-02-09 2018-06-14 GM Global Technology Operations LLC Systems and methods for autonomous vehicle motion planning
US10739768B2 (en) * 2018-08-08 2020-08-11 Toyota Motor Engineering & Manufacturing North America, Inc. Smoothed and regularized Fischer-Burmeister solver for embedded real-time constrained optimal control problems in autonomous systems
CN109712421B (zh) * 2019-02-22 2021-06-04 百度在线网络技术（北京）有限公司 自动驾驶车辆的速度规划方法、装置和存储介质
CN112683290A (zh) * 2020-12-29 2021-04-20 的卢技术有限公司 一种车辆轨迹规划方法、电子设备及计算机可读存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102060018A (zh) * 2009-11-18 2011-05-18 德国曼商用车辆股份公司 车辆特别是载货车的方向控制方法以及方向控制系统
CN102358287A (zh) * 2011-09-05 2012-02-22 北京航空航天大学 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法
CN104133473A (zh) * 2008-10-24 2014-11-05 格瑞股份公司 自主驾驶车辆的控制方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7979173B2 (en) * 1997-10-22 2011-07-12 Intelligent Technologies International, Inc. Autonomous vehicle travel control systems and methods
EP2537735B1 (en) * 2008-09-30 2013-12-11 Nissan Motor Co., Ltd Driving-operation assisting device and driving-operation assisting method
US8473144B1 (en) * 2012-10-30 2013-06-25 Google Inc. Controlling vehicle lateral lane positioning

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104133473A (zh) * 2008-10-24 2014-11-05 格瑞股份公司 自主驾驶车辆的控制方法
CN102060018A (zh) * 2009-11-18 2011-05-18 德国曼商用车辆股份公司 车辆特别是载货车的方向控制方法以及方向控制系统
CN102358287A (zh) * 2011-09-05 2012-02-22 北京航空航天大学 一种用于车辆自动驾驶机器人的轨迹跟踪控制方法

Also Published As

Publication number Publication date
CN104590259A (zh) 2015-05-06

Similar Documents

Publication Publication Date Title
CN104590259B (zh) 一种轨迹搜寻方法和系统
Sun et al. Courteous autonomous cars
CN105335797B (zh) 一种基于全联立动态优化框架的自主泊车轨迹优化方法
CN102999646B (zh) 一种微观交通仿真中车辆跟驰换道方法及系统
CN112590775B (zh) 一种自动泊车方法、装置、车辆及存储介质
CN106610664A (zh) 运动避障装置及控制方法
CN113247023B (zh) 一种行驶规划方法、装置、计算机设备及存储介质
CN113682312A (zh) 一种融合深度强化学习的自主换道方法及系统
CN114372375A (zh) 一种越野车安全行驶区域求解方法及系统
Jiang et al. Integrated decision and control at multi-lane intersections with mixed traffic flow
CN114791734A (zh) 一种基于语义信息串联的履带车辆的控制方法及系统
CN113120003B (zh) 无人驾驶车辆运动行为决策方法
Wang et al. Research on path planning for autonomous vehicle based on Frenet system
CN114167860B (zh) 一种自动驾驶最优轨迹生成方法及装置
Du et al. A Dynamic collaborative planning method for multi-vehicles in the autonomous driving platform of the DeepRacer
CN113674529A (zh) 一种自主超车方法及系统
CN114771509A (zh) 辅助驾驶多环境智慧泊车路径规划控制方法
CN114781064A (zh) 一种基于社会力的车辆行为建模方法
Wang et al. A Risk-field Based Motion Planning Method for Multi-vehicle Conflict Scenario
Li et al. An efficient sampling-based hybrid a* algorithm for intelligent vehicles
Han et al. Moving Horizon path planning for intelligent vehicle oriented to dynamic obstacle avoidance
Rosero et al. CNN-Planner: A neural path planner based on sensor fusion in the bird's eye view representation space for mapless autonomous driving
Diachuk et al. Motion Planning for Autonomous Vehicles Based on Sequential Optimization. Vehicles 2022, 4, 344–374
CN117826825B (zh) 基于人工势场算法的无人矿卡局部路径规划方法及系统
Fang et al. Longitudinal motion control of intelligent vehicle based on two hierarchies optimal method

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C14 Grant of patent or utility model
GR01 Patent grant