CN110928314A - 一种基于轨迹预测的纯跟踪模型改进算法 - Google Patents

一种基于轨迹预测的纯跟踪模型改进算法 Download PDF

Info

Publication number
CN110928314A
CN110928314A CN201911334450.3A CN201911334450A CN110928314A CN 110928314 A CN110928314 A CN 110928314A CN 201911334450 A CN201911334450 A CN 201911334450A CN 110928314 A CN110928314 A CN 110928314A
Authority
CN
China
Prior art keywords
track
calculating
trajectory
current vehicle
scoring
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.)
Granted
Application number
CN201911334450.3A
Other languages
English (en)
Other versions
CN110928314B (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.)
Suzhou Tracking Intelligent Robot Technology Co Ltd
Original Assignee
Suzhou Tracking Intelligent Robot Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou Tracking Intelligent Robot Technology Co Ltd filed Critical Suzhou Tracking Intelligent Robot Technology Co Ltd
Priority to CN201911334450.3A priority Critical patent/CN110928314B/zh
Publication of CN110928314A publication Critical patent/CN110928314A/zh
Application granted granted Critical
Publication of CN110928314B publication Critical patent/CN110928314B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

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 Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于轨迹预测的纯跟踪模型改进算法，其包括的步骤有：获取当前车子的位姿和当前的局部路径；计算出当前车子的线速度；在采样空间中采样；计算出相应的曲率半径和搜索空间，继而计算出所有的角速度；用参考的线速度和角速度仿真下一时刻的所有前向轨迹；对每条轨迹用预设的规则进行评分。本发明提高了算法整体的跟踪精度；提供了一个评分机制的框架，目前只有三个评分准则，可以考虑不同的约束条件继续添加准则；通过轨迹预测自适应地选取前向距离参数，降低了调试地难度；在避免急转地情况下，加快了车子收敛到路径上地速度。

Description

