CN113625703B - 一种动态路径跟踪方法 - Google Patents

一种动态路径跟踪方法 Download PDF

Info

Publication number
CN113625703B
CN113625703B CN202110737298.4A CN202110737298A CN113625703B CN 113625703 B CN113625703 B CN 113625703B CN 202110737298 A CN202110737298 A CN 202110737298A CN 113625703 B CN113625703 B CN 113625703B
Authority
CN
China
Prior art keywords
speed
sampling
motion parameters
point
track
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
CN202110737298.4A
Other languages
English (en)
Other versions
CN113625703A (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 Agv Robot Co ltd
Original Assignee
Suzhou Agv Robot 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 Agv Robot Co ltd filed Critical Suzhou Agv Robot Co ltd
Priority to CN202110737298.4A priority Critical patent/CN113625703B/zh
Publication of CN113625703A publication Critical patent/CN113625703A/zh
Application granted granted Critical
Publication of CN113625703B publication Critical patent/CN113625703B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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)
  • Feedback Control In General (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种动态路径跟踪方法，包括：利用B样条生成全局路径；在每一个控制周期内，获取速度空间；在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；对于每一组运动参数，模拟出未来的一小段圆弧轨迹；通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；选取最优路线，抵达目标点。基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。

Description

一种动态路径跟踪方法
技术领域
本发明属于移动机器人导航领域，涉及一种动态路径跟踪方法。
背景技术
可穿越锚定路径跟踪方法(英文：Traversability-anchored Dynamic PathFollowing，简称：TADPF)是一种AGV自主导航中的路径跟踪方法。动态窗口法(英文：Dynamic Window Approach，简称：DWA)是一种局部路径规划方法，都是将路径跟踪与局部路径规划相结合的基于采样和预测的方法。而目前的AGV在使用这两种方法进行路径规划时，路径跟踪精度不高。
发明内容
本发明目的是：提供一种基于采样和预测的动态路径跟踪方法，提高路径跟踪精度。
本发明的技术方案是：一种动态路径跟踪方法，包括：
利用B样条生成全局路径；
在每一个控制周期内，获取速度空间；
在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；
对于每一组运动参数，模拟出未来的一小段圆弧轨迹；
通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；
选取最优路线，抵达目标点。
其进一步的技术方案是：所述评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。
其进一步的技术方案是：所述对线速度和曲率或角速度进行采样，包括：
在速度空间内采用平均采样的方法，采样步长由采样数Nv_sam和Nk_sam决定，线速度和曲率的样本计算方法如下：
其进一步的技术方案是：所述对于每一组运动参数，模拟出未来的一小段圆弧轨迹，包括：
对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针；
将圆弧轨迹上平均分成Np_sam段，每一段对应的轨迹点坐标的计算方法如下：
当k不为0时：
当k为0时：
其中，(x0,y0,θ0)为从定位程序获取的当前位置坐标，tsam表示采样周期。
其进一步的技术方案是：在控制周期很短的情况下，轨迹模拟通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点。
其进一步的技术方案是：所述通过评价函数对所有轨迹进行评价，包括：
针对与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量进行成本考核；
对各项成本进行归一化处理，各项成本的归一化处理如下：
在叠加过程中，首先对每一个控制周期内的Np_sum段的采样点与B样条上最近点的距离偏差和角度偏差进行累加：
然后将结果按采样点平均，并对其余各项成本进行叠加：
参考轨迹由均匀三阶B样条给出，由Ax,Bx,Cx,Dx,Ay,By,Cy,Dy,八个系数决定，参数方程为：
在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currentS；在计算距采样点最近点的坐标时，从currentS开始采用逐步搜索的方法；
其中，为各项成本系数，决定各项因素的权重。
其进一步的技术方案是：所述选取最优路线，抵达目标点，包括：
得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机，其中，
当currentS接近1且未到达最后一段路径时，Ax,Bx,Cx,Dx,Ay,By,Cy,Dy切换为下一段的系数；
当currentS接近1且已在最后一段路径时，进入最终抵达阶段；
在最终抵达阶段的控制周期内，若计算所得currentS近似于1且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。
本发明的优点是：
基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。
附图说明
下面结合附图及实施例对本发明作进一步描述：
图1是本申请提供的动态路径跟踪方法的流程图。
具体实施方式
实施例：本申请提供了一种动态路径跟踪方法，如图1所示，该方法包括以下步骤。
步骤S1，利用B样条生成全局路径。
步骤S2，在每一个控制周期内，获取速度空间。
速度空间即当前线速度和角速度(或曲率)的取值范围，由当前速度和最大加(减)速度、用户设定的最大/最小速度，以及硬件能达到的最大/最小速度三个边界条件共同决定。在叉车的单舵轮运动模型中，近似的认为加/减速度无穷大，只考虑用户允许的最大/最小速度vmax/vmin，用户允许的最大/最小曲率kmax/kmin。
步骤S3，在速度空间内，通过对线速度v和曲率k或角速度ω进行采样，得到多组运动参数(v,k)的组合。
对于采样，在速度空间内采用平均采样的方法，采样步长由采样数Nv_sam和Nk_sam决定，线速度和曲率的样本计算方法如下：
步骤S4，对于每一组运动参数，模拟出未来的一小段圆弧轨迹。
对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针。
将圆弧轨迹上平均分成Np_sam段，每一段对应的轨迹点坐标的计算方法如下：
当k不为0时：
当k为0时：
其中，(x0,y0,θ0)为从定位程序获取的当前位置坐标，tsam表示采样周期。
鉴于控制周期很短，模拟轨迹可以通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点，从而在不影响跟踪效果的前提下降低运算量。
步骤S5，通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数。
可选的，评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。
本申请主要分五个部分进行成本考核，即与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量。由于在运动控制部分不需要局部避障，暂不考虑障碍物的影响。最后将五个部分成本按权重线性叠加起来：其中，/>为各项成本系数，决定各项因素的权重。一般而言，与样条轨迹角度和速度偏差较小、速度较快、速度和角速度变化量较小的轨迹会获得较低的成本。
成本系数使用动态参数，譬如在急转弯，或快到达目标点时，降低对于平顺度的要求，提高对距离和角度偏差的要求。
对于单舵轮模型，运动中心的速度和曲率存在耦合。选取驱动轮的线速度和转角作为采样对象也许更恰当。
在进行叠加之前，需要对各项成本进行归一化处理，目的是使各项成本的数值均匀、平滑。
各项成本的归一化处理如下：
在叠加过程中，首先对每一个控制周期内的Np_sum段的采样点与B样条上最近点的距离偏差和角度偏差进行累加：
然后将结果按采样点平均，并对其余各项成本进行叠加：
参考轨迹由均匀三阶B样条给出，由Ax,Bx,Cx,Dx,Ay,By,Cy,Dy,八个系数决定，参数方程为：
在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currentS；在计算距采样点最近点的坐标时，从currentS开始采用逐步搜索的方法，步长可以人为设定。
步骤S6，选取最优路线，抵达目标点。
得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机(PLC)，其中，
当currentS接近1(如大于0.95)且未到达最后一段路径时，Ax,Bx,Cx,Dx,Ay,By,Cy,Dy切换为下一段的系数；
当currentS接近1(如大于0.9)且已在最后一段路径时，进入最终抵达阶段；
在最终抵达阶段的控制周期内，若计算所得currentS近似于1(如|currentS-1|<0.05)且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。
综上所述，本申请提供的动态路径跟踪方法，基于采样和预测的动态路径跟踪方法，提高路径跟踪精度，与传统的集合方法相比，提供了一种泛华能力较强的通用方法，对不同形状的路径有一定的适应性，不需要动态调整预瞄点和预瞄距离，可以融合局部避障和局部路径规划。
术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或隐含所指示的技术特征的数量。由此，限定的“第一”、“第二”的特征可以明示或隐含地包括一个或者更多个该特征。在本申请的描述中，除非另有说明，“多个”的含义是两个或者两个以上。
上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器、磁盘或光盘等。
以上所述仅为本申请的较佳实施例，并不用以限制本申请，凡在本申请的精神和原则之内所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (3)

1.一种动态路径跟踪方法，其特征在于，包括：
利用B样条生成全局路径；
在每一个控制周期内，获取速度空间；
在速度空间内，通过对线速度和曲率或角速度进行采样，得到多组运动参数的组合；
对于每一组运动参数，模拟出未来的一小段圆弧轨迹；
通过评价函数对所有轨迹进行评价，采用评分最高或成本最低的轨迹及其相应的运动参数；
选取最优路线，抵达目标点；
所述对线速度和曲率或角速度进行采样，包括：
在速度空间内采用平均采样的方法，采样步长由采样数Nv_sam和Nk_sam决定，线速度和曲率的样本计算方法如下：
所述对于每一组运动参数，模拟出未来的一小段圆弧轨迹，包括：
对于每一组运动参数(v,k)，在理想状态下，下一个控制周期以v为线速度，以为曲率圆半径的圆弧轨迹运动，k的正负性指示角速度的方向，正为逆时针，负为顺时针；
将圆弧轨迹上平均分成Np_sam段，每一段对应的轨迹点坐标的计算方法如下：
当k不为0时：
当k为0时：
其中，(x0,y0,θ0)为从定位程序获取的当前位置坐标，tsam表示采样周期；
所述通过评价函数对所有轨迹进行评价，包括：
针对与样条轨迹上最近点的距离偏差、与样条轨迹上最近点的角度偏差、速度的绝对值大小、与上一周期相比速度的变化量、与上一周期相比曲率的变化量进行成本考核；
对各项成本进行归一化处理，各项成本的归一化处理如下：
在叠加过程中，首先对每一个控制周期内的Np_sum段的采样点与B样条上最近点的距离偏差和角度偏差进行累加：
然后将结果按采样点平均，并对其余各项成本进行叠加：
参考轨迹由均匀三阶B样条给出，由Ax，Bx，Cx，Dx，Ay，By，Cy，Dy，八个系数决定，参数方程为：
在每个控制周期一开始，将计算样条曲线上距小车当前位置最近点所对应的s值，记做currentS；在计算距采样点最近点的坐标时，从currentS开始采用逐步搜索的方法；
其中，kΔd，kΔθ，为各项成本系数，决定各项因素的权重；
所述选取最优路线，抵达目标点，包括：
得到各轨迹的成本后，从中选取成本最低的一条，并将其对应的运动参数转换为(v,ω)下发给下位机，其中，
当currentS接近1且未到达最后一段路径时，Ax，Bx，Cx，Dx，Ay，By，Cy，Dy切换为下一段的系数；
当currentS接近1且已在最后一段路径时，进入最终抵达阶段；
在最终抵达阶段的控制周期内，若计算所得currentS近似于1且/或当前位置坐标与终点坐标的距离、角度偏差都在允许范围内，则视作已经到达，将速度和角速度置零，各开关量复位。
2.根据权利要求1所述的动态路径跟踪方法，其特征在于，所述评价函数的评价标准至少包括避障能力、与全局路径的贴合程度、运动的平顺程度。
3.根据权利要求1所述的动态路径跟踪方法，其特征在于，在控制周期很短的情况下，轨迹模拟通过直线代替圆弧，对于每条轨迹，只取末端时刻的终点作为采样点。
CN202110737298.4A 2021-06-30 2021-06-30 一种动态路径跟踪方法 Active CN113625703B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110737298.4A CN113625703B (zh) 2021-06-30 2021-06-30 一种动态路径跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110737298.4A CN113625703B (zh) 2021-06-30 2021-06-30 一种动态路径跟踪方法

Publications (2)

Publication Number Publication Date
CN113625703A CN113625703A (zh) 2021-11-09
CN113625703B true CN113625703B (zh) 2024-04-30

Family

ID=78378761

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110737298.4A Active CN113625703B (zh) 2021-06-30 2021-06-30 一种动态路径跟踪方法

Country Status (1)

Country Link
CN (1) CN113625703B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113885487A (zh) * 2020-07-02 2022-01-04 苏州艾吉威机器人有限公司 路径跟踪方法、系统、装置及计算机可读存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110001637A (zh) * 2019-04-10 2019-07-12 吉林大学 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法
CN111208807A (zh) * 2018-11-06 2020-05-29 苏州艾吉威机器人有限公司 一种基于b样条曲线的agv运动控制方法
CN111338349A (zh) * 2020-03-09 2020-06-26 兰剑智能科技股份有限公司 Agv的贝塞尔曲线控制方法及系统
CN112230634A (zh) * 2019-06-26 2021-01-15 北京海益同展信息科技有限公司 一种机器人避障方法和装置
CN112325884A (zh) * 2020-10-29 2021-02-05 广西科技大学 一种基于dwa的ros机器人局部路径规划方法
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN112904855A (zh) * 2021-01-19 2021-06-04 四川阿泰因机器人智能装备有限公司 基于改进动态窗口的跟随机器人局部路径规划方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111208807A (zh) * 2018-11-06 2020-05-29 苏州艾吉威机器人有限公司 一种基于b样条曲线的agv运动控制方法
CN110001637A (zh) * 2019-04-10 2019-07-12 吉林大学 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法
CN112230634A (zh) * 2019-06-26 2021-01-15 北京海益同展信息科技有限公司 一种机器人避障方法和装置
CN111338349A (zh) * 2020-03-09 2020-06-26 兰剑智能科技股份有限公司 Agv的贝塞尔曲线控制方法及系统
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN112325884A (zh) * 2020-10-29 2021-02-05 广西科技大学 一种基于dwa的ros机器人局部路径规划方法
CN112904855A (zh) * 2021-01-19 2021-06-04 四川阿泰因机器人智能装备有限公司 基于改进动态窗口的跟随机器人局部路径规划方法

Also Published As

Publication number Publication date
CN113625703A (zh) 2021-11-09

Similar Documents

Publication Publication Date Title
CN109947123B (zh) 一种基于视线导引律的无人机路径跟踪与自主避障方法
CN111811517A (zh) 一种动态局部路径规划方法及系统
CN112731916A (zh) 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN107289938B (zh) 一种地面无人平台局部路径规划方法
CN111913484B (zh) 一种变电站巡检机器人在未知环境下的路径规划方法
CN113625702B (zh) 基于二次规划的无人车同时路径跟踪与避障方法
CN110789530B (zh) 一种四轮独立转向-独立驱动车辆轨迹跟踪方法和系统
CN113625703B (zh) 一种动态路径跟踪方法
CN115857504A (zh) 基于dwa的机器人在狭窄环境局部路径规划方法、设备和存储介质
Said et al. Local trajectory planning for autonomous vehicle with static and dynamic obstacles avoidance
CN113359808A (zh) 一种无人机电力巡检多级路径规划方法及相关装置
Qinpeng et al. Path tracking control of wheeled mobile robot based on improved pure pursuit algorithm
CN113031621A (zh) 一种桥式起重机安全避障路径规划方法及系统
CN110928314A (zh) 一种基于轨迹预测的纯跟踪模型改进算法
CN113701777B (zh) 基于空间向量的高精地图车道关联轨迹线自动生成方法
CN114771551A (zh) 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆
Font et al. Mobile robot localization. Revisiting the triangulation methods
CN112904855B (zh) 基于改进动态窗口的跟随机器人局部路径规划方法
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
CN114167856B (zh) 一种基于人工情感的服务机器人局部路径规划方法
CN114460933A (zh) 一种面向动态环境的移动机器人局部路径规划算法
CN112947481A (zh) 一种家庭服务机器人自主定位控制方法
CN117451068A (zh) 一种基于改进a*算法和动态窗口法的混合路径规划方法
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
CN118092208B (zh) 基于全局视觉伺服的工位内agv智能导航方法及系统

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