CN109976343B - 一种基于动态窗口法的主动避障方法 - Google Patents

一种基于动态窗口法的主动避障方法 Download PDF

Info

Publication number
CN109976343B
CN109976343B CN201910231615.8A CN201910231615A CN109976343B CN 109976343 B CN109976343 B CN 109976343B CN 201910231615 A CN201910231615 A CN 201910231615A CN 109976343 B CN109976343 B CN 109976343B
Authority
CN
China
Prior art keywords
distance
robot
obstacle
path
obs
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.)
Expired - Fee Related
Application number
CN201910231615.8A
Other languages
English (en)
Other versions
CN109976343A (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.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN201910231615.8A priority Critical patent/CN109976343B/zh
Publication of CN109976343A publication Critical patent/CN109976343A/zh
Application granted granted Critical
Publication of CN109976343B publication Critical patent/CN109976343B/zh
Expired - Fee Related 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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas

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

一种基于动态窗口法的主动避障方法
技术领域
本发明主要应用于机器人导航技术领域，具体涉及一种机器人的主动避障方法。
背景技术
当前，机器人导航中的路径规划主要有两部分，全局路径规划和局部路径规划。在实际使用过程中这两部分配合使用以达到更好的导航效果。全局路径规划：在已经构建完成的地图上规划处一条从起到终点的可行的无障碍轨迹。局部路径规划：根据传感器的消息和全局路径规划得到的路径，实时确定机器人的运动轨迹。局部路径规划算法主要作用是在前进过程中避开障碍物，使机器人顺利到达目标点。
现有的动态窗口法(Dynamic Window Approach)主要是避开与机器人距离稍远的静态障碍物，当机器人距离障碍物过近时会造成机器人认为自身已经撞上障碍物导致导航过程的失败。即，当前的局部路径规划算法存在的技术问题主要体现在：(1)在避障方面距离障碍物过近导致避障失败；(2)在避障的过程中无法主动避开障碍物。
发明内容
本发明的发明目的在于：针对现有的局部路径规划避障过程中距离障碍物过近时无法有效避开障碍物，出现局部路径规划失败的技术问题，提供一种基于动态窗口法的主动避障方法。
步骤S1、采集机器人的当前状态参数，所述状态参数包括机器人的位姿、全局路径、距机器人当前位置距离最近的障碍物的位置、机器人能执行的最大速度、最大加速度、最大转向速度、最大转向加速度；
步骤S2、生成多条模拟路径：
基于预设的模拟路径条数N，预设的模拟运行时长T，结合机器人的最大速度、最大加速度、最大转向速度、最大转向加速度，模拟出N条可移动的路径；
其中模拟时长T小于全局路径所花的时长；
步骤S3、对每条模拟路径，计算与全局路径最近点的均值，记为Pdist；
计算每条模拟路径的终点到机器人的目标位置的距离，记为Gdist
计算每条模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离，记为dobs；
步骤S4、基于每条模拟路径的障碍物距离dobs，分别为每条模拟路径设置对应的安全距离dsafe；
所述安全距离dsafe表示机器人在模拟时长内按照最大速度前进不会撞到障碍物的距离阈值；
基于每条模拟路径的距离dobs与安全距离dsafe的大小关系，设置每条模拟路径的评分Cobs：
若障碍物距离dobs大于安全距离dsafe，则不计分，即Cobs＝0；
若障碍物距离dobs为0，则舍去该模拟路径；
若障碍物距离dobs小于或等于安全距离dsafe，则根据障碍物距离dobs的反比指数计算评分Cobs；
步骤S5、对步骤S4中保留得到的各条模拟路径，通过评价函数C＝α×Pdist+β×Gdist+γ×Cobs分别计算各条模拟路径的度量值C，并将度量值C最小的模拟路径作为机器人当前运行路径；其中α，β，γ为Pdist、Gdist、Cobs的预设权重；
步骤S6、设置机器人在当前运行路径上的运行速度：
设置运行路径的碰撞距离docc，其中碰撞距离docc小于运行路径对应的安全距离dsafe；
判断障碍物距离dobs是否大于碰撞距离docc，若是，则设置机器人在当前运行路径上的运行速度为机器人下一时刻的运行速度；否则，设置其运行速度为负值，即向机器人发出后退指令；例如选取机器人下一时刻的运行速度负值作为机器人在当前运行路径上的运行速度；
机器人基于设置的运行速度在当前运行路径上运行，并基于当前机器人的当前位置判断机器人是否到达其目标位置，若是，则结束；否则继续执行步骤S1。
综上所述，由于采用了上述技术方案，本发明的有益效果是：机器人在距离障碍物过近时能主动倒退避开障碍物，继续向目标点前进；与现有的局部路径规划避障处理方式中只计算障碍物的距离，从而当与障碍物的距离过近时没有应对方法，导致机器人避障效果不佳的处理方式相比，本发明在距离障碍物的距离较远时和传统处理方式相近；但在距离障碍物距离较近时能够根据障碍物的距离合理控制机器人主动避开障碍物。
附图说明
图1为具体实施方式的主动避障处理流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面结合实施方式和附图，对本发明作进一步地详细描述。
由于现有的方法在距离障碍物过近时会停下，如果障碍物不移开，机器人无法有效避开障碍物。为此，本发明提供了一种主动避障方法。
本发明基于动态窗口法对模拟路径的评分标准，在模拟路径时根据障碍物的距离确定路径的评分，寻找与机器人最近的障碍物距离，当与障碍物的距离过小时(小于预设距离阈值)，向机器人的驱动处理单元发出后退指令，从而控制机器人执行后退命令，通常可设置后退的距离为一个机器人的直径(机器人在地面的投影的外接圆的直径)；再重新规划到目标点的路径。避免机器人距离障碍物过近导致导航失败的情况，并且能够主动避开障碍物，使机器人顺利快速地到达目标点。从而提高机器人距离障碍物过近时避障的效果。
参见图1，本发明的具体实现步骤如下：
步骤S1、采集机器人的当前状态参数，所述状态参数包括机器人的位姿、全局路径(从起始位置到目标位置的全局路径)、传感器数据(传感器数据包括激光测得的距离数据，主要距机器人当前位置距离最近的障碍物的位置信息)、机器人能执行的最大速度、最大加速度、最大转向速度、最大转向加速度和机器人在地面的投影的外接圆的半径R；
步骤S2、生成多条模拟路径：
基于预设的模拟路径条数N，预设的模拟运行时长T，结合步骤S1中采集的状态参数中的速度信息(最大速度、最大加速度、最大转向速度、最大转向加速度)，模拟出N条可移动的路径；
其中模拟时长T小于全局路径所花的时长(即按照机器人当前的移动速度或最大移动速度，走完全局路径所花的时长)，本具体实施方式中，设置模拟时长(也可定义为模拟时间)为全局路径所花时长的一半。
步骤S3、对于步骤S2中模拟出来的每条路径，计算与全局路径最近点的均值，记为Pdist，计算每条模拟路径的终点到机器人的目标位置(全局路径的终点)的距离，记为Gdist；以及计算每条模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离，记为dobs；基于障碍物距离dobs通过步骤S4计算每条路径的评分Cobs；
步骤S4、对步骤S3中得到的障碍物距离dobs，设置安全距离dsafe，即表示机器人在模拟时间内按照最大速度前进不会撞到障碍物的距离阈值。本具体实施方式中，根据机器人外切圆半径R，设置安全距离dsafe，其中，R＜dsafe≤k×R,k∈(1,3]。
基于每条模拟路径的距离dobs与安全距离dsafe的大小关系，设置每条模拟路径的评分Cobs：
如果障碍物距离dobs大于安全距离dsafe，则不计分，即Cobs＝0；
如果障碍物距离dobs为0，则舍去该模拟路径；
如果障碍物距离dobs小于或等于安全距离dsafe，则根据障碍物距离dobs的反比指数计算评分Cobs。
本具体实施方式中，当dobs或等于dsafe时，其对应的评分Cobs为：
Figure BDA0002006890330000041
步骤S5、对步骤S4中保留得到的各条模拟路径，通过评价函数C＝α×Pdist+β×Gdist+γ×Cobs分别计算模拟路径的度量值C，将该度量值作为每条模拟路径的代价，其中α，β，γ为Pdist、Gdist、Cobs的权重；优选的，其取值范围设置为(0,5]。
从当前保留的多条模拟路径中，选取代价最小的模拟路径作为机器人当前运行路径。
步骤S6、设置机器人在当前运行路径上的运行速度：
设置碰撞距离docc，其中碰撞距离小于安全距离dsafe；
对机器人当前的运行路径(对步骤S5选取出的代价最小的模拟路径)，判断其对应的障碍物距离dobs(代价最小的模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离)是否大于碰撞距离docc，若是，则设置当前运行路径所对应的速度为机器人下一时刻的运行速度；若小于或等于碰撞距离docc，则选取对应速度的负值作为机器人下一时刻的运行速度，即控制机器人后退。
机器人基于设置的运行速度在当前运行路径上运行，并基于当前机器人的当前位置判断机器人是否到达其目标位置，若是，则结束；否则继续执行步骤S1。
本发明根据障碍物的距离，设置安全距离和碰撞距离，判断机器人是否需要后退主动避开障碍物。当与障碍物的距离在安全距离内则会顺利避开障碍物；若与障碍物的距离在碰撞距离内则会选择远离障碍物的路径，退到安全距离外再重新规划路径绕开障碍物。
由于采用了上述避障手段，使得本发明在距离障碍物的距离较远时和传统算法相近；在距离障碍物距离较近时能够根据障碍物的距离合理控制机器人主动避开障碍物。
以上所述，仅为本发明的具体实施方式，本说明书中所公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换；所公开的所有特征、或所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以任何方式组合。

Claims (5)

1.一种基于动态窗口法的主动避障方法，其特征在于，包括下列步骤：
步骤S1、采集机器人的当前状态参数，所述状态参数包括机器人的位姿、全局路径、距机器人当前位置距离最近的障碍物的位置、机器人能执行的最大速度、最大加速度、最大转向速度、最大转向加速度；
步骤S2、生成多条模拟路径：
基于预设的模拟路径条数N，预设的模拟运行时长T，结合机器人的最大速度、最大加速度、最大转向速度、最大转向加速度，模拟出N条可移动的路径；
其中模拟时长T小于全局路径所花的时长；
步骤S3、对每条模拟路径，计算与全局路径最近点的均值，记为Pdist；
计算每条模拟路径的终点到机器人的目标位置的距离，记为Gdist
计算每条模拟路径的终点到距机器人当前位置距离最近的障碍物之间的距离，记为dobs；
步骤S4、基于每条模拟路径的障碍物距离dobs，分别为每条模拟路径设置对应的安全距离dsafe；
所述安全距离dsafe表示机器人在模拟时长内按照最大速度前进不会撞到障碍物的距离阈值；
基于每条模拟路径的距离dobs与安全距离dsafe的大小关系，设置每条模拟路径的评分Cobs：
若障碍物距离dobs大于安全距离dsafe，则不计分，即Cobs＝0；
若障碍物距离dobs为0，则舍去该模拟路径；
若障碍物距离dobs小于或等于安全距离dsafe，则根据障碍物距离dobs的反比指数计算评分Cobs；
步骤S5、对步骤S4中保留得到的各条模拟路径，通过评价函数C＝α×Pdist+β×Gdist+γ×Cobs分别计算各条模拟路径的度量值C，并将度量值C最小的模拟路径作为机器人当前运行路径；其中α，β，γ为Pdist、Gdist、Cobs的预设权重；
步骤S6、设置机器人在当前运行路径上的运行速度：
设置运行路径的碰撞距离docc，其中碰撞距离docc小于运行路径对应的安全距离dsafe；
判断障碍物距离dobs是否大于碰撞距离docc，若是，则设置机器人在当前运行路径上的运行速度为机器人下一时刻的运行速度；否则，设置运行速度为负值，即向机器人发出后退指令；
机器人基于设置的运行速度在当前运行路径上运行，并基于当前机器人的当前位置判断机器人是否到达其目标位置，若是，则结束；否则继续执行步骤S1。
2.如权利要求1所述的方法，其特征在于，所述状态参数还包括机器人在地面投影的外切圆半径R；
且安全距离dsafe满足：R＜dsafe≤k×R，其中k∈(1,3]。
3.如权利要求1或2所述的方法，其特征在于，当dobs等于dsafe时，评分Cobs为：
Figure FDA0002366212280000021
4.如权利要求1或2所述的方法，其特征在于，α，β，γ的取值范围为(0,5]。
5.如权利要求1或2所述的方法，其特征在于，步骤S6中，当障碍物距离dobs小于或等于碰撞距离docc时，设置机器人在当前运行路径上的运行速度为：当前运行速度的负值，即在当前运行路径上按照当前运行速度反方向运行。
CN201910231615.8A 2019-03-26 2019-03-26 一种基于动态窗口法的主动避障方法 Expired - Fee Related CN109976343B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910231615.8A CN109976343B (zh) 2019-03-26 2019-03-26 一种基于动态窗口法的主动避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910231615.8A CN109976343B (zh) 2019-03-26 2019-03-26 一种基于动态窗口法的主动避障方法

Publications (2)

Publication Number Publication Date
CN109976343A CN109976343A (zh) 2019-07-05
CN109976343B true CN109976343B (zh) 2020-03-27

Family

ID=67080582

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910231615.8A Expired - Fee Related CN109976343B (zh) 2019-03-26 2019-03-26 一种基于动态窗口法的主动避障方法

Country Status (1)

Country Link
CN (1) CN109976343B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112445211B (zh) * 2019-08-15 2023-10-27 纳恩博（北京）科技有限公司 机器人的控制方法和装置、存储介质及电子装置
CN110488843B (zh) * 2019-09-04 2023-12-05 达闼机器人股份有限公司 避障方法、移动机器人及计算机可读存储介质
CN112631209A (zh) * 2019-10-08 2021-04-09 北京京东乾石科技有限公司 用于控制无人搬运车的方法和装置
CN111123934A (zh) * 2019-12-25 2020-05-08 深圳深岚视觉科技有限公司 轨迹评估方法、轨迹评估装置及移动机器人
CN111142530B (zh) * 2019-12-31 2023-03-17 达闼机器人股份有限公司 一种机器人运行轨迹的确定方法、机器人和存储介质
CN111703421A (zh) * 2020-05-13 2020-09-25 重庆长安汽车股份有限公司 车辆自动驾驶脱困的轨迹规划***、方法及存储介质
CN112904855B (zh) * 2021-01-19 2022-08-16 四川阿泰因机器人智能装备有限公司 基于改进动态窗口的跟随机器人局部路径规划方法
CN114625150B (zh) * 2022-05-17 2022-08-19 南京汇与信息科技有限公司 基于危险系数和距离函数的快速蚁群无人艇动态避障方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105717929B (zh) * 2016-04-29 2018-06-15 中国人民解放军国防科学技术大学 一种多分辨率障碍物环境下移动机器人混合路径规划方法
CN106325275B (zh) * 2016-09-14 2019-08-20 广州今甲智能科技有限公司 一种机器人导航的***、方法及装置
CN108073166A (zh) * 2016-11-16 2018-05-25 阳光暖果（北京）科技发展有限公司 一种最短时间避障路径规划方法
US11614751B2 (en) * 2017-01-23 2023-03-28 Massachusetts Institute Of Technology System for on-demand high-capacity ride-sharing via dynamic trip-vehicle assignment and related techniques
CN106774347A (zh) * 2017-02-24 2017-05-31 安科智慧城市技术（中国）有限公司 室内动态环境下的机器人路径规划方法、装置和机器人
CN108170134A (zh) * 2017-11-15 2018-06-15 国电南瑞科技股份有限公司 一种变电站智能巡检机器人路径规划方法
CN108762264B (zh) * 2018-05-22 2021-05-04 重庆邮电大学 基于人工势场与滚动窗口的机器人的动态避障方法
CN109062227A (zh) * 2018-09-25 2018-12-21 电子科技大学 一种基于动态窗口的多曲线局部路径规划方法
CN109163728B (zh) * 2018-10-31 2020-08-28 山东大学 一种动态环境避障方法、控制器及机器人

Also Published As

Publication number Publication date
CN109976343A (zh) 2019-07-05

Similar Documents

Publication Publication Date Title
CN109976343B (zh) 一种基于动态窗口法的主动避障方法
CN114234998B (zh) 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法
CN109976340B (zh) 一种基于深度增强学习的人机协同动态避障方法及***
CN108692734B (zh) 一种路径规划方法和装置
CN112882469B (zh) 一种融合全局训练的深度强化学习避障导航方法
CN111142530B (zh) 一种机器人运行轨迹的确定方法、机器人和存储介质
JP7085296B2 (ja) ロボットの再測位方法
CN112888612A (zh) 自动驾驶车辆规划
US11898855B2 (en) Assistance control system that prioritizes route candidates based on unsuitable sections thereof
KR101196374B1 (ko) 이동 로봇의 경로 생성 시스템
CN112590775B (zh) 一种自动泊车方法、装置、车辆及存储介质
CN111694356B (zh) 一种行驶控制方法、装置、电子设备及存储介质
CN107289938B (zh) 一种地面无人平台局部路径规划方法
CN111830979A (zh) 一种轨迹优化方法和装置
CN112577506B (zh) 一种自动驾驶局部路径规划方法和***
US20230256991A1 (en) Method and system for conditional operation of an autonomous agent
CN112906542B (zh) 一种基于强化学习的无人车避障方法及装置
CN114237256B (zh) 一种适用于欠驱动机器人的三维路径规划与导航方法
Crane Iii et al. Team CIMAR's NaviGATOR: An unmanned ground vehicle for the 2005 DARPA grand challenge
CN118235180A (zh) 预测可行驶车道的方法和装置
CN117360546A (zh) 一种车辆控制方法、车辆、电子设备及存储介质
CN117170377A (zh) 一种自动驾驶方法、装置及车辆
Yuan et al. A Q-learning approach based on human reasoning for navigation in a dynamic environment
JP2020149095A (ja) 倒立振子型ロボット
Hellstrom et al. Real-time path planning using a simulator-in-the-loop

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20200327