一种基于轨迹预测的纯跟踪模型改进算法
技术领域
本发明涉及一种移动机器人跟踪路径的算法，尤其涉及一种基于轨迹预测的纯跟踪模型改进算法。
背景技术
导航控制器一直是自动引导车(AGV)系统中必不可少的部分，其中路径跟踪是自主导航领域中一个重要的分支。目前，路径跟踪控制器主要分为三大类：几何学控制器、运动学控制器和动力学控制器。顾名思义，运动学控制器和动力学控制器是分别基于运动学模型和动力学模型设计的，针对不同的地盘模型需要考虑不同的运动约束，如果在高速情形下跟踪复杂路径时，还应考虑车子的动力学模型。
几何控制器因原理简单易懂，不需要复杂的控制理论和高阶数学计算，成为移动机器人导航最常用的方法。目前具有代表性的有卡耐基梅隆大学提出的纯跟踪算法和斯坦福大学提出的斯坦利算法。上述算法利用车子和路径之间的几何关系设计控制率，保证车子到路径的横向误差趋向于零。然而，他们均有各自的却带你：纯跟踪的跟踪效果取决于前向距离的选择，前向距离过长导致车子走捷径，过短会产生震荡；斯坦利算法的收敛速度取决于参数K的选择，K值过大会导致震荡和不稳定，国小则会使得响应速度变得很慢。当然还有一些其他的路径跟踪算法，如经典的PIF控制算法，但其参数整定过于繁琐，一般要配合自适应算法使用。
在合适的前向距离下，纯跟踪算法能够很好并快速地跟踪上预设的路径。但“合适的”前向距离一直困扰着研究者和工程师，因为无法对前向距离建议一个准确的模型，往往需要丰富的调试经验来实际测试，而它优势直接影响跟踪效果的关键参数。单云霄在博士论文“城市无人驾驶规划与控制系统的关键技术研究”中提出两个优化纯跟踪模型以提高精度的方向，(1)优化预瞄(前向距离)策略，寻找更优的目标位置以影响几何关系曲线的生成；(2)使用考虑更多约束的拟合方法以提高拟合精度。他用模糊控制来确定前向距离并用回旋曲线代替圆弧，提高了跟踪的精度。吕文杰在论文“基于纯追踪模型的路径跟踪改进算法”中也使用了模糊控制，用速度和偏航角作为模糊控制器输入，输出前向距离。但隶属度函数和模糊规则的选择具有较强的主观性，需要凭借经验和专家系统。
中国专利申请“CN108673496A一种基于纯追踪模型的工业机器人路径跟踪算法”中利用了与AGV的速度成正比来自适应选取前向距离，该方法在直线上取得不错的效果，但跟踪曲线时误差比较大，特别是转弯半径较小的圆弧。进一步地，当车子刚起步路径横向偏差较大时，由于起步速度较小，会选取一个较小的前向距离，进而纯跟踪模型会计算出一个较大的角速度，这会导致车子有危险的急转动作。
综上所述，在现有技术中，现有的跟踪算法具有跟踪误差大、算法收敛到路径上速度慢、参数选择困难等问题。
针对上述问题，现有技术中还没有解决方案。
发明内容
有鉴于此，为了解决上述问题，本发明提供一种基于轨迹预测的纯跟踪模型改进算法，其包括以下步骤：
步骤一：获取当前车子的位姿和当前的局部路径；
步骤二：计算出当前车子的线速度；
步骤三：在采样空间中采样；
步骤四：计算出相应的曲率半径和搜索空间，继而计算出所有的角速度；
步骤五：用参考的线速度和角速度仿真下一时刻的所有前向轨迹；
步骤六：对每条轨迹用预设的规则进行评分；
步骤七：从轨迹库中选择代价最小的那条轨迹作为最优轨迹。
进一步地，步骤三中所述的采用空间的范围为步骤二中计算出的当前车子的线速度减0.1m/s，到步骤二中计算出的当前出自的线速度加0.1m/s。
进一步地，步骤六中所述的预设的规则包括以下步骤：
6.1将最小转弯半径设为轮间距，将转弯半径小于轮间距的轨迹代价设为无穷大；
6.2过滤掉角速度大于最大值的轨迹，设置代价为无穷大；
6.3给轨迹打分；
6.4获取与当前轨迹相同长度的局部路径；
6.5计算出轨迹的总代价。
本发明提供的一种基于轨迹预测的纯跟踪模型改进算法具有以下优势：
提高了算法整体的跟踪精度；提供了一个评分机制的框架，目前只有三个评分准则，可以考虑不同的约束条件继续添加准则；通过轨迹预测自适应地选取前向距离参数，降低了调试地难度；在避免急转地情况下，加快了车子收敛到路径上地速度。
附图说明
图1所示为本发明的一种实施例中仿真的前向轨迹图。
图2所示为基于轨迹预测的纯跟踪算法的轨迹图。
图3所示为前向距离与速度成线性关系的纯跟踪算法的轨迹图。
图4所示为固定前向距离的原始纯跟踪算法的轨迹图。
图5所示为不同算法比较的局部细节图。
图6所示为本发明提供的一种基于轨迹预测的纯跟踪模型改进算法的流程图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整的说明。
需要说明的是，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例，以下对至少一个示例性实施例的描述实际上仅仅是说明性的，而不应当作为本申请及其应用的限制。
在本发明中，术语“第一”、“第二”仅用于描述的目的，而不能理解为指示或者暗示相对重要性；属于“多个”则指两个或两个以上，除非另有明确的限定。术语“安装”、“相连”、“连接”、“固定”等术语均应作广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接；“相连”可以是直接连接，也可以是通过中间媒介间接相连。术语“上”、“下”等指示的方位或者位置关系为基于附图所示的方式或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或者暗示所指的装置或单元必须具有特定的方向、以特定的方位构造和操作。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
为了解决背景技术中的问题，本发明提供了一种基于轨迹预测的纯跟踪模型改进算法。两轮差速模型下优先选择跟踪圆弧路径，因此选择背景技术中提到的单云霄博士的论文“城市无人驾驶规划与控制系统的关键技术研究”中的其中一个优化纯跟踪模型以提高精度的方向：优化预瞄(前向距离)策略，寻找更优的目标位置以影响几何关系曲线的生成。以此利用轨迹预测来寻找最优的前向距离。这里所述的轨迹预测方法受动态窗口法(DWA)的启发，本发明先利用纯跟踪模型计算出一系列参考速度，然后进行速度采样，相比于DWA直接在速度空间中对称采样，剔除了一部分不必要的轨迹，大大减少了计算量。
如图1所示，车子在轨迹的上侧，发现自身横向偏离路径较大，此时向左转的轨迹是可行区域，而DWA会对称产生向右转的轨迹，增加了计算量。进一步地，DWA利用代价图构建势能场为每条轨迹打分，这样每个循环中都需要维护更新一张代价地图。本发明为了提高实时性，直接用轨迹到路径之间的距离作为代价来代替从代价图中获取。经过两轮差速试验平台测试，改进后的纯跟踪模型具有更高跟踪精度和鲁棒性。
如图6所示，其显示的是本发明提供的一种基于轨迹预测的纯跟踪模型改进算法的流程图，包括以下步骤：
步骤一：获取当前车子的位姿(xc,yc,θc)，根据测试经验，车子行驶速度在小于0.8m/s时，前向距离范围为[0.05,1.5]米。在前向距离的空间中以0.05m为步长进行采样，对每个前向距离用纯跟踪模型公式(1)计算出不同的曲率γi。其中xg是车子与路径之间的横向偏差，Li是第i个车子到预瞄点的欧式距离，其中i表示前向距离样本个数，i＝30。
Figure BDA0002330583070000051
步骤二：用公式(2)计算当前车子的线速度，并将其作为轨迹仿真的参考值。其中vmin和vmax表示速度的最小阈值和最大阈值，dsubgoal_dist表示当前车子与局部目标点之间的距离，dslow_dist为用户设定的减速距离。
Figure BDA0002330583070000052
步骤三：把vr作为参考值，以0.01m/s为步长，在采样空间[vr-0.1,vr+0.1]中采样。当然，vr-0.1和vr+0.1需要满足公式(3-4)。
vr-0.1＝max(vr-0.1,vmin) (3)
vr+0.1＝min(vr+0.1,vmax) (4)
步骤四：有了上述的曲率空间和速度空间后，对第i个曲率γi和第j个速度vj用公式(5)计算所有的角速度wij。
wij＝vjγi (5)
步骤五：根据采样空间中的速度，用公式(6)模拟仿真下一时刻车子所有可能的轨迹。其中Trajk(t)表示第k条轨迹，(v,w)k表示第k组采样速度。如图1所示为车子某个时刻仿真的k条前向轨迹。
Trajk(t)＝∫(v,w)kd(t) (6)
步骤六：对每条轨迹用如下规则进行评分。
6.1.两轮差速模型下，为了避免急转，将最小转弯半径设为轮间距，把转弯半径小于轮间距的轨迹代价设为无穷大。
6.2.过滤掉角速度大于最大值的轨迹，设置代价为无穷大。
6.3.用公式(7-8)给轨迹打分。参考速度是根据纯跟踪模型计算而来，我们更信任它，因此公式(7)表示轨迹线速度越偏离参考线速度，代价越大；我们不希望车子急转，因此公式(8)表示轨迹角速度越大，代价越大。其中α和β为评分准则的权重。
C′k＝α|vk-vr| (7)
C″k＝βwk (8)
6.4.获取与当前轨迹相同长度的局部路径LocalPah，为了保证在低速下也有足够的代价区分轨迹，用线性插值将LocalPah插值到步长为0.025m。用公式(9)给轨迹打分。其中Dclosest表示轨迹上的第q个点到LocalPah上最近一个点的欧式距离，λ为评分准则的权重。
Figure BDA0002330583070000061
6.5.因此，第k条轨迹的总代价为
Ck＝C′k+C″k+C″′k (10)
步骤七：从轨迹库中选择代价最小的那条轨迹作为最优轨迹。
如图2、3、4、5所示为使用固定前向距离、与车子速度成正比和本发明基于轨迹预测的跟踪误差的进行比较。图中两个圆弧的转弯半径分别为0.6m和0.8m，青色的直线为待跟踪路径，红色小方点是车子实际行走的轨迹点。由图可知，本发明的算法无论在直线和曲线上都具有很高的跟踪精度。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (3)

