CN117406756B - 一种运动轨迹参数的确定方法、装置、设备和存储介质 - Google Patents
一种运动轨迹参数的确定方法、装置、设备和存储介质 Download PDFInfo
- Publication number
- CN117406756B CN117406756B CN202311661791.8A CN202311661791A CN117406756B CN 117406756 B CN117406756 B CN 117406756B CN 202311661791 A CN202311661791 A CN 202311661791A CN 117406756 B CN117406756 B CN 117406756B
- Authority
- CN
- China
- Prior art keywords
- motion
- path
- point
- distance
- coordinates
- 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 109
- 238000005070 sampling Methods 0.000 claims abstract description 117
- 238000012549 training Methods 0.000 claims abstract description 101
- 230000008878 coupling Effects 0.000 claims abstract description 29
- 238000010168 coupling process Methods 0.000 claims abstract description 29
- 238000005859 coupling reaction Methods 0.000 claims abstract description 29
- 230000008859 change Effects 0.000 claims abstract description 20
- 230000006870 function Effects 0.000 claims description 141
- 230000001133 acceleration Effects 0.000 claims description 85
- 238000004364 calculation method Methods 0.000 claims description 32
- 230000008569 process Effects 0.000 claims description 23
- 238000010276 construction Methods 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 11
- 238000004458 analytical method Methods 0.000 claims description 5
- 230000009471 action Effects 0.000 description 30
- 238000005457 optimization Methods 0.000 description 20
- 230000002787 reinforcement Effects 0.000 description 14
- 238000006243 chemical reaction Methods 0.000 description 12
- 230000006399 behavior Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000013459 approach Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 5
- 230000003993 interaction Effects 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 230000006978 adaptation Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000012512 characterization method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003116 impacting effect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000005055 memory storage Effects 0.000 description 1
- 238000003062 neural network model Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 239000004576 sand Substances 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
Abstract
本发明涉及自动驾驶技术领域,公开了一种运动轨迹参数的确定方法、装置、设备和存储介质,获取路径对应的俯视图像和运动规划参数;运动规划参数包括路径信息和运动信息;对路径信息进行曲线拟合采样,以得到路径点序列;对运动信息进行曲线拟合采样,以得到运动点序列;按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹,实现了自适应变化的车辆运动轨迹的规划。基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数,增强了运动轨迹参数学习的稳定性和安全性。
Description
技术领域
本发明涉及自动驾驶技术领域,特别是涉及一种运动轨迹参数的确定方法、装置、设备和存储介质。
背景技术
自动驾驶技术在提高各种驾驶场景下的车辆安全性和机动性方面具有巨大潜力。驾驶策略可以包括车辆行驶过程对应的起止位置、航向角、驾驶速度和加速度等。驾驶系统按照驾驶策略控制车辆的自动驾驶。然而现实世界的驾驶场景通常是长尾分布式,对于出现概率较小的风险案例,驾驶系统会由于数据不足而缺乏对环境的了解,无法及时作出合理的响应。
目前改进驾驶策略的方案是针对特定情况,手动调整驾驶策略或添加额外规则。然而手动调节可能与现有的驾驶功能相矛盾,不仅会使得驾驶性能下降,还会因处理新出现策略而带来巨大工程负担。以强化学习(Reinforcement Learning,RL)为代表的数据驱动方法,提供了一种从收集的数据中学习并不断更新驾驶策略的技术方案。
由于真实自动驾驶过程并不是特定个别场景的简单切换,驾驶策略可能会被要求同时处理多种未见场景,RL将需要越来越多的数据和长时间的训练来覆盖大多数场景,以确保足够良好的驾驶性能。
然而RL所采取的试错-探索机制导致其训练过程不平稳,模型难以收敛至相对稳定的状态,此外,RL本质仍是通过神经网络模型拟合状态到动作间的映射,受神经网络内在不确定性影响,驾驶策略的动作输出具有概率随机性,因此即便策略完成足够规模的训练,依然有小概率可能会选择不安全驾驶动作,无法保证驾驶安全性。
可见,如何提升自动驾驶的稳定性和安全性,是本领域技术人员需要解决的问题。
发明内容
本发明实施例的目的是提供一种运动轨迹参数的确定方法、装置、设备和存储介质,可以解决自动驾驶稳定性和安全性差的问题。
为解决上述技术问题,本发明实施例提供一种运动轨迹参数的确定方法,包括:
获取路径对应的俯视图像和运动规划参数;其中,所述运动规划参数包括路径信息和运动信息;
对所述路径信息进行曲线拟合采样,以得到路径点序列;对所述运动信息进行曲线拟合采样,以得到运动点序列;
按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹;
基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型;
利用所述轨迹识别模型对获取的新俯视图像进行分析,以得到与所述新俯视图像匹配的运动轨迹参数。
一方面,所述路径点序列包括多个路径点坐标及其匹配的航向角;
所述对所述路径信息进行曲线拟合采样,以得到路径点序列包括:
根据所述路径信息包含的路径起止参数,将所述路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线;
按照纵向坐标等间隔采样方式从所述路径曲线中取点,以得到多个路径点坐标;
基于路径点坐标和航向角的对应关系,确定出各所述路径点坐标匹配的航向角。
一方面,所述根据所述路径信息包含的路径起止参数,将所述路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线包括:
对所述路径信息进行多项式曲线拟合,以得到初始路径曲线;
根据所述路径信息包含的路径起止参数,构造路径约束条件;其中,所述路径起止参数包括路径起始点坐标和路径起始点航向角,以及路径终止点坐标和路径终止点航向角;
基于所述路径约束条件,确定出所述初始路径曲线的路径曲线系数,以得到最终的路径曲线。
一方面,所述按照纵向坐标等间隔采样方式从所述路径曲线中取点,以得到多个路径点坐标包括:
根据所述路径信息包含的路径起始点纵向坐标和路径终止点纵向坐标、以及设定的窗口时间,确定出路径采样间隔;
根据所述路径起始点纵向坐标以及所述路径采样间隔,确定出各路径点纵向坐标;
从所述路径曲线中选取与各所述路径点纵向坐标匹配的路径点横向坐标。
一方面,所述基于路径点坐标和航向角的对应关系,确定出各所述路径点坐标匹配的航向角包括:
调用航向角计算公式对各所述路径点坐标进行处理,以得到各所述路径点坐标匹配的航向角;所述航向角计算公式为:
;
其中,x i 表示第i个路径点的路径点横向坐标,x i-1表示第i-1个路径点的路径点横向坐标,y i 表示第i个路径点的路径点纵向坐标,y i-1表示第i-1个路径点的路径点纵向坐标,表示第i个路径点的航向角。
一方面,所述运动点序列包括各运动点对应的速度和加速度;
所述对所述运动信息进行曲线拟合采样,以得到运动点序列包括:
根据所述运动信息包含的运动起止参数,将所述运动信息转换为记录速度和时间变化关系的运动曲线;
按照时间等间隔采样方式从所述运动曲线中取点,以得到多个运动点各自对应的速度;
基于速度和加速度的对应关系,确定出各所述速度匹配的加速度。
一方面,所述根据所述运动信息包含的运动起止参数,将所述运动信息转换为记录速度和时间变化关系的运动曲线包括:
对所述运动信息进行多项式曲线拟合,以得到初始运动曲线;
根据所述运动信息包含的运动起止参数,构造运动约束条件;其中,所述运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;
基于所述运动约束条件,确定出所述初始运动曲线的运动曲线系数,以得到最终的运动曲线。
一方面,所述按照时间等间隔采样方式从所述运动曲线中取点,以得到多个运动点各自对应的速度包括:
根据运动点总数与路径点总数的比例关系、以及设定的窗口时间,确定出采样时间;
按照所述采样时间对所述运动曲线进行等间隔采样,以得到多个运动点各自对应的速度。
一方面,所述基于速度和加速度的对应关系,确定出各所述速度匹配的加速度包括:
根据所述采样时间以及各所述运动点各自对应的速度,确定出各所述速度匹配的加速度。
一方面,所述按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹包括:
依据采样时间对所述运动曲线进行积分运算,以得到第一距离序列;
基于多个所述路径点坐标,构建第二距离序列;
根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点;
根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
一方面,所述根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点包括:
根据距离最小原则,从所述第二距离序列中选取与第一距离匹配的第二距离;其中,第一距离为第一运动点对应的行驶距离,第一运动点为所有所述运动点中的任意一个运动点;
将所述第二距离所对应的前后两个路径点作为与所述第一运动点匹配的路径点。
一方面,所述运动轨迹包括各所述运动点各自对应的运动轨迹参数;
所述根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹包括:
基于所述第一距离与所述第二距离的偏差,确定出距离权重;
根据所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与所述第一运动点匹配的路径点坐标和航向角;
将所述第一运动点对应的速度和加速值、以及与所述第一运动点匹配的路径点坐标和航向角作为所述第一运动点的运动轨迹参数。
一方面,所述基于所述第一距离与所述第二距离的偏差,确定出距离权重包括:
将所述第二距离与所述第一距离的差值作为所述第二距离匹配的路径点索引;
调用权重计算公式对所述第一距离以及所述路径点索引所对应的行驶距离进行处理,以得到距离权重;所述权重计算公式为:
;
其中,dist(j)表示第j个运动点对应的第一距离,ind表示所述第二距离匹配的路径点索引,表示所述路径点索引对应的行驶距离,/>表示与所述路径点索引相邻的前一个路径点索引对应的行驶距离。
一方面,所述根据所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与所述第一运动点匹配的路径点坐标和航向角包括:
调用路径点位姿计算公式,对所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角进行处理,以得到与所述第一运动点匹配的路径点坐标和航向角;所述路径点位姿计算公式为:
;
其中,表示与所述第一运动点匹配的路径点横向坐标,/>表示与所述第一运动点匹配的路径点纵向坐标,/>表示与所述第一运动点匹配的航向角,x idx-1表示所述第二距离所对应的前一个路径点的路径点横向坐标,x idx 表示所述第二距离所对应的后一个路径点的路径点横向坐标,y idx-1表示所述第二距离所对应的前一个路径点的路径点纵向坐标,y idx 表示所述第二距离所对应的后一个路径点的路径点纵向坐标,/>表示所述第二距离所对应的前一个路径点的航向角,/>表示所述第二距离所对应的后一个路径点的航向角,∈表示距离权重。
一方面,所述基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型包括:
基于所述俯视图像、所述运动规划参数以及所述运动轨迹对应的动作奖励,构建训练样本;
基于所述运动轨迹、所述俯视图像以及所述运动规划参数,构建价值函数;
根据所述价值函数与所述安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数;
利用所述训练样本对所述初始轨迹识别模型的参数进行更新,以得到轨迹识别模型。
一方面,所述利用所述训练样本对所述初始轨迹识别模型的参数进行更新,以得到轨迹识别模型包括:
从所述训练样本中随机采样单次训练数据;
利用所述单次训练数据对所述初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型;
判断迭代次数是否达到迭代上限值;
在所述迭代次数未达到迭代上限值的情况下,返回所述从所述训练样本中随机采样单次训练数据的步骤;
在所述迭代次数达到迭代上限值的情况下,结束对所述初始轨迹识别模型的训练,并将最新的初始轨迹识别模型作为轨迹识别模型。
一方面,所述根据所述价值函数与所述安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数包括:
基于拉格朗日乘子、所述成本函数以及所述价值函数对应的运动轨迹参数,构建拉格朗日函数。
一方面,所述利用所述单次训练数据对所述初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型包括:
按照固定所述拉格朗日乘子、固定所述成本函数以及最大化拉格朗日函数的方式,对所述拉格朗日函数进行训练,以得到第一拉格朗日函数;
按照固定所述价值函数对应的运动轨迹参数、固定所述成本函数以及最小化拉格朗日函数的方式,对所述第一拉格朗日函数进行训练,以得到第二拉格朗日函数;
按照固定所述价值函数对应的运动轨迹参数、固定所述拉格朗日乘子以及最大化拉格朗日函数的方式,对所述第二拉格朗日函数进行训练,以得到训练后的初始轨迹识别模型。
一方面,获取路径对应的运动规划参数包括:
获取车辆在起始点位置对应的路径起始点坐标、路径起始点航向角、路径起始点对应的速度和加速度;
将车辆在终止点处的车道中心线的横向坐标作为路径终止点横向坐标;
根据车辆最大速度、设定的最大窗口时间以及所述路径起始点坐标包含的路径起始点纵向坐标,确定出路径终止点纵向坐标;
根据车辆最大航向角限制条件,确定出路径终止点航向角;
基于所述路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
本发明实施例还提供了一种运动轨迹参数的确定装置,包括获取单元、第一采样单元、第二采样单元、耦合单元、训练单元和分析单元;
所述获取单元,用于获取路径对应的俯视图像和运动规划参数;其中,所述运动规划参数包括路径信息和运动信息;
所述第一采样单元,用于对所述路径信息进行曲线拟合采样,以得到路径点序列;
所述第二采样单元,用于对所述运动信息进行曲线拟合采样,以得到运动点序列;
所述耦合单元,用于按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹;
所述训练单元,用于基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型;
所述分析单元,用于利用所述轨迹识别模型对获取的新俯视图像进行分析,以得到与所述新俯视图像匹配的运动轨迹参数。
一方面,所述路径点序列包括多个路径点坐标及其匹配的航向角;
所述第一采样单元包括第一转换子单元、第一取点子单元和第一确定子单元;
所述第一转换子单元,用于根据所述路径信息包含的路径起止参数,将所述路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线;
所述第一取点子单元,用于按照纵向坐标等间隔采样方式从所述路径曲线中取点,以得到多个路径点坐标;
所述第一确定子单元,用于基于路径点坐标和航向角的对应关系,确定出各所述路径点坐标匹配的航向角。
一方面,所述第一转换子单元用于对所述路径信息进行多项式曲线拟合,以得到初始路径曲线;根据所述路径信息包含的路径起止参数,构造路径约束条件;其中,所述路径起止参数包括路径起始点坐标和路径起始点航向角,以及路径终止点坐标和路径终止点航向角;基于所述路径约束条件,确定出所述初始路径曲线的路径曲线系数,以得到最终的路径曲线。
一方面,所述第一取点子单元用于根据所述路径信息包含的路径起始点纵向坐标和路径终止点纵向坐标、以及设定的窗口时间,确定出路径采样间隔;根据所述路径起始点纵向坐标以及所述路径采样间隔,确定出各路径点纵向坐标;从所述路径曲线中选取与各所述路径点纵向坐标匹配的路径点横向坐标。
一方面,所述第一确定子单元用于调用航向角计算公式对各所述路径点坐标进行处理,以得到各所述路径点坐标匹配的航向角;所述航向角计算公式为:
;
其中,x i 表示第i个路径点的路径点横向坐标,x i-1表示第i-1个路径点的路径点横向坐标,y i 表示第i个路径点的路径点纵向坐标,y i-1表示第i-1个路径点的路径点纵向坐标,表示第i个路径点的航向角。
一方面,所述运动点序列包括各运动点对应的速度和加速度;
所述第二采样单元包括第二转换子单元、第二取点子单元和第二确定子单元;
所述第二转换子单元,用于根据所述运动信息包含的运动起止参数,将所述运动信息转换为记录速度和时间变化关系的运动曲线;
所述第二取点子单元,用于按照时间等间隔采样方式从所述运动曲线中取点,以得到多个运动点各自对应的速度;
所述第二确定子单元,用于基于速度和加速度的对应关系,确定出各所述速度匹配的加速度。
一方面,所述第二转换子单元用于对所述运动信息进行多项式曲线拟合,以得到初始运动曲线;根据所述运动信息包含的运动起止参数,构造运动约束条件;其中,所述运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;基于所述运动约束条件,确定出所述初始运动曲线的运动曲线系数,以得到最终的运动曲线。
一方面,所述第二取点子单元用于根据运动点总数与路径点总数的比例关系、以及设定的窗口时间,确定出采样时间;按照所述采样时间对所述运动曲线进行等间隔采样,以得到多个运动点各自对应的速度。
一方面,所述第二确定子单元用于根据所述采样时间以及各所述运动点各自对应的速度,确定出各所述速度匹配的加速度。
一方面,所述耦合单元包括运算子单元、距离序列构建子单元、匹配子单元和轨迹确定子单元;
所述运算子单元,用于依据采样时间对所述运动曲线进行积分运算,以得到第一距离序列;
所述距离序列构建子单元,用于基于多个所述路径点坐标,构建第二距离序列;
所述匹配子单元,用于根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点;
所述轨迹确定子单元,用于根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
一方面,所述匹配子单元用于根据距离最小原则,从所述第二距离序列中选取与第一距离匹配的第二距离;其中,第一距离为第一运动点对应的行驶距离,第一运动点为所有所述运动点中的任意一个运动点;将所述第二距离所对应的前后两个路径点作为与所述第一运动点匹配的路径点。
一方面,所述运动轨迹包括各所述运动点各自对应的运动轨迹参数;
所述轨迹确定子单元用于基于所述第一距离与所述第二距离的偏差,确定出距离权重;根据所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与所述第一运动点匹配的路径点坐标和航向角;将所述第一运动点对应的速度和加速值、以及与所述第一运动点匹配的路径点坐标和航向角作为所述第一运动点的运动轨迹参数。
一方面,所述轨迹确定子单元用于将所述第二距离与所述第一距离的差值作为所述第二距离匹配的路径点索引;
调用权重计算公式对所述第一距离以及所述路径点索引所对应的行驶距离进行处理,以得到距离权重;所述权重计算公式为:
;
其中,dist(j)表示第j个运动点对应的第一距离,ind表示所述第二距离匹配的路径点索引,表示所述路径点索引对应的行驶距离,/>表示与所述路径点索引相邻的前一个路径点索引对应的行驶距离。
一方面,所述轨迹确定子单元用于调用路径点位姿计算公式,对所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角进行处理,以得到与所述第一运动点匹配的路径点坐标和航向角;所述路径点位姿计算公式为:
;
其中,表示与所述第一运动点匹配的路径点横向坐标,/>表示与所述第一运动点匹配的路径点纵向坐标,/>表示与所述第一运动点匹配的航向角,x idx-1表示所述第二距离所对应的前一个路径点的路径点横向坐标,x idx 表示所述第二距离所对应的后一个路径点的路径点横向坐标,y idx-1表示所述第二距离所对应的前一个路径点的路径点纵向坐标,y idx 表示所述第二距离所对应的后一个路径点的路径点纵向坐标,/>表示所述第二距离所对应的前一个路径点的航向角,/>表示所述第二距离所对应的后一个路径点的航向角,∈表示距离权重。
一方面,所述训练单元包括样本构建子单元、价值函数构建子单元、目标函数构建子单元和更新子单元;
所述样本构建子单元,用于基于所述俯视图像、所述运动规划参数以及所述运动轨迹对应的动作奖励,构建训练样本;
所述价值函数构建子单元,用于基于所述运动轨迹、所述俯视图像以及所述运动规划参数,构建价值函数;
所述目标函数构建子单元,用于根据所述价值函数与所述安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数;
所述更新子单元,用于利用所述训练样本对所述初始轨迹识别模型的参数进行更新,以得到轨迹识别模型。
一方面,所述更新子单元用于从所述训练样本中随机采样单次训练数据;利用所述单次训练数据对所述初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型;判断迭代次数是否达到迭代上限值;在所述迭代次数未达到迭代上限值的情况下,返回所述从所述训练样本中随机采样单次训练数据的步骤;在所述迭代次数达到迭代上限值的情况下,结束对所述初始轨迹识别模型的训练,并将最新的初始轨迹识别模型作为轨迹识别模型。
一方面,所述目标函数构建子单元用于基于拉格朗日乘子、所述成本函数以及所述价值函数对应的运动轨迹参数,构建拉格朗日函数。
一方面,所述更新子单元用于按照固定所述拉格朗日乘子、固定所述成本函数以及最大化拉格朗日函数的方式,对所述拉格朗日函数进行训练,以得到第一拉格朗日函数;按照固定所述价值函数对应的运动轨迹参数、固定所述成本函数以及最小化拉格朗日函数的方式,对所述第一拉格朗日函数进行训练,以得到第二拉格朗日函数;按照固定所述价值函数对应的运动轨迹参数、固定所述拉格朗日乘子以及最大化拉格朗日函数的方式,对所述第二拉格朗日函数进行训练,以得到训练后的初始轨迹识别模型。
一方面,所述获取单元用于获取车辆在起始点位置对应的路径起始点坐标、路径起始点航向角、路径起始点对应的速度和加速度;将车辆在终止点处的车道中心线的横向坐标作为路径终止点横向坐标;根据车辆最大速度、设定的最大窗口时间以及所述路径起始点坐标包含的路径起始点纵向坐标,确定出路径终止点纵向坐标;根据车辆最大航向角限制条件,确定出路径终止点航向角;基于所述路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
本发明实施例还提供了一种运动轨迹参数的确定设备,包括:
存储器,用于存储计算机程序;
处理器,用于执行所述计算机程序以实现如上述运动轨迹参数的确定方法的步骤。
本发明实施例还提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如上述运动轨迹参数的确定方法的步骤。
由上述技术方案可以看出,获取路径对应的俯视图像和运动规划参数;其中,运动规划参数包括路径信息和运动信息;对路径信息进行曲线拟合采样,以得到路径点序列;对运动信息进行曲线拟合采样,以得到运动点序列;按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹;基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。本发明的有益效果在于,为提升运动轨迹参数的可行性,采用驾驶环境信息作为驾驶策略状态输入表示形式,并设计基于运动规划参数的驾驶策略动作表示,将车辆运动规划问题转化为曲线的搜索寻优,从而基于驾驶策略动作输出运动轨迹,实现了自适应变化的车辆运动轨迹的规划,确保了车辆行驶的平稳性和安全性。同时引入成本约束条件对运动轨迹参数优化目标进行自适应安全性约束,增强了运动轨迹参数学习的稳定性和安全性,实现了驾驶安全性和效率间的良好平衡。
附图说明
为了更清楚地说明本发明实施例,下面将对实施例中所需要使用的附图做简单的介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的一种运动轨迹参数的确定方法的流程图;
图2为本发明实施例提供的一种获取路径点序列的方法的流程图;
图3为本发明实施例提供的一种获取运动点序列的方法的流程图;
图4为本发明实施例提供的一种对路径点序列和运动点序列进行耦合得到运动轨迹的方法的流程图;
图5为本发明实施例提过的一种训练轨迹识别模型的方法的流程图;
图6为本发明实施例提供的一种自适应安全约束的运动轨迹参数学习方法的示意图;
图7为本发明实施例提供的一种运动轨迹参数的确定装置的结构示意图;
图8为本发明实施例提供的一种运动轨迹参数的确定设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下,所获得的所有其他实施例,都属于本发明保护范围。
本发明的说明书和权利要求书及上述附图中的术语“包括”和“具有”,以及与“包括”和“具有”相关的任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元,而是可包括没有列出的步骤或单元。
为了使本技术领域的人员更好地理解本发明方案,下面结合附图和具体实施方式对本发明作进一步的详细说明。
大多数基于强化学习的自动驾驶研究工作都集中在学习速度、样本复杂度或计算复杂度上,目前仍难以从根本上解决驾驶策略性能稳定性、安全性的问题。因此,强化学习的不稳定性质阻碍了其应用于自动驾驶汽车等关键任务的现实世界应用。
目前有两种主要的方法来保证基于强化学习的驾驶策略性能稳定性和安全性。一种方法是专家策略启发方法,首先是模仿一个专家策略,然后在此基础上进行学习以获得更好的性能。专家策略模仿可以使用监督学习、逆向强化学习或添加专家策略启发奖励等方法。然后,强化学习将继续优化该策略以获得更好的性能。理想情况下,模仿的策略将具有与专家驾驶策略相同的性能,而强化学习将进一步改善这些策略。然而,这些方法需要大量的训练数据来达到理想的性能,否则无法保证最终的驾驶策略在面临长尾情况时是比专家策略更好。
另一种方法是将动作约束在安全范围内。一种直接的处理方式是通过基于规则的安全措施直接调整自动驾驶系统的动作。但是,这样改进后的驾驶策略可能过于保守,在复杂场景下可能无法产生可行的解决方案。对于自动驾驶汽车等智能系统来说,在保持安全性的同时优先考虑效率也很重要。例如,过于保守的驾驶策略可能会导致自动驾驶车辆长时间静止以避免碰撞,从而对交通效率产生负面影响。为了实现安全性和效率之间的平衡,一些方法通过设计以安全为导向的奖励函数来校正动作,并在策略导致危险结果时引入惩罚项。这些方法仍然需要大量的训练数据或人为设计复杂的安全措施,无法保证自动驾驶车辆所采取的策略性能始终是最好的。
提高RL驾驶策略稳定性的一个有效方法是选择或设计恰当的动作空间,大多数现有方法都直接学习车辆的控制空间,例如一个时间步长的转向和加速命令。然而,在缺少密集导航点引导路线的情况下,仅仅依靠一系列独立不一致的单步控制信号难以实现典型的驾驶操作,无法为驾驶策略学习提供有效的奖励信号,导致车辆在探索控制空间时会频繁修正航向角和加速度,出现不规则移动,很难学习到复杂驾驶行为,期间频繁失败的控制动作也将无法收集到足够的奖励信号以供驾驶策略改进。
一些研究通过将传统运动规划方法与RL驾驶策略相结合,以提升车辆驾驶稳定性和安全性。通过概率图方法(Probabilistic Roadmap Method,PRM)生成稀疏导航路径点,然后采用RL策略跟踪导航点,输出车辆控制动作,然而依旧没解决控制动作不连贯的问题,所生成的轨迹容易出现振荡,无法满足车辆平稳驾驶需求。
故此,本发明实施例提供了一种运动轨迹参数的确定方法、装置、设备和存储介质,获取路径对应的俯视图像和运动规划参数;对运动规划参数包含的路径信息进行曲线拟合采样,以得到路径点序列;对运动规划参数包含的运动信息进行曲线拟合采样,以得到运动点序列;按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹;基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。采用驾驶环境信息作为驾驶策略状态输入表示形式,并设计基于运动规划参数的驾驶策略动作表示,将车辆运动规划问题转化为曲线的搜索寻优,从而基于驾驶策略动作输出运动轨迹,实现了自适应变化的车辆运动轨迹的规划,确保了车辆行驶的平稳性和安全性。同时引入成本约束条件对运动轨迹参数优化目标进行自适应安全性约束,增强了运动轨迹参数学习的稳定性和安全性,实现了驾驶安全性和效率间的良好平衡。
接下来,详细介绍本发明实施例所提供的一种运动轨迹参数的确定方法。图1为本发明实施例提供的一种运动轨迹参数的确定方法的流程图,该方法包括:
S101:获取路径对应的俯视图像和运动规划参数。
其中,运动规划参数包括路径信息和运动信息。路径信息可以包括路径起止参数。路径起止参数可以分为路径起始点参数和路径终止点参数。路径起始点参数可以包括路径起始点坐标和路径起始点航向角,路径终止点参数可以包括路径终止点坐标和路径终止点航向角。
俯视图像涵盖了与车辆所处驾驶环境相关的信息,可以包括车辆自身和周围目标的时空信息以及道路几何和导航信息。在实际应用中,俯视图像可以采用以视觉为中心的俯视图像(Birds-eye view,BEV)。
在本发明实施例中,可以将驾驶环境信息作为驾驶策略的状态输入。为了便于描述,可以采用s表示驾驶策略的状态输入。可以将运动规划参数作为驾驶策略的动作输出,可以采用a表示驾驶策略的动作输出。
在实际应用中,可以获取车辆在起始点位置对应的路径起始点坐标、路径起始点航向角、路径起始点对应的速度和加速度。
为了便于描述,可以采用(x s ,y s )表示路径起始点坐标,采用表示路径起始点航向角,采用v s 表示路径起始点对应的速度,采用/>表示路径起始点对应的加速度。
对于终止点处的路径终止点横向坐标可以采用x e 表示,传统方案中会以道路中心线为基准设置一个最大横向偏移距离x max ,以限制x e 的取值范围即x e ∈(-x max ,x max ),确保车辆处于可行驶区域内。然而这将导致车辆一味的追求高行驶效率,而出现长时间跨车道线行驶的行为,这不符合实际驾驶要求。
在发明实施例中,为了鼓励车辆尽可能沿车道中心线行驶,可以将x e 的取值仅设定在车道中心点处,考虑车辆变道、超车等驾驶行为,x e 可设定在当前车道、左侧车道或右侧车道,因此x e 应当具有离散的取值空间即x e ∈(x left ,x center ,x right ),其中,x left 为左车道的中心点、x center 为当前车道的中心点、x right 为右车道的中心点。因此在本发明实施例中,可以将车辆在终止点处的车道中心线的横向坐标作为路径终止点横向坐标。
对于终止点处的路径终止点纵向坐标可以采用y e 表示,传统方案中会将路径终止点纵向坐标设定为车辆在固定窗口时间T内可以达到的最远距离,即y e =y s + v max *T,其中,v max 表示车辆最大速度,T表示窗口时间。该方式虽然能够确保可行的路径-速度匹配投影,但会使得每次规划得到的轨迹执行步数固定,无法根据驾驶情况自适应调整,当出现突发紧急事件,车辆将难以做出应急避让行为,缺乏风险响应能力。
为满足动态不确定场景中的车辆多样化驾驶需求,在本发明实施例中,将路径终止点纵向坐标y e 和窗口时间T均作为待学习参数,从而使运动轨迹参数可根据车辆所处环境的风险程度自适应调整路径长度,以实现长度可变的自适应路径规划,有效增强所生成候选路径的灵活性与风险响应能力。
在本发明实施中,可以根据车辆最大速度、设定的最大窗口时间以及路径起始点坐标包含的路径起始点纵向坐标,确定出路径终止点纵向坐标,即y e ∈(0,y max ),其中,y max 表示车辆最远感知距离。T∈(0,T max ),T max 表示最大窗口时间。
对于终止点处的路径终止点航向角,考虑车辆自身动力学限制,可以根据车辆最大航向角限制条件,确定出路径终止点航向角。在实际应用中,车辆最大航向角限制条件可以以航向角取值范围表示,,其中,/>表示车辆最大航向角。
路径终止点处的运动状态可以由速度v e 、加速度两个参数表征,二者取值范围均受车辆自身动力学限制,因此,在本发明实施例中,可以基于路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
路径终止点处的运动状态可以由速度v e 、加速度两个参数表征,二者取值范围均受车辆自身动力学限制,因此,在本发明实施例中,可以基于路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
对于任意局部运动轨迹,其待学习参数可以包括路径终止点对应的横向坐标x e 、纵向坐标y e 、航向角、窗口时间T、速度v e 以及加速度/>,因此在本发明实施例中,可以将驾驶策略的输出动作设计为/>,各动作分量的取值范围表示为:
;
S102:对路径信息进行曲线拟合采样,以得到路径点序列;对运动信息进行曲线拟合采样,以得到运动点序列。
在实际应用中,车辆行驶路径可行性通常会受到车辆自身动力学约束,包括转向角、安全距离等。因此路径上的任意一点的曲率必须小于目标的最大曲率限制或最小转弯半径等路径可行性约束,同时所产生的可行路径还应该是曲率连续的。
为满足路径可行性约束,同时降低求解空间维度,在本发明实施例中,可以基于多项式曲线分别生成路径信息对应的路径曲线、生成运动信息对应的运动曲线,将车辆运动规划问题转化为曲线参数的搜索寻优。通过在路径曲线上取点得到路径点序列,在运动曲线上取点得到运动点序列。
S103:按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹。
由于路径曲线和运动曲线的生成过程相对独立,无法直接按照规划步长或序列索引将路径点序列和运动点序列等间隔对应起来,因此在本发明实施例中,可以分别通过运动点序列计算车辆在不同运动点处的行驶距离、通过路径点序列计算车辆在不同路径点处的行驶距离。为了便于区分,可以将基于运动点序列计算得到的各行驶距离称作第一距离序列,将基于路径点序列计算得到的各行驶距离称作第二距离序列。
通过对比第一距离序列中各行驶距离和第二距离序列中各行驶距离之间的差距,实现运动点与路径点间的耦合,从而构成可行驶的运动轨迹。运动轨迹可以包括不同运动点各自所对应的位姿信息,位姿信息可以包括运动点匹配的横向坐标、运动点匹配的纵向坐标、运动点匹配的航向角、运动点对应的速度以及加速度。
S104:基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型。
在本发明实施例中,可以采用强化学习框架训练基于运动规划的运动轨迹参数。强化学习是一项强大的自学习技术,强化学习框架中,运动轨迹参数将通过与环境交互不断进行探索与试错,以在线产生学习样本,同时根据环境实时反馈的奖励信号,每一次交互周期中按照预期累计回报最大化的优化目标进行运动轨迹参数更新。
为了提升驾驶安全性,同时避免运动轨迹参数过保守对交通效率产生负面影响,在本发明实施例中,在运动轨迹参数训练过程中引入基于安全距离的成本约束条件,以实现驾驶安全性和效率之间的良好平衡。
在实际应用中,可以基于运动轨迹、俯视图像以及运动规划参数,构建价值函数。根据价值函数与安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数。通过最大化目标函数的方式,实现出初始轨迹识别模型的训练,从而得到最终的轨迹识别模型。
S105:利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。
在训练得到轨迹识别模型后,可以利用轨迹识别模型对获取的新俯视图像进行分析,从而得到与新俯视图像匹配的运动轨迹参数。在该运动轨迹参数中可以包含多个运动点各自对应的位姿信息。按照该运动轨迹参数可以实现车辆的自动化驾驶控制。
由上述技术方案可以看出,获取路径对应的俯视图像和运动规划参数;其中,运动规划参数包括路径信息和运动信息;对路径信息进行曲线拟合采样,以得到路径点序列;对运动信息进行曲线拟合采样,以得到运动点序列;按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹;基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。本发明的有益效果在于,为提升运动轨迹参数的可行性,采用驾驶环境信息作为驾驶策略状态输入表示形式,并设计基于运动规划参数的驾驶策略动作表示,将车辆运动规划问题转化为曲线的搜索寻优,从而基于驾驶策略动作输出运动轨迹,实现了自适应变化的车辆运动轨迹的规划,确保了车辆行驶的平稳性和安全性。同时引入成本约束条件对运动轨迹参数优化目标进行自适应安全性约束,增强了运动轨迹参数学习的稳定性和安全性,实现了驾驶安全性和效率间的良好平衡。
在实际应用中,可以通过间隔采样的方式获取路径点序列,路径点序列可以包括多个路径点坐标及其匹配的航向角。
图2为本发明实施例提供的一种获取路径点序列的方法的流程图,该方法包括:
S201:根据路径信息包含的路径起止参数,将路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线。
在本发明实施例中,可以对路径信息进行多项式曲线拟合,以得到初始路径曲线。根据路径信息包含的路径起止参数,构造路径约束条件;其中,路径起止参数包括路径起始点坐标和路径起始点航向角,以及路径终止点坐标和路径终止点航向角。基于路径约束条件,确定出初始路径曲线的路径曲线系数,以得到最终的路径曲线。
对于路径曲线拟合的方式可以采用五次多项式进行曲线拟合。
假设一次规划期中驾驶策略的状态输入为s,根据状态s获取到运动规划参数,已知路径起始点状态为,路径终止点状态为/>,采用5次多项式描述横向坐标与纵向坐标的变化关系,如下公式(1)所示:
(1);
其中,α 0~α 5均为路径曲线系数,x表示横向坐标,y表示纵向坐标。
通过如下公式(2)表示路径约束条件:
(2);
其中,表示路径起始点纵向坐标在初始路径曲线上所对应的路径起始点横向坐标,/>表示路径起始点纵向坐标在初始路径曲线上的一阶导数,/>表示路径起始点纵向坐标在初始路径曲线上的二阶导数,/>表示路径终止点纵向坐标在初始路径曲线上所对应的路径终止点横向坐标,/>表示路径终止点纵向坐标在初始路径曲线上的一阶导数,/>表示路径终止点纵向坐标在初始路径曲线上的一阶导数。
根据路径约束条件通过下式(3)可以求解出各路径曲线系数:
(3);
将公式(3)求解出的各路径曲线系数带入公式(1)可以得到所需的路径曲线的表达式。
S202:按照纵向坐标等间隔采样方式从路径曲线中取点,以得到多个路径点坐标。
在本发明实施例中,可以根据路径信息包含的路径起始点纵向坐标和路径终止点纵向坐标、以及设定的窗口时间,确定出路径采样间隔;根据路径起始点纵向坐标以及路径采样间隔,确定出各路径点纵向坐标;从路径曲线中选取与各路径点纵向坐标匹配的路径点横向坐标。
在实际应用中,可以对车辆纵向位置在[y s ,y e ]内等间隔采样取点,结合公式(1)和公式(3),可以得到一系列离散路径点。其中,路径采样间隔的设置与窗口时间T相关,表示为,/>表示纵向坐标对应的路径采样间隔。
根据路径采样间隔和路径起始点纵向坐标,可以依次确定出各路径点纵向坐标。将各路径点纵向坐标带入公式(1)可以得到各路径点纵向坐标匹配的路径点横向坐标。
将路径点序列表示为。该序列包含100T+1个路径点,其中(x i ,y i )为第i个路径点对应的路径点坐标。/>
S203:基于路径点坐标和航向角的对应关系,确定出各路径点坐标匹配的航向角。
在实际应用中,可以调用航向角计算公式对各路径点坐标进行处理,以得到各路径点坐标匹配的航向角;航向角计算公式为:
;
其中,x i 表示第i个路径点的路径点横向坐标,x i-1表示第i-1个路径点的路径点横向坐标,y i 表示第i个路径点的路径点纵向坐标,y i-1表示第i-1个路径点的路径点纵向坐标,表示第i个路径点的航向角。
在本发明实施例中,根据路径信息包含的路径起止参数,将路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线。按照纵向坐标等间隔采样方式从路径曲线中取点,以得到多个路径点坐标。基于路径点坐标和航向角的对应关系,确定出各路径点坐标匹配的航向角。通过曲线拟合的方式,可以将车辆运动规划问题转化为曲线参数的搜索寻优。采用等间隔采样的方式可以得到一系列离散路径点,从而为后续路径点和运动点的耦合提供了数据支持。
图3为本发明实施例提供的一种获取运动点序列的方法的流程图,该方法包括:
S301:根据运动信息包含的运动起止参数,将运动信息转换为记录速度和时间变化关系的运动曲线。
由于路径曲线不包含速度、加速度等车辆运动信息,为确保车辆可沿该路径行驶,还需相应地进行运动规划。
在本发明实施例中,可以对运动信息进行多项式曲线拟合,以得到初始运动曲线;根据运动信息包含的运动起止参数,构造运动约束条件;其中,运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度。基于运动约束条件,确定出初始运动曲线的运动曲线系数,以得到最终的运动曲线。
对于运动曲线拟合的方式可以采用五次多项式进行曲线拟合。
在已知运动起止参数的情况下,可以采用五次多项式描述速度v与时间步t的变化关系,如下公式(4)所示:
(5);
其中,β 0~β 5均为运动曲线系数,f v (t) 表示运动曲线上时间步t所对应的速度,v表示速度,t表示时间步,t s 表示局部路径的起始时间步。为便于处理,可以令起始时间步t s =0。结束时间步等同于窗口时间,即t e =T。
根据起始点和终止点处的车辆运动状态构造运动约束条件,如下公式(5)所示:
(5) ;
其中,表示在运动曲线上起始时间步为零时对应的速度,/>表示在运动曲线上起始时间步为零时对应的一阶导数,/>表示在运动曲线上起始时间步为零时对应的二阶导数,/>表示在运动曲线上结束时间步为窗口时间时对应的速度,/>表示在运动曲线上结束时间步为窗口时间时对应的一阶导数,/>表示在运动曲线上结束时间步为窗口时间时对应的二阶导数。
根据运动约束条件,通过如下公式(6)可以求解出各运动曲线系数:
(6);
S302:按照时间等间隔采样方式从运动曲线中取点,以得到多个运动点各自对应的速度。
在本发明实施例中,可以根据运动点总数与路径点总数的比例关系、以及设定的窗口时间,确定出采样时间;按照采样时间对运动曲线进行等间隔采样,以得到多个运动点各自对应的速度。
在实际应用中,将公式(6)求解出的各运动曲线系数带入公式(4),可以得到所需的运动曲线的表达式。可以对窗口时间进行等间隔采样,确定出采样时间。按照采样时间从运动曲线中取点,可以得到一系列离散速度值。
为了减小各路径点处位置和速度的匹配误差,可以将运动点的数量设置的小于路径点的数量。例如,可以将运动点数量设置为路径点数量的1/10,因此运动曲线对应的采样时间设置为∆t=0.1,产生的运动点数量为10T+1。
S303:基于速度和加速度的对应关系,确定出各速度匹配的加速度。
在实际应用中,可以根据采样时间以及各运动点各自对应的速度,确定出各速度匹配的加速度。
在得到一系列离散速度值后,可以按照公式,计算出每个速度所匹配的加速度,其中,/>表示第j个运动点对应的加速度,v j+1表示第j+1个运动点对应的速度,v j 表示第j个运动点对应的速度,/>表示采样时间。
将运动点序列表示为,其中,起始运动点/>,终止运动点。
在本发明实施例中,根据运动信息包含的运动起止参数,将运动信息转换为记录速度和时间变化关系的运动曲线。按照时间等间隔采样方式从运动曲线中取点,以得到多个运动点各自对应的速度。基于速度和加速度的对应关系,确定出各速度匹配的加速度。通过曲线拟合的方式,可以将车辆运动规划问题转化为曲线参数的搜索寻优。采用等间隔采样的方式可以得到一系列离散运动点,从而为后续路径点和运动点的耦合提供了数据支持。
图4为本发明实施例提供的一种对路径点序列和运动点序列进行耦合得到运动轨迹的方法的流程图,该方法包括:
S401:依据采样时间对运动曲线进行积分运算,以得到第一距离序列。
按照采样时间和窗口时间,可以确定出运动点的数量,每个运动点有其对应的一个行驶距离,在本发明实施例中,可以将所有运动点各自对应的行驶距离进行汇总作为第一距离序列。
在确定出采样时间∆t后,可以按照如下公式对运动曲线进行积分运算,以得到第一距离序列:
;
其中,dist(j)表示第j个运动点处车辆对应的行驶距离。
根据上式可得到与运动点序列相对应的第一距离序列,其中,车辆在行驶距离dist(j)处的速度和加速度为/>。
S402:基于多个路径点坐标,构建第二距离序列。
在本发明实施例中,可以将所有路径点各自对应的行驶距离进行汇总作为第二距离序列。
在实际应用中,可以按照如下公式确定出各路径点处车辆对应的行驶距离:
;
其中,表示第i个路径点处车辆对应的行驶距离,x k 表示第k个路径点横向坐标,y k 表示第k个路径点纵向坐标,x k-1表示第k-1个路径点横向坐标,y k-1表示第k-1个路径点纵向坐标。
S403:根据距离最小原则对第一距离序列和第二距离序列进行距离匹配,以确定出与各运动点匹配的路径点。
由于路径曲线和运动曲线的生成过程相对独立,无法直接按照规划步长或序列索引将其等间隔对应起来。因此在本发明实施例中,可以通过比对运动点对应的行驶距离和路径点对应的行驶距离,实现路径点序列和运动点序列的耦合,耦合指的是确定出与每个运动点最接近的路径点。
每个运动点的处理方式类似,以所有运动点中的任意一个运动点即第一运动点为例,可以将第一运动点对应的行驶距离称作第一距离。
在实际应用中,可以根据距离最小原则,从第二距离序列中选取与第一距离匹配的第二距离。将第二距离所对应的前后两个路径点作为与第一运动点匹配的路径点。
在本发明实施例中,为了区分不同的路径点,针对于每个路径点有其对应的路径点索引。按照路径点从起始位置到终止位置的顺序,路径点索引可以为依次递增1的数字。同理,运动点也可以设置其对应的运动点索引。在实际应用中,可以通过计算路径点索引的方式,确定出与各运动点匹配的路径点。
举例说明,对于任意一个运动点,已知该点处的行驶距离为dist(j),按照距离值大小选取与之最接近的路径点索引。例如,可以按照公式计算出路径点索引:,其中,ind表示路径点索引。
S404:根据各运动点对应的速度和加速度、以及与各运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
在本发明实施例中,可以基于第一距离与第二距离的偏差,确定出距离权重;根据距离权重、第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与第一运动点匹配的路径点坐标和航向角;将第一运动点对应的速度和加速值、以及与第一运动点匹配的路径点坐标和航向角作为第一运动点的运动轨迹参数。
距离权重可以基于路径点索引所对应的行驶距离确定得到。
在实际应用中,可以将第二距离与第一距离的差值作为第二距离匹配的路径点索引。调用权重计算公式对第一距离以及路径点索引所对应的行驶距离进行处理,以得到距离权重;权重计算公式为:
;
其中,∈表示距离权重,dist(j)表示第j个运动点对应的第一距离,ind表示第二距离匹配的路径点索引,表示路径点索引对应的行驶距离,/>表示与路径点索引相邻的前一个路径点索引对应的行驶距离。
在确定出距离权重之后,可以调用路径点位姿计算公式,对距离权重、第二距离所对应的前后两个路径点各自的路径点坐标和航向角进行处理,以得到与第一运动点匹配的路径点坐标和航向角;路径点位姿计算公式为:
;
其中,表示与第一运动点匹配的路径点横向坐标,/>表示与第一运动点匹配的路径点纵向坐标,/>表示与第一运动点匹配的航向角,x idx-1表示第二距离所对应的前一个路径点的路径点横向坐标,x idx 表示第二距离所对应的后一个路径点的路径点横向坐标,y idx-1表示第二距离所对应的前一个路径点的路径点纵向坐标,y idx 表示第二距离所对应的后一个路径点的路径点纵向坐标,/>表示第二距离所对应的前一个路径点的航向角,/>表示第二距离所对应的后一个路径点的航向角,∈表示距离权重。
在确出与每个运动点匹配的路径点坐标和航向角后,可以将每个运动点对应的速度和加速度以及与该运动点匹配的路径点坐标和航向角作为位姿信息。
基于每个运动点对应的位姿信息,可以得到所需的运动轨迹,其中,τ表示运动轨迹,p j 表示第j个运动点对应的位姿信息,/>表示与第j个运动点匹配的路径点横向坐标,/>表示与第j个运动点匹配的路径点纵向坐标,/>表示第j个运动点匹配的航向角,v j 第j个运动点对应的速度,/>第j个运动点对应的加速度。
S501:基于俯视图像、运动规划参数以及运动轨迹对应的动作奖励,构建训练样本。
在本发明实施例中,可以将俯视图像作为状态输入s,将运动规划参数作为动作表示a,采用基于强化学习框架训练基于运动规划的运动轨迹参数。
运动轨迹τ对应的动作奖励可以采用r表示。在实际应用中,可以将驾驶数据(s,a,r)作为训练样本。该训练样本可以看作一个数据集。
动作奖励的形式如下:r=λ e r e +λ s r s +λ ot r ot ;
其中,r e 、r s 和r ot 分别为车辆效率、安全和超车奖惩项。
各项具体设置为;
其中,v target 为人为设置的车辆理想行驶速度,λ j 和λ v 是用于调节奖励尺度的系数,R jerk 是根据状态输入s和动作表示a所生成运动轨迹的加加速度积分即;其中,/>表示运动轨迹τ任一点处的加加速度。
;
其中,是碰撞指示标志,若发生碰撞,/>,否则为0,r c 是人为设定的碰撞惩罚值。
;
其中,是超车指示标志,若实现超车行为,/>,否则为0,r pass 是人为设定的超车奖励值。
车辆是否发生碰撞或是否完成超车行为,需根据车辆轨迹位置信息和周围环境信息进行判断。
S502:基于运动轨迹、俯视图像以及运动规划参数,构建价值函数。
为了便于描述,可以采用π表示运动轨迹参数,表示运动轨迹参数的优化目标,其优化过程可以表示为:
;
其中,T i 表示一次交互过程总步长,表示奖励衰减因子,/>表示求期望,s t 表示时间步t对应的状态输入,a t 表示时间步t对应的驾驶策略的动作输出,a t =π(s t )表示在时间步t下状态输入为s t 的情况下驾驶策略所对应的动作输出,r为动作奖励,其以函数的形式呈现可以称作奖励函数,r通常设计为行驶效率、安全性、平稳性等量化指标的线性组合。
驾驶策略的性能通过价值函数进行评估,价值函数的形式如下:
;
其中,表示驾驶策略对应的价值函数。
因此,驾驶策略的优化目标可以表示为:
。
S503:根据价值函数与安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数。
为了提升驾驶安全性,同时避免策略过保守对交通效率产生负面影响,本发明在驾驶策略训练过程中引入基于安全距离的成本约束条件,以实现驾驶安全和效率之间的良好平衡。
成本约束条件如下:
;
其中,是控制安全指数下降率的松弛变量,/>是为了避免碰撞所设置的成本函数。/>
定义为/>;
其中,d是车辆与要避开的运动目标之间的距离,d min 是最小安全距离,是距离相对于时间的导数,/>是待优化的可调参数。成本值越高意味着安全指数越低,这时安全约束也将变得更保守。
一般研究为了确保安全性会对运动轨迹参数施加严格的安全约束,不考虑对于效率的影响,可能会导致过保守问题。本发明设计了一种与安全指数相关的成本函数,对运动轨迹参数目标函数进行约束,可以在确保安全性的同时,避免策略过保守。本发明实施例通过采用一种可学习的成本函数,避免人为设计的成本和误差,该成本函数可以在训练期间和运动轨迹参数进行联合优化。
为求解带约束的策略优化问题,可以构建拉格朗日函数作为目标函数。在实际应用中,可以基于拉格朗日乘子、成本函数以及价值函数对应的运动轨迹参数,构建拉格朗日函数。
拉格朗日函数形式如下:
;
其中,表示拉格朗日函数,/>表示拉格朗日乘子网络,用于处理状态约束。基于该拉格朗日函数,可以实现运动轨迹参数π和成本函数/>的联合学习。
为了避免成本函数的成本值过高影响效率,这里在目标函数中添加一个与成本值相关的正则化项:;
其中,α和b均为超参数,取值在[0,1]范围内。基于上述优化目标,在每个迭代周期中,运动轨迹参数π、拉格朗日乘子λ以及成本函数将交替完成更新。
通过引入与成本值大小相关的正则化项,可以有效的防止策略过保守,实现效率上的提升。
S504:利用训练样本对初始轨迹识别模型的参数进行更新,以得到轨迹识别模型。
在实际应用中,可以从训练样本中随机采样单次训练数据;利用单次训练数据对初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型;判断迭代次数是否达到迭代上限值;在迭代次数未达到迭代上限值的情况下,返回从训练样本中随机采样单次训练数据的步骤;在迭代次数达到迭代上限值的情况下,结束对初始轨迹识别模型的训练,并将最新的初始轨迹识别模型作为轨迹识别模型。
模型每次的训练过程可以划分为三个阶段,第一阶段按照固定拉格朗日乘子、固定成本函数以及最大化拉格朗日函数的方式,对拉格朗日函数进行训练,以得到第一拉格朗日函数。第二阶段按照固定价值函数对应的运动轨迹参数、固定成本函数以及最小化拉格朗日函数的方式,对第一拉格朗日函数进行训练,以得到第二拉格朗日函数。第三阶段按照固定价值函数对应的运动轨迹参数、固定拉格朗日乘子以及最大化拉格朗日函数的方式,对第二拉格朗日函数进行训练,以得到训练后的初始轨迹识别模型。
举例说明,在实际应用中,可以首先固定拉格朗日乘子λ和成本函数,通过最大化目标函数学习策略π,/>。然后,固定策略π和成本函数/>,通过最小化目标函数学习拉格朗日乘子λ,/>。接下来,固定策略π和拉格朗日乘子λ,通过最大化目标函数学习成本函数/>,/>。
图6为本发明实施例提供的一种自适应安全约束的运动轨迹参数学习方法的示意图,包含状态和动作空间定义、运动轨迹生成和基于运动规划的运动轨迹参数学习这三个模块。状态和动作空间定义模块包含状态空间和动作空间这两部分,其中,状态空间包含的是俯视图像,动作空间包含的是运动轨迹参数。运动轨迹生成模块可以由路径曲线和运动曲线经过耦合的方式生成运动轨迹。基于运动轨迹的运动轨迹参数学习模块可以由成本约束条件对应的成本函数和基于强化学习的运动轨迹参数优化目标对应的价值函数进行结合,从而得到带约束的运动轨迹参数的优化目标。通过最大化目标函数的方式,可以训练得到轨迹识别模型。
训练过程包括以下步骤:
步骤a基于运动轨迹参数π执行环境交互,收集驾驶数据:①获取车辆驾驶状态s,采用运动轨迹参数π选择运动规划参数,并将车辆当前位姿与运动信息作为起始状态/>;② 基于起始位姿/>、结束位姿/>以及窗口时间T,生成路径曲线/>;③基于起始运动状态/>、结束位姿以及窗口时间T,生成运动曲线/>;④ 结合路径曲线和运动曲线,进行耦合,生成运动轨迹/>;⑤ 车辆执行轨迹τ,得到动作奖励r,将驾驶数据(s,a,r)放入数据集/>;⑥ 判断是否达到最大交互步数T i ,若是则结束交互,进入网络更新阶段,反之转至步骤①。
步骤b利用历史驾驶数据进行网络更新:① 从数据集中随机采样小批次数据;② 基于/>更新策略π;③ 基于/>更新拉格朗日乘子网络;④ 基于/>更新成本函数/>;⑤ 判断结束条件:若迭代数达到上限,则结束更新;否则,迭代数+1,转至步骤①。
步骤c判断结束条件:若优化周期达到上限,则结束更新;否则,周期数+1,转至步骤a。
本发明提出一种自适应安全约束的运动轨迹参数学习方法,通过将运动规划参数作为策略输出,设计基于运动规划的运动轨迹参数学习方法,以实现窗口时间自适应可调的车辆运动规划,确保车辆行驶轨迹的平稳性和复杂驾驶行为的实现能力,提升驾驶策略的可行性。在策略训练期间,将可学习的安全成本函数与强化学习的优化目标相结合进行自适应安全性约束,在确保驾驶安全性的同时避免过保守行为,增强运动轨迹参数学习稳定性,实现安全性和效率双提升。
图7为本发明实施例提供的一种运动轨迹参数的确定装置的结构示意图,包括获取单元71、第一采样单元72、第二采样单元73、耦合单元74、训练单元75和分析单元76;
获取单元71,用于获取路径对应的俯视图像和运动规划参数;其中,运动规划参数包括路径信息和运动信息;
第一采样单元72,用于对路径信息进行曲线拟合采样,以得到路径点序列;
第二采样单元73,用于对运动信息进行曲线拟合采样,以得到运动点序列;
耦合单元74,用于按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹;
训练单元75,用于基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;
分析单元76,用于利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。
在一些实施例中,路径点序列包括多个路径点坐标及其匹配的航向角;
第一采样单元包括第一转换子单元、第一取点子单元和第一确定子单元;
第一转换子单元,用于根据路径信息包含的路径起止参数,将路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线;
第一取点子单元,用于按照纵向坐标等间隔采样方式从路径曲线中取点,以得到多个路径点坐标;
第一确定子单元,用于基于路径点坐标和航向角的对应关系,确定出各路径点坐标匹配的航向角。
在一些实施例中,第一转换子单元用于对路径信息进行多项式曲线拟合,以得到初始路径曲线;根据路径信息包含的路径起止参数,构造路径约束条件;其中,路径起止参数包括路径起始点坐标和路径起始点航向角,以及路径终止点坐标和路径终止点航向角;基于路径约束条件,确定出初始路径曲线的路径曲线系数,以得到最终的路径曲线。
在一些实施例中,第一取点子单元用于根据路径信息包含的路径起始点纵向坐标和路径终止点纵向坐标、以及设定的窗口时间,确定出路径采样间隔;根据路径起始点纵向坐标以及路径采样间隔,确定出各路径点纵向坐标;从路径曲线中选取与各路径点纵向坐标匹配的路径点横向坐标。
在一些实施例中,第一确定子单元用于调用航向角计算公式对各路径点坐标进行处理,以得到各路径点坐标匹配的航向角;航向角计算公式为:
;
其中,x i 表示第i个路径点的路径点横向坐标,x i-1表示第i-1个路径点的路径点横向坐标,y i 表示第i个路径点的路径点纵向坐标,y i-1表示第i-1个路径点的路径点纵向坐标,表示第i个路径点的航向角。
在一些实施例中,运动点序列包括各运动点对应的速度和加速度;
第二采样单元包括第二转换子单元、第二取点子单元和第二确定子单元;
第二转换子单元,用于根据运动信息包含的运动起止参数,将运动信息转换为记录速度和时间变化关系的运动曲线;
第二取点子单元,用于按照时间等间隔采样方式从运动曲线中取点,以得到多个运动点各自对应的速度;
第二确定子单元,用于基于速度和加速度的对应关系,确定出各速度匹配的加速度。
在一些实施例中,第二转换子单元用于对运动信息进行多项式曲线拟合,以得到初始运动曲线;根据运动信息包含的运动起止参数,构造运动约束条件;其中,运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;基于运动约束条件,确定出初始运动曲线的运动曲线系数,以得到最终的运动曲线。
在一些实施例中,第二取点子单元用于根据运动点总数与路径点总数的比例关系、以及设定的窗口时间,确定出采样时间;按照采样时间对运动曲线进行等间隔采样,以得到多个运动点各自对应的速度。
在一些实施例中,第二确定子单元用于根据采样时间以及各运动点各自对应的速度,确定出各速度匹配的加速度。
在一些实施例中,耦合单元包括运算子单元、距离序列构建子单元、匹配子单元和轨迹确定子单元;
运算子单元,用于依据采样时间对运动曲线进行积分运算,以得到第一距离序列;
距离序列构建子单元,用于基于多个路径点坐标,构建第二距离序列;
匹配子单元,用于根据距离最小原则对第一距离序列和第二距离序列进行距离匹配,以确定出与各运动点匹配的路径点;
轨迹确定子单元,用于根据各运动点对应的速度和加速度、以及与各运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
在一些实施例中,匹配子单元用于根据距离最小原则,从第二距离序列中选取与第一距离匹配的第二距离;其中,第一距离为第一运动点对应的行驶距离,第一运动点为所有运动点中的任意一个运动点;将第二距离所对应的前后两个路径点作为与第一运动点匹配的路径点。
在一些实施例中,运动轨迹包括各运动点各自对应的运动轨迹参数;
轨迹确定子单元用于基于第一距离与第二距离的偏差,确定出距离权重;根据距离权重、第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与第一运动点匹配的路径点坐标和航向角;将第一运动点对应的速度和加速值、以及与第一运动点匹配的路径点坐标和航向角作为第一运动点的运动轨迹参数。
在一些实施例中,轨迹确定子单元用于将第二距离与第一距离的差值作为第二距离匹配的路径点索引;
调用权重计算公式对第一距离以及路径点索引所对应的行驶距离进行处理,以得到距离权重;权重计算公式为:
;
其中,dist(j)表示第j个运动点对应的第一距离,ind表示第二距离匹配的路径点索引,表示路径点索引对应的行驶距离,/>表示与路径点索引相邻的前一个路径点索引对应的行驶距离。
在一些实施例中,轨迹确定子单元用于调用路径点位姿计算公式,对距离权重、第二距离所对应的前后两个路径点各自的路径点坐标和航向角进行处理,以得到与第一运动点匹配的路径点坐标和航向角;路径点位姿计算公式为:
;/>
其中,表示与第一运动点匹配的路径点横向坐标,/>表示与第一运动点匹配的路径点纵向坐标,/>表示与第一运动点匹配的航向角,x idx-1表示第二距离所对应的前一个路径点的路径点横向坐标,x idx 表示第二距离所对应的后一个路径点的路径点横向坐标,y idx-1表示第二距离所对应的前一个路径点的路径点纵向坐标,y idx 表示第二距离所对应的后一个路径点的路径点纵向坐标,/>表示第二距离所对应的前一个路径点的航向角,/>表示第二距离所对应的后一个路径点的航向角,∈表示距离权重。
在一些实施例中,训练单元包括样本构建子单元、价值函数构建子单元、目标函数构建子单元和更新子单元;
样本构建子单元,用于基于俯视图像、运动规划参数以及运动轨迹对应的动作奖励,构建训练样本;
价值函数构建子单元,用于基于运动轨迹、俯视图像以及运动规划参数,构建价值函数;
目标函数构建子单元,用于根据价值函数与安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数;
更新子单元,用于利用训练样本对初始轨迹识别模型的参数进行更新,以得到轨迹识别模型。
在一些实施例中,更新子单元用于从训练样本中随机采样单次训练数据;利用单次训练数据对初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型;判断迭代次数是否达到迭代上限值;在迭代次数未达到迭代上限值的情况下,返回从训练样本中随机采样单次训练数据的步骤;在迭代次数达到迭代上限值的情况下,结束对初始轨迹识别模型的训练,并将最新的初始轨迹识别模型作为轨迹识别模型。
在一些实施例中,目标函数构建子单元用于基于拉格朗日乘子、成本函数以及价值函数对应的运动轨迹参数,构建拉格朗日函数。
在一些实施例中,更新子单元用于按照固定拉格朗日乘子、固定成本函数以及最大化拉格朗日函数的方式,对拉格朗日函数进行训练,以得到第一拉格朗日函数;按照固定价值函数对应的运动轨迹参数、固定成本函数以及最小化拉格朗日函数的方式,对第一拉格朗日函数进行训练,以得到第二拉格朗日函数;按照固定价值函数对应的运动轨迹参数、固定拉格朗日乘子以及最大化拉格朗日函数的方式,对第二拉格朗日函数进行训练,以得到训练后的初始轨迹识别模型。
在一些实施例中,获取单元用于获取车辆在起始点位置对应的路径起始点坐标、路径起始点航向角、路径起始点对应的速度和加速度;将车辆在终止点处的车道中心线的横向坐标作为路径终止点横向坐标;根据车辆最大速度、设定的最大窗口时间以及路径起始点坐标包含的路径起始点纵向坐标,确定出路径终止点纵向坐标;根据车辆最大航向角限制条件,确定出路径终止点航向角;基于路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
图7所对应实施例中特征的说明可以参见图1至图6所对应实施例的相关说明,这里不再一一赘述。
由上述技术方案可以看出,获取路径对应的俯视图像和运动规划参数;其中,运动规划参数包括路径信息和运动信息;对路径信息进行曲线拟合采样,以得到路径点序列;对运动信息进行曲线拟合采样,以得到运动点序列;按照路径点和运动点之间行驶距离匹配的方式,对路径点序列和运动点序列进行耦合,以得到运动轨迹;基于安全距离的成本约束条件、运动轨迹、俯视图像以及运动规划参数,训练得到轨迹识别模型;利用轨迹识别模型对获取的新俯视图像进行分析,以得到与新俯视图像匹配的运动轨迹参数。本发明的有益效果在于,为提升运动轨迹参数的可行性,采用驾驶环境信息作为驾驶策略状态输入表示形式,并设计基于运动规划参数的驾驶策略动作表示,将车辆运动规划问题转化为曲线的搜索寻优,从而基于驾驶策略动作输出运动轨迹,实现了自适应变化的车辆运动轨迹的规划,确保了车辆行驶的平稳性和安全性。同时引入成本约束条件对运动轨迹参数优化目标进行自适应安全性约束,增强了运动轨迹参数学习的稳定性和安全性,实现了驾驶安全性和效率间的良好平衡。
图8为本发明实施例提供的一种运动轨迹参数的确定设备的结构图,如图8所示,运动轨迹参数的确定设备包括:存储器80,用于存储计算机程序;
处理器81,用于执行计算机程序时实现如上述实施例运动轨迹参数的确定方法的步骤。
本实施例提供的运动轨迹参数的确定设备可以包括但不限于智能手机、平板电脑、笔记本电脑或台式电脑等。
其中,处理器81可以包括一个或多个处理核心,比如4核心处理器、8核心处理器等。处理器81可以采用DSP(Digital Signal Processing,数字信号处理)、FPGA(Field-Programmable Gate Array,现场可编程门阵列)、PLA(Programmable Logic Array,可编程逻辑阵列)中的至少一种硬件形式来实现。处理器81也可以包括主处理器和协处理器,主处理器是用于对在唤醒状态下的数据进行处理的处理器,也称CPU(Central ProcessingUnit,中央处理器);协处理器是用于对在待机状态下的数据进行处理的低功耗处理器。在一些实施例中,处理器81可以在集成有GPU(Graphics Processing Unit,图像处理器),GPU用于负责显示屏所需要显示的内容的渲染和绘制。一些实施例中,处理器81还可以包括AI(Artificial Intelligence,人工智能)处理器,该AI处理器用于处理有关机器学习的计算操作。
存储器80可以包括一个或多个计算机可读存储介质,该计算机可读存储介质可以是非暂态的。存储器80还可包括高速随机存取存储器,以及非易失性存储器,比如一个或多个磁盘存储设备、闪存存储设备。本实施例中,存储器80至少用于存储以下计算机程序801,其中,该计算机程序被处理器81加载并执行之后,能够实现前述任一实施例公开的运动轨迹参数的确定方法的相关步骤。另外,存储器80所存储的资源还可以包括操作系统802和数据803等,存储方式可以是短暂存储或者永久存储。其中,操作系统802可以包括Windows、Unix、Linux等。数据803可以包括但不限于俯视图像、运动规划参数等。
在一些实施例中,运动轨迹参数的确定设备还可包括有显示屏82、输入输出接口83、通信接口84、电源85以及通信总线86。
本领域技术人员可以理解,图8中示出的结构并不构成对运动轨迹参数的确定设备的限定,可以包括比图示更多或更少的组件。
可以理解的是,如果上述实施例中的运动轨迹参数的确定方法以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对目前技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(Read-Only Memory,ROM)、随机存取存储器(Random Access Memory,RAM)、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、磁碟或者光盘等各种可以存储程序代码的介质。
基于此,本发明实施例还提供了一种计算机可读存储介质,计算机可读存储介质上存储有计算机程序,计算机程序被处理器执行时实现如上述运动轨迹参数的确定方法的步骤。
以上对本发明实施例所提供的一种运动轨迹参数的确定方法、装置、设备和存储介质进行了详细介绍。说明书中各个实施例采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
专业人员还可以进一步意识到,结合本文中所公开的实施例描述的各示例的单元及算法步骤,能够以电子硬件、计算机软件或者二者的结合来实现,为了清楚地说明硬件和软件的可互换性,在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行,取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能,但是这种实现不应认为超出本发明的范围。
以上对本发明所提供的一种运动轨迹参数的确定方法、装置、设备和存储介质进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以对本发明进行若干改进和修饰,这些改进和修饰也落入本发明权利要求的保护范围内。
Claims (21)
1.一种运动轨迹参数的确定方法,其特征在于,包括:
获取路径对应的俯视图像和运动规划参数;其中,所述运动规划参数包括路径信息和运动信息;路径信息包括路径起止参数,路径起止参数分为路径起始点参数和路径终止点参数,路径起始点参数包括路径起始点坐标和路径起始点航向角,路径终止点参数包括路径终止点坐标和路径终止点航向角;运动信息包含运动起止参数;运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;
对所述路径信息进行曲线拟合采样,以得到路径点序列;对所述运动信息进行曲线拟合采样,以得到运动点序列;路径点序列包括多个路径点坐标及其匹配的航向角;运动点序列包括各运动点对应的速度和加速度;
按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹;运动轨迹包括各运动点各自对应的运动轨迹参数;
基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型;
利用所述轨迹识别模型对获取的新俯视图像进行分析,以得到与所述新俯视图像匹配的运动轨迹参数;运动轨迹参数中包含多个运动点各自对应的位姿信息;位姿信息包括运动点匹配的横向坐标、运动点匹配的纵向坐标、运动点匹配的航向角、运动点对应的速度以及加速度;
所述按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹包括:
依据采样时间对运动曲线进行积分运算,以得到第一距离序列;
基于多个所述路径点坐标,构建第二距离序列;
根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点;
根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
2.根据权利要求1所述的运动轨迹参数的确定方法,其特征在于,所述路径点序列包括多个路径点坐标及其匹配的航向角;
所述对所述路径信息进行曲线拟合采样,以得到路径点序列包括:
根据所述路径信息包含的路径起止参数,将所述路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线;
按照纵向坐标等间隔采样方式从所述路径曲线中取点,以得到多个路径点坐标;
基于路径点坐标和航向角的对应关系,确定出各所述路径点坐标匹配的航向角。
3.根据权利要求2所述的运动轨迹参数的确定方法,其特征在于,所述根据所述路径信息包含的路径起止参数,将所述路径信息转换为记录横向坐标和纵向坐标变化关系的路径曲线包括:
对所述路径信息进行多项式曲线拟合,以得到初始路径曲线;
根据所述路径信息包含的路径起止参数,构造路径约束条件;其中,所述路径起止参数包括路径起始点坐标和路径起始点航向角,以及路径终止点坐标和路径终止点航向角;
基于所述路径约束条件,确定出所述初始路径曲线的路径曲线系数,以得到最终的路径曲线。
4.根据权利要求2所述的运动轨迹参数的确定方法,其特征在于,所述按照纵向坐标等间隔采样方式从所述路径曲线中取点,以得到多个路径点坐标包括:
根据所述路径信息包含的路径起始点纵向坐标和路径终止点纵向坐标、以及设定的窗口时间,确定出路径采样间隔;
根据所述路径起始点纵向坐标以及所述路径采样间隔,确定出各路径点纵向坐标;
从所述路径曲线中选取与各所述路径点纵向坐标匹配的路径点横向坐标。
5.根据权利要求2所述的运动轨迹参数的确定方法,其特征在于,所述基于路径点坐标和航向角的对应关系,确定出各所述路径点坐标匹配的航向角包括:
调用航向角计算公式对各所述路径点坐标进行处理,以得到各所述路径点坐标匹配的航向角;所述航向角计算公式为:
;
其中,x i 表示第i个路径点的路径点横向坐标,x i-1表示第i-1个路径点的路径点横向坐标,y i 表示第i个路径点的路径点纵向坐标,y i-1表示第i-1个路径点的路径点纵向坐标,表示第i个路径点的航向角。
6.根据权利要求2所述的运动轨迹参数的确定方法,其特征在于,所述运动点序列包括各运动点对应的速度和加速度;
所述对所述运动信息进行曲线拟合采样,以得到运动点序列包括:
根据所述运动信息包含的运动起止参数,将所述运动信息转换为记录速度和时间变化关系的运动曲线;
按照时间等间隔采样方式从所述运动曲线中取点,以得到多个运动点各自对应的速度;
基于速度和加速度的对应关系,确定出各所述速度匹配的加速度。
7.根据权利要求6所述的运动轨迹参数的确定方法,其特征在于,所述根据所述运动信息包含的运动起止参数,将所述运动信息转换为记录速度和时间变化关系的运动曲线包括:
对所述运动信息进行多项式曲线拟合,以得到初始运动曲线;
根据所述运动信息包含的运动起止参数,构造运动约束条件;其中,所述运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;
基于所述运动约束条件,确定出所述初始运动曲线的运动曲线系数,以得到最终的运动曲线。
8.根据权利要求6所述的运动轨迹参数的确定方法,其特征在于,所述按照时间等间隔采样方式从所述运动曲线中取点,以得到多个运动点各自对应的速度包括:
根据运动点总数与路径点总数的比例关系、以及设定的窗口时间,确定出采样时间;
按照所述采样时间对所述运动曲线进行等间隔采样,以得到多个运动点各自对应的速度。
9.根据权利要求8所述的运动轨迹参数的确定方法,其特征在于,所述基于速度和加速度的对应关系,确定出各所述速度匹配的加速度包括:
根据所述采样时间以及各所述运动点各自对应的速度,确定出各所述速度匹配的加速度。
10.根据权利要求1所述的运动轨迹参数的确定方法,其特征在于,所述根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点包括:
根据距离最小原则,从所述第二距离序列中选取与第一距离匹配的第二距离;其中,第一距离为第一运动点对应的行驶距离,第一运动点为所有所述运动点中的任意一个运动点;
将所述第二距离所对应的前后两个路径点作为与所述第一运动点匹配的路径点。
11.根据权利要求10所述的运动轨迹参数的确定方法,其特征在于,所述运动轨迹包括各所述运动点各自对应的运动轨迹参数;
所述根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹包括:
基于所述第一距离与所述第二距离的偏差,确定出距离权重;
根据所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与所述第一运动点匹配的路径点坐标和航向角;
将所述第一运动点对应的速度和加速值、以及与所述第一运动点匹配的路径点坐标和航向角作为所述第一运动点的运动轨迹参数。
12.根据权利要求11所述的运动轨迹参数的确定方法,其特征在于,所述基于所述第一距离与所述第二距离的偏差,确定出距离权重包括:
将所述第二距离与所述第一距离的差值作为所述第二距离匹配的路径点索引;
调用权重计算公式对所述第一距离以及所述路径点索引所对应的行驶距离进行处理,以得到距离权重;所述权重计算公式为:
;
其中,dist(j)表示第j个运动点对应的第一距离,ind表示所述第二距离匹配的路径点索引,表示所述路径点索引对应的行驶距离,/>表示与所述路径点索引相邻的前一个路径点索引对应的行驶距离。
13.根据权利要求12所述的运动轨迹参数的确定方法,其特征在于,所述根据所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角,确定出与所述第一运动点匹配的路径点坐标和航向角包括:
调用路径点位姿计算公式,对所述距离权重、所述第二距离所对应的前后两个路径点各自的路径点坐标和航向角进行处理,以得到与所述第一运动点匹配的路径点坐标和航向角;所述路径点位姿计算公式为:
;
其中,表示与所述第一运动点匹配的路径点横向坐标,/>表示与所述第一运动点匹配的路径点纵向坐标,/>表示与所述第一运动点匹配的航向角,x idx-1表示所述第二距离所对应的前一个路径点的路径点横向坐标, x idx 表示所述第二距离所对应的后一个路径点的路径点横向坐标,y idx-1表示所述第二距离所对应的前一个路径点的路径点纵向坐标, y idx 表示所述第二距离所对应的后一个路径点的路径点纵向坐标,/>表示所述第二距离所对应的前一个路径点的航向角,/>表示所述第二距离所对应的后一个路径点的航向角,∈表示距离权重。
14.根据权利要求1所述的运动轨迹参数的确定方法,其特征在于,所述基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型包括:
基于所述俯视图像、所述运动规划参数以及所述运动轨迹对应的动作奖励,构建训练样本;
基于所述运动轨迹、所述俯视图像以及所述运动规划参数,构建价值函数;
根据所述价值函数与所述安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数;
利用所述训练样本对所述初始轨迹识别模型的参数进行更新,以得到轨迹识别模型。
15.根据权利要求14所述的运动轨迹参数的确定方法,其特征在于,所述利用所述训练样本对所述初始轨迹识别模型的参数进行更新,以得到轨迹识别模型包括:
从所述训练样本中随机采样单次训练数据;
利用所述单次训练数据对所述初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型;
判断迭代次数是否达到迭代上限值;
在所述迭代次数未达到迭代上限值的情况下,返回所述从所述训练样本中随机采样单次训练数据的步骤;
在所述迭代次数达到迭代上限值的情况下,结束对所述初始轨迹识别模型的训练,并将最新的初始轨迹识别模型作为轨迹识别模型。
16.根据权利要求15所述的运动轨迹参数的确定方法,其特征在于,所述根据所述价值函数与所述安全距离的成本约束条件对应的成本函数,构建初始轨迹识别模型的目标函数包括:
基于拉格朗日乘子、所述成本函数以及所述价值函数对应的运动轨迹参数,构建拉格朗日函数。
17.根据权利要求16所述的运动轨迹参数的确定方法,其特征在于,所述利用所述单次训练数据对所述初始轨迹识别模型进行训练,以得到训练后的初始轨迹识别模型包括:
按照固定所述拉格朗日乘子、固定所述成本函数以及最大化拉格朗日函数的方式,对所述拉格朗日函数进行训练,以得到第一拉格朗日函数;
按照固定所述价值函数对应的运动轨迹参数、固定所述成本函数以及最小化拉格朗日函数的方式,对所述第一拉格朗日函数进行训练,以得到第二拉格朗日函数;
按照固定所述价值函数对应的运动轨迹参数、固定所述拉格朗日乘子以及最大化拉格朗日函数的方式,对所述第二拉格朗日函数进行训练,以得到训练后的初始轨迹识别模型。
18.根据权利要求1至17任意一项所述的运动轨迹参数的确定方法,其特征在于,获取路径对应的运动规划参数包括:
获取车辆在起始点位置对应的路径起始点坐标、路径起始点航向角、路径起始点对应的速度和加速度;
将车辆在终止点处的车道中心线的横向坐标作为路径终止点横向坐标;
根据车辆最大速度、设定的最大窗口时间以及所述路径起始点坐标包含的路径起始点纵向坐标,确定出路径终止点纵向坐标;
根据车辆最大航向角限制条件,确定出路径终止点航向角;
基于所述路径起始点对应的速度、车辆最大加速度以及最大窗口时间,确定出路径终止点对应的速度和加速度。
19.一种运动轨迹参数的确定装置,其特征在于,包括获取单元、第一采样单元、第二采样单元、耦合单元、训练单元和分析单元;
所述获取单元,用于获取路径对应的俯视图像和运动规划参数;其中,所述运动规划参数包括路径信息和运动信息;路径信息包括路径起止参数,路径起止参数分为路径起始点参数和路径终止点参数,路径起始点参数包括路径起始点坐标和路径起始点航向角,路径终止点参数包括路径终止点坐标和路径终止点航向角;运动信息包含运动起止参数;运动起止参数包括路径起始点对应的速度和加速度,以及路径终止点对应的速度和加速度;
所述第一采样单元,用于对所述路径信息进行曲线拟合采样,以得到路径点序列;路径点序列包括多个路径点坐标及其匹配的航向角;
所述第二采样单元,用于对所述运动信息进行曲线拟合采样,以得到运动点序列;运动点序列包括各运动点对应的速度和加速度;
所述耦合单元,用于按照路径点和运动点之间行驶距离匹配的方式,对所述路径点序列和所述运动点序列进行耦合,以得到运动轨迹;运动轨迹包括各运动点各自对应的运动轨迹参数;
所述训练单元,用于基于安全距离的成本约束条件、所述运动轨迹、所述俯视图像以及所述运动规划参数,训练得到轨迹识别模型;
所述分析单元,用于利用所述轨迹识别模型对获取的新俯视图像进行分析,以得到与所述新俯视图像匹配的运动轨迹参数;运动轨迹参数中包含多个运动点各自对应的位姿信息;位姿信息包括运动点匹配的横向坐标、运动点匹配的纵向坐标、运动点匹配的航向角、运动点对应的速度以及加速度;
所述耦合单元包括运算子单元、距离序列构建子单元、匹配子单元和轨迹确定子单元;所述运算子单元,用于依据采样时间对运动曲线进行积分运算,以得到第一距离序列;所述距离序列构建子单元,用于基于多个所述路径点坐标,构建第二距离序列;所述匹配子单元,用于根据距离最小原则对所述第一距离序列和所述第二距离序列进行距离匹配,以确定出与各所述运动点匹配的路径点;所述轨迹确定子单元,用于根据各所述运动点对应的速度和加速度、以及与各所述运动点匹配的路径点所对应的路径点坐标和航向角,确定出运动轨迹。
20.一种运动轨迹参数的确定设备,其特征在于,包括:
存储器,用于存储计算机程序;
处理器,用于执行所述计算机程序以实现如权利要求1至18任意一项所述运动轨迹参数的确定方法的步骤。
21.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至18任意一项所述运动轨迹参数的确定方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311661791.8A CN117406756B (zh) | 2023-12-06 | 2023-12-06 | 一种运动轨迹参数的确定方法、装置、设备和存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311661791.8A CN117406756B (zh) | 2023-12-06 | 2023-12-06 | 一种运动轨迹参数的确定方法、装置、设备和存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117406756A CN117406756A (zh) | 2024-01-16 |
CN117406756B true CN117406756B (zh) | 2024-03-08 |
Family
ID=89494696
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311661791.8A Active CN117406756B (zh) | 2023-12-06 | 2023-12-06 | 一种运动轨迹参数的确定方法、装置、设备和存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117406756B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117681213A (zh) * | 2024-02-04 | 2024-03-12 | 之江实验室 | 一种工业机器人运动轨迹规划及评价方法和装置 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018019111A1 (zh) * | 2016-07-29 | 2018-02-01 | 深圳光启合众科技有限公司 | 机器人及其关节运动控制方法和装置 |
CN109990783A (zh) * | 2017-12-29 | 2019-07-09 | 深圳市优必选科技有限公司 | 一种机器人运动路径规划方法、机器人及存储介质 |
CN115309170A (zh) * | 2022-10-12 | 2022-11-08 | 之江实验室 | 一种考虑舒适性约束的轨迹规划方法、装置和系统 |
CN115981328A (zh) * | 2022-12-30 | 2023-04-18 | 上海大学 | 一种基于动态运动基元的轨迹规划与调整方法及系统 |
CN117141520A (zh) * | 2023-10-31 | 2023-12-01 | 合肥综合性国家科学中心人工智能研究院(安徽省人工智能实验室) | 一种实时轨迹规划方法、装置和设备 |
-
2023
- 2023-12-06 CN CN202311661791.8A patent/CN117406756B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018019111A1 (zh) * | 2016-07-29 | 2018-02-01 | 深圳光启合众科技有限公司 | 机器人及其关节运动控制方法和装置 |
CN109990783A (zh) * | 2017-12-29 | 2019-07-09 | 深圳市优必选科技有限公司 | 一种机器人运动路径规划方法、机器人及存储介质 |
CN115309170A (zh) * | 2022-10-12 | 2022-11-08 | 之江实验室 | 一种考虑舒适性约束的轨迹规划方法、装置和系统 |
CN115981328A (zh) * | 2022-12-30 | 2023-04-18 | 上海大学 | 一种基于动态运动基元的轨迹规划与调整方法及系统 |
CN117141520A (zh) * | 2023-10-31 | 2023-12-01 | 合肥综合性国家科学中心人工智能研究院(安徽省人工智能实验室) | 一种实时轨迹规划方法、装置和设备 |
Also Published As
Publication number | Publication date |
---|---|
CN117406756A (zh) | 2024-01-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110745136B (zh) | 一种驾驶自适应控制方法 | |
CN110796856B (zh) | 车辆变道意图预测方法及变道意图预测网络的训练方法 | |
US20220212693A1 (en) | Method and apparatus for trajectory prediction, device and storage medium | |
WO2022052406A1 (zh) | 一种自动驾驶训练方法、装置、设备及介质 | |
JP7060625B2 (ja) | 自動運転車において3dcnnネットワークを用いてソリューション推断を行うlidar測位 | |
JP7086111B2 (ja) | 自動運転車のlidar測位に用いられるディープラーニングに基づく特徴抽出方法 | |
US10795360B2 (en) | Utility decomposition with deep corrections | |
CN112888612A (zh) | 自动驾驶车辆规划 | |
CN110834644A (zh) | 一种车辆控制方法、装置、待控制车辆及存储介质 | |
CN113044064B (zh) | 基于元强化学习的车辆自适应的自动驾驶决策方法及系统 | |
CN117406756B (zh) | 一种运动轨迹参数的确定方法、装置、设备和存储介质 | |
Li et al. | Reinforcement learning and deep learning based lateral control for autonomous driving | |
Sefati et al. | Towards tactical behaviour planning under uncertainties for automated vehicles in urban scenarios | |
CN113807460A (zh) | 智能体动作的确定方法和装置、电子设备和介质 | |
CN113255998B (zh) | 基于多智能体强化学习的高速道路无人驾驶车辆编队方法 | |
JP7282271B2 (ja) | 混合自律車両隊列の直接および間接制御 | |
CN114153213A (zh) | 一种基于路径规划的深度强化学习智能车行为决策方法 | |
JP2023523350A (ja) | 乗り物に基づくデータ処理方法、データ処理装置、コンピュータ機器、及びコンピュータプログラム | |
CN116134292A (zh) | 用于性能测试和/或训练自动驾驶车辆规划器的工具 | |
US20210398014A1 (en) | Reinforcement learning based control of imitative policies for autonomous driving | |
CN113928341B (zh) | 一种道路决策方法、系统、设备和介质 | |
EP4160478A1 (en) | Driving decision-making method, device, and chip | |
CN117360552B (zh) | 一种车辆控制方法、装置、设备及可读存储介质 | |
Yang et al. | Deep Reinforcement Learning Lane-Changing Decision Algorithm for Intelligent Vehicles Combining LSTM Trajectory Prediction | |
CN112766310B (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 |