CN113771034B - 一种基于模型置信度和高斯过程的机器人轨迹预测方法 - Google Patents
一种基于模型置信度和高斯过程的机器人轨迹预测方法 Download PDFInfo
- Publication number
- CN113771034B CN113771034B CN202111089958.9A CN202111089958A CN113771034B CN 113771034 B CN113771034 B CN 113771034B CN 202111089958 A CN202111089958 A CN 202111089958A CN 113771034 B CN113771034 B CN 113771034B
- Authority
- CN
- China
- Prior art keywords
- robot
- model
- path
- probability
- motion
- 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 89
- 230000008569 process Effects 0.000 title claims abstract description 59
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 31
- 238000012549 training Methods 0.000 claims abstract description 20
- 230000007774 longterm Effects 0.000 claims abstract description 6
- 238000005070 sampling Methods 0.000 claims description 27
- 238000009826 distribution Methods 0.000 claims description 26
- 238000013507 mapping Methods 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 11
- 230000003416 augmentation Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 7
- 238000005457 optimization Methods 0.000 claims description 6
- KDYFGRWQOYBRFD-UHFFFAOYSA-N succinic acid Chemical compound OC(=O)CCC(O)=O KDYFGRWQOYBRFD-UHFFFAOYSA-N 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 4
- 238000007476 Maximum Likelihood Methods 0.000 claims description 3
- 230000009471 action Effects 0.000 claims description 3
- 238000002939 conjugate gradient method Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 2
- 230000003190 augmentative effect Effects 0.000 abstract description 5
- 238000013473 artificial intelligence Methods 0.000 abstract description 2
- 230000001419 dependent effect Effects 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 9
- 238000004088 simulation Methods 0.000 description 5
- 238000012360 testing method Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000006073 displacement reaction Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 206010039203 Road traffic accident Diseases 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012502 risk assessment Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
-
- 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)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于模型置信度和高斯过程的机器人轨迹预测方法，属于目标跟踪与人工智能领域。本发明旨在解决现有的基于学习的方法严重依赖于训练数据集，在数据稀疏区域轨迹预测精度较低的问题，首先建立基于模型置信度的机器人决策概率模型，然后利用模型生成的概率路径获得增广数据集，并利用增广数据集训练高斯过程模型，结合高斯过程和RRT设计机器人长期运动轨迹预测算法，提高了数据稀疏情况下机器人长期运动轨迹预测精度。
Description
技术领域
本发明属于目标跟踪与人工智能领域，具体涉及一种基于模型置信度和高斯过程的机器人轨迹预测方法。
背景技术
随着科技的发展，机器人在工业制造和人们生活中的应用逐渐广泛，机器人是改善人类生活方式的重要切入点，大力发展机器人产业，对于推动工业转型升级，改善人民生活水平具有重要意义。机器人在执行任务的过程中首先需要考虑避障的问题，路径规划是机器人能够安全高效工作的重要环节，而轨迹预测作为威胁评估系统的重要组成部分是机器人规划领域的关键研究问题之一。轨迹预测可应用在无人驾驶车辆的防撞检测系统，通过在系统规划器中加入对车辆未来长时间运动轨迹预测，从而合理规划路径引导车辆避开可能会发生交通堵塞的区域。轨迹预测也可以在交叉路口进行风险评估，通过对移动物体进行未来长期运动轨迹预测，先进的交通监控系统可以对可能发生的碰撞作出警报，减少交通事故发生。
在现有的轨迹预测算法中，基于学习的方法根据训练数据集逼近目标的动力学函数，该方法通过观察到的运动轨迹学习对应的统计学行为模式进而预测目标未来运动轨迹。其中，基于高斯过程的方法通过已知的训练集进行模型学习，学习得到机器人位置与轨迹导数分布的映射关系，然后利用该映射关系预测机器人未来运动轨迹。高斯过程模型对于无序含噪声观测量有很强的鲁棒性，适合对非等距离散时间序列采样轨迹点进行建模描述。但是，基于学习的方法严重依赖于训练的数据集，需要大量数据样本，故在数据稀疏区域机器人未来长期运动轨迹预测精度较低。
发明内容
要解决的技术问题
为了解决现有的基于学习的方法严重依赖于训练的数据集，在数据稀疏区域轨迹预测精度较低的问题，本发明提供一种基于模型置信度和高斯过程的机器人轨迹预测方法，首先建立基于模型置信度的机器人决策概率模型，然后利用模型生成的概率路径获得增广数据集，并利用增广数据集训练高斯过程模型，结合高斯过程和RRT设计机器人长期运动轨迹预测算法，提高了数据稀疏情况下机器人长期运动轨迹预测精度。
技术方案
一种基于模型置信度和高斯过程的机器人轨迹预测方法，其特征在于步骤如下：
步骤1：生成环境栅格地图，设计针对固定障碍物的机器人移动规则；
步骤2：建立基于模型置信度的机器人决策概率模型：
步骤2.1：计算机器人在当前位置采取不同决策后的状态-动作值，即Q值；
步骤2.2：根据Q值计算机器人未来运动状态的概率分布；
步骤2.3：更新模型置信度；
步骤3：利用概率模型计算机器人未来多时间步长状态的概率分布，通过模拟传感器量测信息的更新，得到起始点到目标点的概率路径；
步骤4：在最大模型置信度路径集合中选择概率最大的三条路径作为训练集的参考运动路径；
步骤5：以三条参考运动路径为基准，通过双轮差速小车模型做参考运动路径跟踪构建增广数据集；
步骤6：利用增广数据集训练高斯过程模型，得到模型的超参数集合；
步骤7：模拟传感器量测信息，利用训练好的模型预测轮式机器人未来长时间运动轨迹：
步骤7.1：初始化各运动模式的先验概率和随机树，随机树的根节点为机器人当前位置；
步骤7.2：采用高斯过程和RRT算法扩展随机树，直到随机树生成的预测轨迹达到预定长度N；
步骤8：输出机器人当前观测位置处未来多时间步长的运动轨迹。
本发明进一步的技术方案为：所述步骤1中针对固定障碍物的机器人移动规则为：如果两相邻栅格的中心点之间的连接路径接触到障碍物栅格，则此两个栅格之间的连接路径不能直接生成。
本发明进一步的技术方案为：所述步骤2.1中计算机器人在当前位置采取不同决策后的状态-动作值指的是：根据机器人当前所在位置，在无碰撞的情况下，向外探索一步有八种动作，产生八种状态，分别计算该状态下Q值，计算过程如下：
Q(xr,ur；xg)＝-vrΔt-||xr+vrΔt[cosur,sinur]T-xg||2 (1)
其中，xr为机器人当前位置，vr为机器人速度，Δt为采样周期，ur为控制输入，xg为目标位置。
本发明进一步的技术方案为：所述步骤2.2中的根据Q值计算机器人未来运动状态的概率分布指的是：设定机器人对模型的信任程度β，也可将其视为对Q值预测准确性的度量；通过Kolmogorov前向方程递归地得到机器人k步预测状态的概率分布，令θ＝xg，计算过程如下：
剔除每一步的小概率状态，再进行归一化处理，选取2个概率最大的状态继续进行扩展，得到机器人三步预测状态的概率分布。
本发明进一步的技术方案为：所述步骤2.3中的更新模型置信度指的是：对五组β初始化模型置信度
量测信息更新后，采用贝叶斯规则更新模型置信度
的更新过程使用"ε-static"转移模型，ε表示上一步的观测对当前模型置信度的影响，计算过程如下：
本发明进一步的技术方案为：所述步骤5中，以三条参考运动路径为基准，通过双轮差速小车模型做参考运动路径跟踪构建增广数据集，包括如下步骤：
步骤5.1：创建膨胀地图以考虑小车的尺寸，确保小车不会与障碍物发生碰撞；
步骤5.2：配置双轮差速模型参数，指定小车运动路径，使用路径跟踪算法控制小车从起始位置运动到目标位置，得到小车运动轨迹集；
步骤5.3：将轨迹集中的位置与速度的全部序列信息做处理作为高斯过程的训练集。
本发明进一步的技术方案为：所述步骤6的实现方法如下：将运动模式定义为机器人位置与其在该位置处轨迹导数的映射关系，建立基于高斯过程的运动模式模型得到该映射关系；假设机器人当前位置(xt,yt)，根据映射关系得到该位置处的轨迹导数
预测下一时间步长的位置(xt+1,yt+1)为
通过极大似然估计法学习高斯过程模型的超参数集合，此时参数选择问题转化为优化问题，采用共轭梯度法求解优化问题，每种运动模式分别得到两组超参数集合，即x方向和y方向。
本发明进一步的技术方案为：所述步骤7.2中扩展随机树的实现方法如下：首先根据学习到的模型得到该位置处的轨迹导数高斯后验分布，对其在空间中采样；设定启发函数为采样点间的欧几里得距离，从“时间容器”tGP-Δt中选距离采样点最近的三个点作为下一步待扩展节点；然后将待扩展节点与采样点间以直线作为参考路径，使用路径跟踪算法产生路径的中间节点并进行碰撞检测，从而保证预测路径的可行性且满足动力学约束；接下来将满足约束的可行路径的中间节点添加到随机树中，新的采样点添加到“时间容器”tGP中；每一步扩展过程中统计连接失败路径数nfail和连接成功路径数nsucc，当nsucc达到阈值时，tGP增加，重复以上扩展步骤，采样下一个时间步长；扩展过程中，当nfail达到阈值时，放弃该随机树的扩展；最终，生成不同运动模式下根节点位于当前观测位置处的随机树，即为该位置处的长期运动轨迹预测。
有益效果
本发明提出的一种基于模型置信度和高斯过程的机器人轨迹预测方法，通过基于模型置信度的概率模型生成机器人从起始点运动到目标点的概率路径，在最大模型置信度路径集合中选取概率最大的三条路径作为训练集的参考路径，以参考路径为基准获得增广数据集，然后利用增广数据集训练高斯过程模型，结合高斯过程模型和RRT算法预测机器人未来运动轨迹，提高了位于数据稀疏区域的测试集的轨迹预测精度。本发明在仿真数据集下的测试结果为：在满足实时性的同时，相较于基于高斯过程的轨迹预测算法而言，预测精度提高了36.44％。
本发明方法相比于现有技术，具有以下技术特征：1.生成环境栅格地图；2.建立基于模型置信度的机器人决策概率模型；3.利用模型生成的概率路径构建增广数据集，并利用增广数据集训练高斯过程模型；4.结合高斯过程和RRT设计机器人长时间轨迹预测算法。本发明由于采用了上述技术特征，在数据稀疏的场景中，通过对数据集进行增广，尽可能产生更多样本，即使在偏离数据集的测试路径中也能达到较好的轨迹预测精度，增强了场景的可推广性。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例的流程图；
图2为带有固定障碍物的栅格地图；
图3为针对固定障碍物的机器人移动规则；
图4为预测状态概率分布图：(1)为第一种运动模式；(2)为第二种运动模式；
图5为两种运动模式下训练轨迹图；
图6为运动轨迹预测结果图：(1)为t＝0s时轨迹预测结果；(2)为t＝3s时轨迹预测结果；(3)为本算法与基于GP的算法在t＝3s时未来八步平均预测位置误差对比图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
实验采用仿真数据集对本算法进行测试，实验所用计算机的配置为：CPU为Intel(R)Core(TM)i5-8400，主频为2.80Ghz，内存为8GB，系统为Windows 10。
图1为本发明实施例的流程图。如图1所示，本发明所提出的一种基于模型置信度和高斯过程的机器人轨迹预测方法，包括以下步骤：
步骤1：生成环境栅格地图，设计针对固定障碍物的机器人移动规则；
仿真实验中，环境栅格地图如图2所示，将地图分解成小网格，栅格地图大小设置为1000cm×1000cm，在环境地图中设置三个固定障碍物。
图3为机器人移动规则。配置空间(C)中，使用网格包围障碍物，忽略障碍物形状，障碍物包围空间为Cobst，满足C＝Cfree∪Cobst且
其中Cfree为机器人可行区域。机器人在可行区域的移动规则为：如果两相邻栅格的中心点之间的连接路径接触到障碍物栅格，则此两个栅格之间的连接路径不能直接生成。如图3中，无法生成xi→xj的连接路径，从xi到达xj可通过路径xi→xa→xj实现。
步骤2：建立基于模型置信度的机器人决策概率模型，包括如下步骤：
步骤2.1：计算机器人在当前位置采取不同决策后的状态-动作值(Q值)；
设定机器人的起始位置xs＝[4.25,8.75]T，目标位置1为xg1＝[2.25,1.75]T，目标位置2为xg2＝[6.75,1.75]T，在Δt时间内，机器人在位置xr处执行动作ur的Q值为：
Q(xr,ur；xg)＝-vrΔt-||xr+vrΔt[cosur,sinur]T-xg||2 (1)
其中，机器人速度vr＝1m/s，采样周期Δt＝0.1s。
步骤2.2：根据Q值计算机器人未来运动状态的概率分布；
设定机器人对概率模型的信任程度β＝[0.05,0.1,1,5,10]，也可将其视为对Q值预测准确性的度量。通过Kolmogorov前向方程递归地得到机器人k步预测状态的概率分布，令θ＝xg，计算过程如下：
其中，
为机器人运动学模型。
剔除每一步的小概率状态
再进行归一化处理，从中选取2个概率最大的状态继续进行扩展，得到机器人三步预测状态的概率分布。
步骤2.3：更新模型置信度。
对五组β分别初始化模型置信度
传感器量测信息更新后，采用贝叶斯规则更新模型置信度
的更新过程使用"ε-static"转移模型，设定ε＝0.2表示上一步的观测对当前模型置信度的影响，计算过程如下：
步骤3：利用概率模型计算机器人未来多时间步长状态的概率分布，通过模拟传感器量测信息的更新，从而得到机器人从起始点到目标点的概率路径；
传感器量测信息更新周期为0.1s，通过量测信息的不断更新，分别得到机器人从起始位置到达两个目标位置的概率路径，如图4所示。图4-1中，机器人从起始位置xs到目标位置xg1为第一种运动模式b1，图4-2中，从起始位置xs到目标位置xg2为第二种运动模式b2。
步骤4：在最大模型置信度路径集合中选择概率最大的三条路径作为训练集的参考运动路径；
步骤5：以三条参考运动路径为基准，通过双轮差速小车模型做参考运动路径跟踪构建增广数据集，包括如下步骤：
步骤5.1：创建膨胀地图以考虑小车的尺寸，从而确保小车不会与障碍物发生碰撞；
步骤5.2：配置双轮差速模型参数，指定小车运动路径，使用路径跟踪算法控制小车从起始位置运动到目标位置，得到小车运动轨迹集；
双轮差速车辆运动模型如下，Xk＝[xk,yk,θk]T为机器人在k时刻的位置和航向角，ω和v分别为两轮的角速度和线速度，Wk-1为过程噪声。
小车的参考运动路径为步骤4得到的路径，使用路径跟踪算法生成小车从起始位置到目标位置的运动轨迹，跟踪算法以小车后轴为切点，小车纵向车身为切线，通过控制前轮转角θ驱动小车沿一条经过路径点的圆弧行驶，控制量的计算方式如下：
其中，设定小车轴距L＝0.3m，前视距离系数k＝0.1，α为当前车身姿态与目标路径点的夹角，采样频率f＝100Hz。
步骤5.3：将轨迹集中的位置与速度的全部序列信息做处理作为高斯过程的训练集。
基于三条参考路径生成30条运动轨迹(15条为模式1，15条为模式2)用于训练高斯过程模型。得到的训练轨迹集如图5所示。
步骤6：通过增广数据集训练高斯过程模型，得到模型的超参数集合；
将运动模式定义为机器人位置与其在该位置处轨迹导数的映射关系，通过建立基于高斯过程的运动模式模型得到该映射关系。假设机器人当前位置(xt,yt)，根据映射关系得到该位置处的轨迹导数
预测下一时间步长的位置为：
在多运动模式下，根据观测到的运动轨迹ti，运动模式bj的后验概率为：
p(bj|ti)∝p(ti|bj)p(bj) (10)
p(bj)为运动模式bj的先验概率，p(ti|bj)为在运动模式bj下轨迹ti的概率。Li为轨迹ti的长度，zk表示将轨迹分配为运动模式j，{tk：zk＝j}为属于运动模式j的轨迹集，
和
分别为运动模式j的高斯过程(GP)模型超参数。
运动模式的GP由一组均值函数和协方差函数确定。x轴方向和y轴方向的均值函数分别为
x轴方向的协方差函数为Kx(x,y,x',y')，描述了(x,y)和(x',y')两点处轨迹导数间的相关性，选择如下平方指数协方差函数：
其中，δ(x,y,x',y')为克罗内克函数，如果x＝x'且y＝y'，则δ(x,y,x',y')＝1，其他则为0。ωx和ωy为方差尺度参数，它们对数据分布范围进行归一化。σn为样本点自身标准差，由量测噪声产生。σn和σx的比例为协方差函数受噪声和周围点影响的相对关系。ωx、ωy、σx、σn为运动模式bj的高斯过程超参数集合，一种运动模式对应一组超参数集合，一组超参数集合包含两个方向的参数，x方向记为
y方向为
可通过极大似然估计法进行模型学习，即根据给定的训练数据集
为模型选取最优参数集合。超参数集合θ的后验估计为：
当条件概率p(θ|y,x)最大时，此时对θ的估值为最大后验估计。由于p(θ)为先验概率无法准确获得，故采用均匀分布作为参数集合的先验分布。可得：
argmax(p(θ|y,x))≈argmax(p(y|x,θ)) (14)
p(y|x,θ)的对数似然形式为：
将参数选择问题转化为优化问题，采用共轭梯度法求解优化问题，能够得到不同运动模式下的高斯过程模型超参数集合。
基于建立的高斯过程模型，得到小车所在位置与该位置处轨迹导数的映射关系。根据小车当前位置，可估计出该点处轨迹导数的后验分布，利用该分布得到小车下一时间步长的位置。给定小车当前位置(xt,yt)和运动模式bj，通过下式采样得到N时间步长后小车的概率轨迹：
步骤7：模拟传感器量测信息，利用训练好的模型预测轮式机器人未来长时间运动轨迹，包括如下步骤：
步骤7.1：初始化各运动模式的先验概率和随机树，设定随机树的根节点为机器人当前位置；
设定两种运动模式的先验概率为p(b1)＝p(b2)＝0.5。机器人当前位置为(xt,yt)，高斯过程采样时间Δt＝1s，随机树
扩展时间步长tGP初始化为t+Δt。
步骤7.2：采用高斯过程和RRT算法扩展随机树，直到随机树生成的预测轨迹达到预定长度N；
利用训练好的高斯过程模型得到该位置处的轨迹导数高斯后验分布，对其在空间中采样，生成采样点。设定启发函数为采样点间的欧几里得距离，然后从“时间容器”tGP-Δt中选择距离采样点最近的三个点作为待扩展节点。待扩展节点与采样点间以直线作为参考路径，并使用路径跟踪算法产生路径的中间节点，接下来进行碰撞检测，保证路径的可行性且满足动力学约束。将满足约束的可行路径的中间节点添加到随机树中，并把新的采样点添加到“时间容器”tGP中。统计每一时间步长连接失败路径数nfail和连接成功路径数nsucc，当nsucc＝30达到阈值时，tGP增加，重复以上步骤，对下一时间步长采样,直到随机树生成的预测轨迹达到预定长度N＝8。当nfail＝150达到阈值时，放弃该随机树的扩展。最终，生成不同运动模式下根节点位于当前观测位置处的随机树。
扩展过程中，当小车的实际观测位置偏离该运动模式高斯过程模型预测结果时，扩展树仍然按照该模式扩展，导致轨迹预测精度下降。算法中引入动态调整可行性概率
在随机树扩展中，当运动模式bj产生的采样点的失败路径数达到阈值，则可行性概率很低，此时将运动模式bj从可采取运动模式中剔除，并对其余运动模式的概率作归一化处理。通过引入此动态调整策略，预测轨迹分布能快速收敛到真实运动模式。
步骤8：输出机器人当前观测位置处未来多时间步长的运动轨迹。
得到轮式机器人当前观测位置处未来八个时间步长的可行运动轨迹。
接下来，根据得到的预测轨迹，对本算法的性能进行评价分析。图6为本算法的仿真实验结果，图6-1为t＝0s时轨迹预测结果，图6-2为t＝3s时轨迹预测结果。在预测精度分析的过程中，采用了平均位移误差(Average Displacement Error,ADE)作为评价指标来评估本算法的预测精度，ADE是真实值与预测值之间的平均欧式距离。
将本算法与基于GP的算法选100组测试轨迹集做对比实验，图6-3为本算法与基于GP的算法在t＝3s时未来八步平均预测位置误差对比图，为了更好的对比预测精度，将对比实验结果汇总如下表：
由上表可知，在仿真实验中，本算法在每一时刻的平均位移误差都小于基于GP的算法，提高了约36.44％的预测精度。
本算法进行机器人未来八步轨迹预测的平均处理时间为0.45s，对于采样时间为1s的高斯过程模型而言，本算法能够满足实时性要求。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。
Claims (3)
1.一种基于模型置信度和高斯过程的机器人轨迹预测方法，其特征在于步骤如下：
步骤1：生成环境栅格地图，设计针对固定障碍物的机器人移动规则；
步骤2：建立基于模型置信度的机器人决策概率模型：
步骤2.1：计算机器人在当前位置采取不同决策后的状态-动作值，即Q值；
根据机器人当前所在位置，在无碰撞的情况下，向外探索一步有八种动作，产生八种状态，分别计算该状态下Q值，计算过程如下：
Q(xr,ur；xg)＝-vrΔt-||xr+vrΔt[cosur,sinur]T-xg||2 (1)
其中，xr为机器人当前位置，vr为机器人速度，Δt为采样周期，ur为控制输入，xg为目标位置；
步骤2.2：根据Q值计算机器人未来运动状态的概率分布；
设定机器人对模型的信任程度β，也可将其视为对Q值预测准确性的度量；通过Kolmogorov前向方程递归地得到机器人k步预测状态的概率分布，令θ=xg，计算过程如下：
剔除每一步的小概率状态，再进行归一化处理，选取2个概率最大的状态继续进行扩展，得到机器人三步预测状态的概率分布；
步骤2.3：更新模型置信度；
对五组β初始化模型置信度
量测信息更新后，采用贝叶斯规则更新模型置信度
的更新过程使用"ε-static"转移模型，ε表示上一步的观测对当前模型置信度的影响，计算过程如下：
步骤3：利用概率模型计算机器人未来多时间步长状态的概率分布，通过模拟传感器量测信息的更新，得到起始点到目标点的概率路径；
步骤4：在最大模型置信度路径集合中选择概率最大的三条路径作为训练集的参考运动路径；
步骤5：以三条参考运动路径为基准，通过双轮差速小车模型做参考运动路径跟踪构建增广数据集；
步骤6：利用增广数据集训练高斯过程模型，得到模型的超参数集合；
将运动模式定义为机器人位置与其在该位置处轨迹导数的映射关系，建立基于高斯过程的运动模式模型得到该映射关系；假设机器人当前位置(xt,yt)，根据映射关系得到该位置处的轨迹导数
预测下一时间步长的位置(xt+1,yt+1)为
通过极大似然估计法学习高斯过程模型的超参数集合，此时参数选择问题转化为优化问题，采用共轭梯度法求解优化问题，每种运动模式分别得到两组超参数集合，即x方向和y方向；
步骤7：模拟传感器量测信息，利用训练好的模型预测轮式机器人未来长时间运动轨迹：
步骤7.1：初始化各运动模式的先验概率和随机树，随机树的根节点为机器人当前位置；
步骤7.2：采用高斯过程和RRT算法扩展随机树，直到随机树生成的预测轨迹达到预定长度N；
首先根据学习到的模型得到该位置处的轨迹导数高斯后验分布，对其在空间中采样；设定启发函数为采样点间的欧几里得距离，从“时间容器”tGP-Δt中选距离采样点最近的三个点作为下一步待扩展节点；然后将待扩展节点与采样点间以直线作为参考路径，使用路径跟踪算法产生路径的中间节点并进行碰撞检测，从而保证预测路径的可行性且满足动力学约束；接下来将满足约束的可行路径的中间节点添加到随机树中，新的采样点添加到“时间容器”tGP中；每一步扩展过程中统计连接失败路径数nfail和连接成功路径数nsucc，当nsucc达到阈值时，tGP增加，重复以上扩展步骤，采样下一个时间步长；扩展过程中，当nfail达到阈值时，放弃该随机树的扩展；最终，生成不同运动模式下根节点位于当前观测位置处的随机树，即为该位置处的长期运动轨迹预测；
步骤8：输出机器人当前观测位置处未来多时间步长的运动轨迹。
2.根据权利要求1所述的一种基于模型置信度和高斯过程的机器人轨迹预测方法，其特征在于：所述步骤1中针对固定障碍物的机器人移动规则为：如果两相邻栅格的中心点之间的连接路径接触到障碍物栅格，则此两个栅格之间的连接路径不能直接生成。
3.根据权利要求1所述的一种基于模型置信度和高斯过程的机器人轨迹预测方法，其特征在于：所述步骤5中，以三条参考运动路径为基准，通过双轮差速小车模型做参考运动路径跟踪构建增广数据集，包括如下步骤：
步骤5.1：创建膨胀地图以考虑小车的尺寸，确保小车不会与障碍物发生碰撞；
步骤5.2：配置双轮差速模型参数，指定小车运动路径，使用路径跟踪算法控制小车从起始位置运动到目标位置，得到小车运动轨迹集；
步骤5.3：将轨迹集中的位置与速度的全部序列信息做处理作为高斯过程的训练集。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111089958.9A CN113771034B (zh) | 2021-09-17 | 2021-09-17 | 一种基于模型置信度和高斯过程的机器人轨迹预测方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111089958.9A CN113771034B (zh) | 2021-09-17 | 2021-09-17 | 一种基于模型置信度和高斯过程的机器人轨迹预测方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113771034A CN113771034A (zh) | 2021-12-10 |
| CN113771034B true CN113771034B (zh) | 2023-05-05 |
Family
ID=78851672
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111089958.9A Active CN113771034B (zh) | 2021-09-17 | 2021-09-17 | 一种基于模型置信度和高斯过程的机器人轨迹预测方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113771034B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114970714B (zh) * | 2022-05-26 | 2024-05-03 | 哈尔滨工业大学 | 一种考虑移动目标不确定行为方式的轨迹预测方法及系统 |
| CN116520681B (zh) * | 2023-07-03 | 2023-09-05 | 小舟科技有限公司 | 轮椅自动行驶控制方法及装置、自动行驶轮椅 |
| CN116985142B (zh) * | 2023-09-25 | 2023-12-08 | 北京航空航天大学 | 一种机器人运动规划方法、装置和机器人 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190184561A1 (en) * | 2017-12-15 | 2019-06-20 | The Regents Of The University Of California | Machine Learning based Fixed-Time Optimal Path Generation |
| CN111324987B (zh) * | 2020-02-20 | 2022-07-08 | 西北工业大学 | 一种基于稀疏高斯过程的目标运动预测方法 |
| CN112318509B (zh) * | 2020-10-30 | 2022-04-29 | 东南大学 | 一种空间机器人高斯过程轨迹跟踪控制方法 |
-
2021
- 2021-09-17 CN CN202111089958.9A patent/CN113771034B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113771034A (zh) | 2021-12-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113771034B (zh) | 一种基于模型置信度和高斯过程的机器人轨迹预测方法 | |
| Bansal et al. | Deepreach: A deep learning approach to high-dimensional reachability | |
| Huang et al. | Driving behavior modeling using naturalistic human driving data with inverse reinforcement learning | |
| Salzmann et al. | Trajectron++: Dynamically-feasible trajectory forecasting with heterogeneous data | |
| Lefkopoulos et al. | Interaction-aware motion prediction for autonomous driving: A multiple model kalman filtering scheme | |
| CN112356830B (zh) | 一种基于模型强化学习的智能泊车方法 | |
| Rosolia et al. | Autonomous racing using learning model predictive control | |
| Huang et al. | Differentiable integrated motion prediction and planning with learnable cost function for autonomous driving | |
| Zhao et al. | A novel direct trajectory planning approach based on generative adversarial networks and rapidly-exploring random tree | |
| Motlagh et al. | An expert fuzzy cognitive map for reactive navigation of mobile robots | |
| Zhang et al. | Integrating kinematics and environment context into deep inverse reinforcement learning for predicting off-road vehicle trajectories | |
| Wang et al. | A survey of learning‐based robot motion planning | |
| CN109597425B (zh) | 基于强化学习的无人机导航和避障方法 | |
| CN109885883A (zh) | 一种基于gk聚类算法模型预测的无人车横向运动的控制方法 | |
| CN116804879B (zh) | 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法 | |
| Sefati et al. | Towards tactical behaviour planning under uncertainties for automated vehicles in urban scenarios | |
| Vallon et al. | Data-driven hierarchical predictive learning in unknown environments | |
| Nozari et al. | Active inference integrated with imitation learning for autonomous driving | |
| Lee et al. | Recurrent neuro fuzzy control design for tracking of mobile robots via hybrid algorithm | |
| Alcalde et al. | DA-SLAM: Deep Active SLAM based on Deep Reinforcement Learning | |
| Fan et al. | Spatiotemporal path tracking via deep reinforcement learning of robot for manufacturing internal logistics | |
| Xu et al. | Learning strategy for continuous robot visual control: A multi-objective perspective | |
| Guo et al. | Optimal navigation for AGVs: A soft actor–critic-based reinforcement learning approach with composite auxiliary rewards | |
| Zhang et al. | Enhancing Multi-UAV Reconnaissance and Search Through Double Critic DDPG With Belief Probability Maps | |
| Guo et al. | A survey of trajectory planning methods for autonomous driving—Part I: Unstructured scenarios |
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 |