1.一种基于轨迹预测的纯跟踪模型改进算法，其包括以下步骤：
步骤一：获取当前车子的位姿和当前的局部路径；
步骤二：计算出当前车子的线速度；
步骤三：在采样空间中采样；
步骤四：计算出相应的曲率半径和搜索空间，继而计算出所有的角速度；
步骤五：用参考的线速度和角速度仿真下一时刻的所有前向轨迹；
步骤六：对每条轨迹用预设的规则进行评分；
步骤七：从轨迹库中选择代价最小的那条轨迹作为最优轨迹。
2.根据权利要求1所述的基于轨迹预测的纯跟踪模型改进算法，其特征在于，步骤三中所述的采用空间的范围为步骤二中计算出的当前车子的线速度减0.1m/s，到步骤二中计算出的当前出自的线速度加0.1m/s。
3.根据权利要求1所述的基于轨迹预测的纯跟踪模型改进算法，其特征在于，步骤六中所述的预设的规则包括以下步骤：
将最小转弯半径设为轮间距，将转弯半径小于轮间距的轨迹代价设为无穷大；
过滤掉角速度大于最大值的轨迹，设置代价为无穷大；
给轨迹打分；
获取与当前轨迹相同长度的局部路径；
计算出轨迹的总代价。
CN201911334450.3A 2019-12-23 2019-12-23 一种基于轨迹预测的纯跟踪模型改进算法 Active CN110928314B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911334450.3A CN110928314B (zh) 2019-12-23 2019-12-23 一种基于轨迹预测的纯跟踪模型改进算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911334450.3A CN110928314B (zh) 2019-12-23 2019-12-23 一种基于轨迹预测的纯跟踪模型改进算法

Publications (2)

Publication Number Publication Date
CN110928314A true CN110928314A (zh) 2020-03-27
CN110928314B CN110928314B (zh) 2022-11-08

Family

ID=69861618

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911334450.3A Active CN110928314B (zh) 2019-12-23 2019-12-23 一种基于轨迹预测的纯跟踪模型改进算法

Country Status (1)

Country Link
CN (1) CN110928314B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112731932A (zh) * 2020-12-24 2021-04-30 哈尔滨工业大学芜湖机器人产业技术研究院 一种移动机器人的路径跟踪方法
CN112758106A (zh) * 2020-12-31 2021-05-07 东软睿驰汽车技术（沈阳）有限公司 一种车辆运行轨迹预测方法及装置
CN113311845A (zh) * 2021-07-29 2021-08-27 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 基于路径曲率的纯跟踪控制误差补偿方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106020197A (zh) * 2016-06-30 2016-10-12 苏州坤厚自动化科技有限公司 一种基于势能场的机器人路径跟踪算法
CN106515722A (zh) * 2016-11-08 2017-03-22 西华大学 一种垂直泊车轨迹规划方法
CN108673496A (zh) * 2018-05-02 2018-10-19 苏州智伟达机器人科技有限公司 一种基于纯追踪模型的工业机器人路径跟踪方法
CN108681243A (zh) * 2018-05-03 2018-10-19 南京理工大学 一种机器人轨迹跟踪方法
CN109885891A (zh) * 2019-01-24 2019-06-14 中国科学院合肥物质科学研究院 一种智能车gpu并行加速轨迹规划方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106020197A (zh) * 2016-06-30 2016-10-12 苏州坤厚自动化科技有限公司 一种基于势能场的机器人路径跟踪算法
CN106515722A (zh) * 2016-11-08 2017-03-22 西华大学 一种垂直泊车轨迹规划方法
CN108673496A (zh) * 2018-05-02 2018-10-19 苏州智伟达机器人科技有限公司 一种基于纯追踪模型的工业机器人路径跟踪方法
CN108681243A (zh) * 2018-05-03 2018-10-19 南京理工大学 一种机器人轨迹跟踪方法
CN109885891A (zh) * 2019-01-24 2019-06-14 中国科学院合肥物质科学研究院 一种智能车gpu并行加速轨迹规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
DIETER FOX 等: ""The Dynamic Window Approach to Collision Avoidance"", 《IEEE ROBOTICS &AUTOMATION MAGAZINE》 *
何壮壮 等: ""基于D-star和DWA的改进机器人导航方法"", 《电子测量技术》 *
徐保来 等: ""改进动态窗口法的阿克曼移动机器人局部路径规划器"", 《机电工程技术》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112731932A (zh) * 2020-12-24 2021-04-30 哈尔滨工业大学芜湖机器人产业技术研究院 一种移动机器人的路径跟踪方法
CN112758106A (zh) * 2020-12-31 2021-05-07 东软睿驰汽车技术（沈阳）有限公司 一种车辆运行轨迹预测方法及装置
CN112758106B (zh) * 2020-12-31 2022-06-03 东软睿驰汽车技术（沈阳）有限公司 一种车辆运行轨迹预测方法及装置
CN113311845A (zh) * 2021-07-29 2021-08-27 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 基于路径曲率的纯跟踪控制误差补偿方法及装置
CN113311845B (zh) * 2021-07-29 2021-11-16 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) 基于路径曲率的纯跟踪控制误差补偿方法及装置

Also Published As

Publication number Publication date
CN110928314B (zh) 2022-11-08

Similar Documents

Publication Publication Date Title
CN110928314B (zh) 一种基于轨迹预测的纯跟踪模型改进算法
CN107702716B (zh) 一种无人驾驶路径规划方法、系统和装置
CN110568758B (zh) 一种自动驾驶汽车的参数自适应横向运动lqr控制方法
CN104881025B (zh) 一种地下矿用车辆的反应式导航控制方法
CN110703763A (zh) 无人车路径跟踪及避障方法
CN113204236A (zh) 一种智能体路径跟踪控制方法
CN109991974A (zh) 自动驾驶路径跟随方法、装置及控制设备
CN112857385B (zh) 一种基于非均匀栅格模型的快速无人车局部路径规划方法
KR20160081906A (ko) 적응 퍼지 추정기 기반의 필터링 백스테핑 선박 운동 제어 시스템
CN110471289A (zh) 一种基于视觉导航移动设备的自适应路径跟踪方法及系统
CN111538328A (zh) 一种用于自主驾驶车辆避障轨迹规划与跟踪控制的优先级分层预测控制方法
CN110297500A (zh) 一种给定多航路点下无人机的连续航迹规划方法
CN113538973B (zh) 一种基于改进粒子群算法的船舶自动避碰方法
CN113093804B (zh) 基于反演滑模控制的无人艇编队控制方法和控制系统
CN109606364B (zh) 一种分层式自学习可拓神经网络车道保持控制方法
CN114384916A (zh) 一种越野车路径规划的自适应决策方法及系统
Farag Model-predictive-control complex-path tracking for self-driving cars
CN116430856A (zh) 一种基于变前视距离los算法的船舶航迹控制方法
CN113625703B (zh) 一种动态路径跟踪方法
Hegedüs et al. Hybrid trajectory planning for autonomous vehicles using neural networks
CN114545971A (zh) 一种通讯约束下多智能体分布式可飞路径规划方法、系统、计算机设备及介质
CN113204248A (zh) 基于决策变量与自适应算法的多无人机航迹协同方法
CN111857112A (zh) 一种汽车局部路径规划方法及电子设备
Zhou et al. A Fast and Smooth Planning Framework for Autonomous Mobile Robot in Complex Environments
Bin et al. Research on AGV steering control algorithm based on improving pure tracking model

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