CN112506199B - 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 - Google Patents

一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 Download PDF

Info

Publication number
CN112506199B
CN112506199B CN202011464715.4A CN202011464715A CN112506199B CN 112506199 B CN112506199 B CN 112506199B CN 202011464715 A CN202011464715 A CN 202011464715A CN 112506199 B CN112506199 B CN 112506199B
Authority
CN
China
Prior art keywords
robot
obstacle
point
distance
evaluation function
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
CN202011464715.4A
Other languages
English (en)
Other versions
CN112506199A (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.)
Jiangxi Hongdu Aviation Industry Group Co Ltd
Original Assignee
Jiangxi Hongdu Aviation Industry Group 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 Jiangxi Hongdu Aviation Industry Group Co Ltd filed Critical Jiangxi Hongdu Aviation Industry Group Co Ltd
Priority to CN202011464715.4A priority Critical patent/CN112506199B/zh
Publication of CN112506199A publication Critical patent/CN112506199A/zh
Application granted granted Critical
Publication of CN112506199B publication Critical patent/CN112506199B/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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Abstract

一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，首先根据机器人自身携带传感器获得机器人的姿态与障碍物的位置，再计算避障起始点坐标、局部目标点的坐标和方向，确定最终的速度空间；同时对机器人的避障过程进行分段确定评价函数，并判断当前处于分段避障中的段数选择相应的评价函数进行处理，得到最小的评价函数值所对应的速度，作为机器人运行的最优速度，重复执行获得的最优速度，根据机器人当前位置判断机器人是否到达局部目标点，若到达则避障结束；根据障碍物的大小、位置及机器人速度和最小转弯半径进行局部路径规划，促使机器人在能避开障碍物的前提下以最优路径运动且在避障结束时能以平滑的方向回归至全局路径。

Description

一种基于动态窗口法且适用于阿克曼模型机器人的局部路径 规划方法
技术领域
本发明涉及机器人导航技术领域，尤其涉及一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法。
背景技术
近年来，随着人工智能的发展，机器人在高危性、重复性、复杂性的任务中展现出优良特性，但机器人为了完成特定的任务，在复杂环境中能主动绕开障碍物成为一项基本能力，而如何快速绕开障碍物，寻找避开障碍物的最优路径，成为影响其性能的关键因素之一。
机器人路径规划包括全局路径规划和局部路径规划，全局路径规划是在已知的环境中规划一条安全可行的路径，局部路径规划是通过传感器对机器人工作环境进行探测后，根据局部环境信息规划路径，使机器人避开障碍物，顺利到达目标点。
动态窗口法(Dynamic Window Approach，DWA)，是目前应用最广泛的局部路径规划算法之一，传统的DWA算法只适合两轮差速模型或者万向轮机器人，即可在一个位置原地转弯的机器人。阿克曼模型的机器人，在运动过程中有最小转弯半径的限制，不能够原地转弯，同时阿克曼模型的机器人采用传统的DWA算法避障时，存在如下技术问题：
(1)若机器人在离障碍物较近的位置开始避障，由于有最小转弯半径限制，机器人不能成功绕开障碍物；若机器人在离障碍物较远的位置开始避障，导致局部路径过长，不能以最优路径绕开障碍物；
(2)在避障结束时，不能确保机器人以平滑的方向回归到全局路径，当局部路径与全局路径存在较大的夹角时，由于有最小转弯半径的限制，机器人需要绕一个较大的圆弧以调整机器人的方向与全局路径方向一致。
发明内容
本发明所解决的技术问题在于提供一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，以解决上述背景技术中的问题。
本发明所解决的技术问题采用以下技术方案来实现：
一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，具体步骤如下：
步骤1)根据机器人自身携带传感器获得机器人的姿态与障碍物的位置，并创建环境模型；
步骤2)在环境模型中计算避障起始点S1的坐标，局部目标点P0,S2的坐标和方向；
步骤3)综合考虑运动学约束、安全容许的速度约束、动力学约束和最小转弯半径约束，确定最终的速度空间；
步骤4)对机器人的避障过程进行分段，并确定每段的评价函数；
步骤5)根据机器人当前位置与障碍物位置，判断当前处于分段避障中的段数，以选择相应的评价函数，计算当前状态下评价函数的输入项，并对其做归一化处理；
步骤6)待步骤5)中归一化处理完毕后，计算评价函数的最终值，得到最小的评价函数值所对应的速度，作为机器人运行的最优速度；
步骤7)执行步骤6)中的最优速度，并根据机器人当前的位置判断机器人是否到达局部目标点S2，若到达则避障结束，否则返回步骤3)继续循环计算。
在本发明中，步骤1)中，假设P1(x1,y1)为机器人起始点位置，P2(x2,y2)为机器人本次任务最终目标点位置，将障碍物抽象为圆形模型，即障碍物是以中心点为圆的圆形区域，障碍区域半径RD为
RD＝max(R,R0) (1)
式(1)中，R为机器人最小转弯半径，Ro为障碍物最远端到中心点距离。
在本发明中，步骤2)中，避障起始点S1与局部目标点S2到障碍物的距离相等，局部目标点P0位于S1S2的中垂线上，根据实际需求，设置P0点距障碍物边界的距离为d，那么P0点距离障碍物圆心的距离P0C为d+R；
步骤2-1：求P0的坐标和方向
P1,P2的坐标已知，因此可得P0C的直线方程：
Ax+By+C＝0 (2)
P0(x0,y0)点到障碍物的圆心C的距离为d+R，P0(x0,y0)满足如下关系：
Figure BDA0002832654400000031
由公式(2)与(3)即可得局部目标点P0的坐标，其方向与
Figure BDA0002832654400000041
方向相同；
步骤2-2：计算绕障区域圆心D的坐标(xD,yD)
判断障碍物半径R0与机器人最小转弯半径R的关系，若R0≥R，障碍物圆心与绕障区域圆心D重合，若R0＜R，按如下计算绕障圆心D(xD,yD)的坐标：
P0点为绕障区域圆上的一点，因此D(xD,yD)满足如下关系：
Figure BDA0002832654400000042
D点在直线P0C上，由公式(2)和(4)即可得绕障区域圆心D点的坐标；
步骤2-3：计算避障起始点S1(xs1,ys1)和局部目标点S2(xs2,ys2)坐标
由绕障区域圆心D点的坐标可得D到P1P2的距离设为r，则O1A距离为R+r，O1D距离为RD+R，设AD距离为l，S1D距离为a，有如下关系式：
Figure BDA0002832654400000043
a2＝l2+r2 (6)
因S1,S2到D点距离相等，长度为a，满足如下关系：
a2＝(x-xD)2+(y-yD)2 (7)
a2＝(xs2-xD)2+(ys2-yD)2 (8)
由公式(5)、(6)、(7)与(8)可得，避障起始点S1和局部目标点S2坐标，局部目标点S2的方向与
Figure BDA0002832654400000044
的方向相同。
在本发明中，步骤3)中，确定满足机器人运动学约束的速度Vs，即满足最大最小速度的限制；
确定满足机器人安全容许约束的速度Va，即碰到障碍物前能够停下来的速度被认为是满足安全性的可接受速度；
确定满足动力学约束的速度Vd，即满足电机最大加减速度的限制；
确定满足最小转弯半径限制的速度Vr，即线速度与减速度满足阿克曼模型的运动控制约束；
综合考虑以上约束条件，因此最终的速度空间V为：
V＝Vs∩Va∩Vd∩Vr。
在本发明中，步骤4)中，由步骤3)得到的速度空间V包括多组可行的运动轨迹，通过评价函数选择一条最优轨迹用于避障，机器人的避障过程分为四段，每一段采用不同的评价函数来筛选最优轨迹：
第一段：从S1到P0，且机器人到S1的距离在一定阈值内，评价函数为：
G(v,ω)＝α*dist_goal1(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*curvature(v,ω)
(9)
其中，dist_goal1(v,ω)为机器人预测轨迹中距离局部目标点P0的最小距离，dist_obstacle(v,ω)为机器人预测轨迹中距离障碍物的最小距离的倒数，v(v,ω)＝vmax-v(v,ω)，vmax为机器人最大速度限制，curvature(v,ω)为机器人运动的曲率，式(9)中，距离目标点的距离越小，离障碍物越远，速度越大，曲率越大，其评价函数的G值越小；在第一段中机器人以较大的曲率避开障碍物，因此α取较小的值，λ取较大的值；
第二段：从S1到P0，且机器人到S1的距离大于一定阈值，即机器人在P0点附近，为保证机器人在避障结束时能平滑回归至全局路径，使其在P0点方向与
Figure BDA0002832654400000061
方向一致，因此评价函数为：
G(v,ω)＝α*dist_goal1(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*direction(v,ω)
(10)
其中，direction(v,ω)为机器人预测轨迹上距离P0最近的点的方向与P0方向的夹角，其他变量一定的条件下，其夹角越小，评价函数(10)的G值越小；
第三段：从P0到S2，且机器人到P0的距离在一定阈值内，评价函数为：
G(v,ω)＝α*dist_goal2(v,ω)+β*dist_obstacle2(v,ω)+γ*v(v,ω)+λ*curvature(v,ω)
(11)
其中，dist_goal2(v,ω)为机器人预测轨迹中距离目标点S2的最小距离，dist_obstacle2(v,ω)为机器人预测轨迹中距离障碍物的最小距离，其距离越小，评价函数G值越小，为保证在第四段中机器人能平滑的回归至全局路径，在本段中机器人在满足最小转弯半径限制的前提下，机器人需紧贴着障碍物运动；
第四段：从P0到S2，且机器人到P0的距离大于一定阈值，即机器人在局部目标点S2的附近，评价函数为：
G(v,ω)＝α*dist_goal2(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*direction(v,ω)
(12)
其中，direction(v,ω)为机器人预测轨迹上距离S2最近的点的方向与S2方向的夹角，其夹角越小，评价函数的G值越小，在此过程中筛选能以切线方向回归至全局路径的轨迹，因此λ取较大值。
有益效果：本发明根据障碍物的大小、位置，以及机器人速度和最小转弯半径进行局部路径规划，促使机器人在能避开障碍物的前提下以最优路径运动，且在避障结束时能以平滑的方向回归至全局路径，有效解决动态窗口算法不适用阿克曼模型机器人的问题。
附图说明
图1为本发明的较佳实施例中的避障模型示意图。
图2～3为本发明的较佳实施例中的避障模型生成示意图。
具体实施方式
为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。
一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，具体步骤如下：
步骤1)根据机器人自身携带传感器获得机器人的姿态与障碍物的位置，并创建环境模型；
步骤2)在环境模型中计算避障起始点S1的坐标，局部目标点P0,S2的坐标和方向；
步骤3)综合考虑运动学约束、安全容许的速度约束、动力学约束和最小转弯半径约束，确定最终的速度空间；
步骤4)对机器人的避障过程进行分段，并确定每段的评价函数；
步骤5)根据机器人当前位置与障碍物位置，判断当前处于分段避障中的段数，以选择相应的评价函数，计算当前状态下评价函数的输入项，并对其做归一化处理；
步骤6)待步骤5)中归一化处理完毕后，计算评价函数的最终值，得到最小的评价函数值所对应的速度(v,ω)，作为机器人运行的最优速度；
步骤7)执行步骤6)中的最优速度，并根据机器人当前的位置判断机器人是否到达局部目标点S2，若到达则避障结束，否则返回步骤3)继续循环计算。
在本实施例中，步骤1)中，结合图1所示，假设P1(x1,y1)为机器人起始点位置，P2(x2,y2)为机器人本次任务最终目标点位置，将障碍物抽象为圆形模型，即障碍物是以中心点为圆的圆形区域，障碍区域半径RD为
RD＝max(R,R0) (1)
式(1)中，R为机器人最小转弯半径，Ro为障碍物最远端到中心点距离；
判断障碍物圆心C(xc,yc)与
Figure BDA0002832654400000081
的位置关系，若在左侧，生成如图1所示的几何模型，即机器人先绕O1逆时针运动，再绕D做顺时针运动，而后绕O2逆时针运动，最后以切线的方向回归到全局路径；若在右侧，生成如图2所示的几何模型，即机器人先绕O1顺时针运动，再绕绕D做逆时针运动，然后绕O2顺时针运动，最后以切线的方向回归至全局路径，其中，O1和O2的半径为R，S1为避障起始点，P0,S2为局部目标点，机器人的局部避障路径为S1→P0→S2；
在本实施例中，步骤2)中，如图3所示，避障起始点S1与局部目标点S2到障碍物的距离相等，局部目标点P0位于S1S2的中垂线上，根据实际需求，设置P0点距障碍物边界的距离为d，那么P0点距离障碍物圆心的距离P0C为d+R；
步骤2-1：求P0的坐标和方向
P1,P2的坐标已知，因此可得P0C的直线方程：
Ax+By+C＝0 (2)
P0(x0,y0)点到障碍物的圆心C的距离为d+R，P0(x0,y0)满足如下关系：
Figure BDA0002832654400000091
由公式(2)与(3)即可得局部目标点P0的坐标，其方向与
Figure BDA0002832654400000092
方向相同；
步骤2-2：计算绕障区域圆心D的坐标(xD,yD)
判断障碍物半径R0与机器人最小转弯半径R的关系，若R0≥R，障碍物圆心与绕障区域圆心D重合，若R0＜R，按如下计算绕障圆心D(xD,yD)的坐标：
P0点为绕障区域圆上的一点，因此D(xD,yD)满足如下关系：
Figure BDA0002832654400000093
D点在直线P0C上，由公式(2)和(4)即可得绕障区域圆心D点的坐标；
步骤2-3：计算避障起始点S1(xs1,ys1)和局部目标点S2(xs2,ys2)坐标
由绕障区域圆心D点的坐标可得D到P1P2的距离设为r，则O1A距离为R+r，O1D距离为RD+R。设AD距离为l，S1D距离为a，有如下关系式：
Figure BDA0002832654400000101
a2＝l2+r2 (6)
因S1,S2到D点距离相等，长度为a，满足如下关系：
a2＝(x-xD)2+(y-yD)2 (7)
a2＝(xs2-xD)2+(ys2-yD)2 (8)
由公式(5)、(6)、(7)与(8)可得，避障起始点S1和局部目标点S2坐标，且局部目标点S2的方向与
Figure BDA0002832654400000102
的方向相同。
在本实施例中，步骤3)中，确定满足机器人运动学约束的速度Vs，即满足最大最小速度的限制；
确定满足机器人安全容许约束的速度Va，即碰到障碍物前能够停下来的速度被认为是满足安全性的可接受速度；
确定满足动力学约束的速度Vd，即满足电机最大加减速度的限制；
确定满足最小转弯半径限制的速度Vr，即线速度与减速度满足阿克曼模型的运动控制约束；
综合考虑以上约束条件，因此最终的速度空间V为：
V＝Vs∩Va∩Vd∩Vr
步骤4)中，由步骤3)得到的速度空间V包括多组可行的运动轨迹，通过评价函数选择一条最优轨迹用于避障，本实施例中将机器人的避障过程分为四段，每一段采用不同的评价函数来筛选最优轨迹：
第一段：从S1到P0，且机器人到S1的距离在一定阈值内，评价函数为：
G(v,ω)＝α*dist_goal1(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*curvature(v,ω)
(9)
其中，dist_goal1(v,ω)为机器人预测轨迹中距离局部目标点P0的最小距离，dist_obstacle(v,ω)为机器人预测轨迹中距离障碍物的最小距离的倒数，v(v,ω)＝vmax-v(v,ω)，vmax为机器人最大速度限制，curvature(v,ω)为机器人运动的曲率，式(9)中，距离目标点的距离越小，离障碍物越远，速度越大，曲率越大，其评价函数的G值越小；在第一段中机器人以较大的曲率避开障碍物，因此α取较小的值，λ取较大的值；
第二段：从S1到P0，且机器人到S1的距离大于一定阈值，即机器人在P0点附近，为保证机器人在避障结束时能平滑回归至全局路径，使其在P0点方向与
Figure BDA0002832654400000111
方向一致，因此评价函数为：
G(v,ω)＝α*dist_goal1(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*direction(v,ω)
(10)
其中，direction(v,ω)为机器人预测轨迹上距离P0最近的点的方向与P0方向的夹角，其他变量一定的条件下，其夹角越小，评价函数(10)的G值越小；
第三段：从P0到S2，且机器人到P0的距离在一定阈值内，评价函数为：
G(v,ω)＝α*dist_goal2(v,ω)+β*dist_obstacle2(v,ω)+γ*v(v,ω)+λ*curvature(v,ω)
(11)
其中，dist_goal2(v,ω)为机器人预测轨迹中距离目标点S2的最小距离，dist_obstacle2(v,ω)为机器人预测轨迹中距离障碍物的最小距离，其距离越小，评价函数G值越小，为保证在第四段中机器人能平滑的回归至全局路径，在本段中机器人在满足最小转弯半径限制的前提下，机器人需紧贴着障碍物运动；
第四段：从P0到S2，且机器人到P0的距离大于一定阈值，即机器人在局部目标点S2的附近，评价函数为：
G(v,ω)＝α*dist_goal2(v,ω)+β*dist_obstacle(v,ω)+γ*v(v,ω)+λ*direction(v,ω)
(12)
其中，direction(v,ω)为机器人预测轨迹上距离S2最近的点的方向与S2方向的夹角，其夹角越小，评价函数的G值越小，在此过程中筛选能以切线方向回归至全局路径的轨迹，因此λ取较大值。
步骤6)中，计算评价函数的G值，得到最小的G值所对应的速度(v,ω)，作为机器人运行的速度；
步骤7)，重复以上循环，直至绕开障碍物，以切向方向回归至全局路径。

Claims (2)

1.一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，其特征在于，具体步骤如下：
步骤1）根据机器人自身携带传感器获得机器人的姿态与障碍物的位置，并创建环境模型；
步骤 2）在环境模型中计算避障起始点
Figure DEST_PATH_IMAGE001
的坐标，局部目标点
Figure 403257DEST_PATH_IMAGE002
的坐标和方向，避障起始点
Figure 372350DEST_PATH_IMAGE001
与局部目标点
Figure DEST_PATH_IMAGE003
到障碍物的距离相等，局部目标点
Figure 120863DEST_PATH_IMAGE004
位于
Figure DEST_PATH_IMAGE005
的中垂线上，设置
Figure 655749DEST_PATH_IMAGE006
点距障碍物边界的距离为d，则局部目标点
Figure 34778DEST_PATH_IMAGE006
点距离障碍物圆心的距离
Figure DEST_PATH_IMAGE007
为d+R；假设
Figure 225588DEST_PATH_IMAGE008
为机器人起始点位置，
Figure DEST_PATH_IMAGE009
为机器人本次任务最终目标点位置，将障碍物抽象为圆形模型，即障碍物是以中心点为圆的圆形区域，障碍区域半径
Figure 512213DEST_PATH_IMAGE010
为
Figure DEST_PATH_IMAGE011
（1）
求局部目标点
Figure 167185DEST_PATH_IMAGE006
的坐标和方向
Figure 982694DEST_PATH_IMAGE012
的坐标已知，因此可得
Figure 926380DEST_PATH_IMAGE007
的直线方程：
Ax+By+C=0 （2）
Figure DEST_PATH_IMAGE013
点到障碍物的圆心
Figure 751116DEST_PATH_IMAGE014
的距离为d+R，
Figure 463857DEST_PATH_IMAGE013
满足如下关系：
Figure DEST_PATH_IMAGE015
（3）
由公式（2）与（3）即可得局部目标点
Figure 184688DEST_PATH_IMAGE006
的坐标，其方向与
Figure 615670DEST_PATH_IMAGE016
的方向相同；
计算绕障区域圆心D的坐标
Figure DEST_PATH_IMAGE017
判断障碍物半径
Figure 244097DEST_PATH_IMAGE018
与机器人最小转弯半径
Figure DEST_PATH_IMAGE019
的关系，若
Figure 608082DEST_PATH_IMAGE020
，障碍物圆心与绕障区域圆心D重合，若
Figure DEST_PATH_IMAGE021
，按如下计算绕障圆心
Figure 296553DEST_PATH_IMAGE022
的坐标：
Figure 214830DEST_PATH_IMAGE006
点为绕障区域圆上的一点，因此
Figure 53473DEST_PATH_IMAGE022
满足如下关系：
Figure DEST_PATH_IMAGE023
（4）
绕障区域圆心
Figure 271965DEST_PATH_IMAGE024
点在直线
Figure 865757DEST_PATH_IMAGE007
上，由公式（2）和（4）即可得绕障区域圆心D点的坐标；
计算避障起始点
Figure DEST_PATH_IMAGE025
和局部目标点
Figure 531051DEST_PATH_IMAGE026
坐标
由绕障区域圆心D点的坐标可得
Figure 235702DEST_PATH_IMAGE024
到
Figure DEST_PATH_IMAGE027
的距离设为r，则
Figure 308700DEST_PATH_IMAGE028
距离为
Figure DEST_PATH_IMAGE029
，
Figure 338973DEST_PATH_IMAGE030
距离为
Figure DEST_PATH_IMAGE031
，设AD距离为
Figure 497422DEST_PATH_IMAGE032
，
Figure DEST_PATH_IMAGE033
距离为a，有如下关系式：
Figure 209026DEST_PATH_IMAGE034
（5）
Figure DEST_PATH_IMAGE035
（6）
因
Figure 402110DEST_PATH_IMAGE036
到D点距离相等，长度为a，满足如下关系：
Figure DEST_PATH_IMAGE037
（7）
Figure 603284DEST_PATH_IMAGE038
（8）
由公式（5）、（6）、（7）与（8）可得，避障起始点
Figure DEST_PATH_IMAGE039
与局部目标点
Figure 514608DEST_PATH_IMAGE003
坐标，且局部目标点
Figure 295482DEST_PATH_IMAGE003
的方向与的方向相同；
步骤3）综合约束因素，确定最终的速度空间；
步骤 4）将机器人的避障过程分为四段，每一段采用不同的评价函数来筛选最优轨迹，且在机器人内设置有用于筛选最优轨迹的阈值；筛选最优轨迹具体如下：
第一段：从
Figure 811914DEST_PATH_IMAGE001
到
Figure 652831DEST_PATH_IMAGE006
，且机器人到
Figure 254714DEST_PATH_IMAGE001
的距离在阈值内，评价函数为：
Figure 42541DEST_PATH_IMAGE040
（9）
其中，
Figure DEST_PATH_IMAGE041
为机器人预测轨迹中距离局部目标点
Figure 944638DEST_PATH_IMAGE006
的最小距离，
Figure 956457DEST_PATH_IMAGE042
为机器人预测轨迹中距离障碍物的最小距离的倒数，
Figure DEST_PATH_IMAGE043
，
Figure 576794DEST_PATH_IMAGE044
为机器人最大速度限制，
Figure DEST_PATH_IMAGE045
为机器人运动的曲率，式（9）中，距离目标点的距离越小，离障碍物越远，速度越大，曲率越大，其评价函数的G值越小；在第一段中机器人以较大的曲率避开障碍物，因此
Figure 496208DEST_PATH_IMAGE046
取较小的值，
Figure DEST_PATH_IMAGE047
取较大的值；
第二段：从
Figure 252812DEST_PATH_IMAGE001
到
Figure 435531DEST_PATH_IMAGE006
，且机器人到
Figure 12006DEST_PATH_IMAGE001
的距离大于阈值，即机器人在
Figure 141636DEST_PATH_IMAGE006
点附近，为保证机器人在避障结束时能平滑回归至全局路径，使其在
Figure 221588DEST_PATH_IMAGE006
点方向与
Figure 840788DEST_PATH_IMAGE048
方向一致，因此评价函数为：
Figure DEST_PATH_IMAGE049
（10）
其中，为机器人预测轨迹上距离
Figure 175997DEST_PATH_IMAGE006
最近的点的方向与
Figure 640477DEST_PATH_IMAGE006
方向的夹角，其他变量一定的条件下，其夹角越小，评价函数（10）的G值越小；
第三段：从
Figure 840514DEST_PATH_IMAGE006
到
Figure 630615DEST_PATH_IMAGE003
，且机器人到
Figure 650524DEST_PATH_IMAGE006
的距离在阈值内，评价函数为：
Figure 981011DEST_PATH_IMAGE050
（11）
其中，
Figure DEST_PATH_IMAGE051
为机器人预测轨迹中距离目标点的最小距离，
Figure 769976DEST_PATH_IMAGE052
为机器人预测轨迹中距离障碍物的最小距离，其距离越小，评价函数G值越小，为保证在第四段中机器人能平滑的回归至全局路径，在本段中机器人在满足最小转弯半径限制的前提下，机器人需紧贴着障碍物运动；
第四段：从到
Figure 465399DEST_PATH_IMAGE003
，且机器人到
Figure 34921DEST_PATH_IMAGE006
的距离大于阈值，即机器人在局部目标点
Figure 106782DEST_PATH_IMAGE003
的附近，评价函数为：
Figure DEST_PATH_IMAGE053
（12）
其中，
Figure 546991DEST_PATH_IMAGE054
为机器人预测轨迹上距离最近的点的方向与
Figure 678895DEST_PATH_IMAGE003
方向的夹角，其夹角越小，评价函数的G值越小，在此过程中筛选能以切线方向回归至全局路径的轨迹，因此
Figure 673395DEST_PATH_IMAGE047
取较大值；
步骤5）根据机器人当前位置与障碍物位置，判断当前处于分段避障中的段数，以选择相应的评价函数，计算当前状态下评价函数的输入项，并对其做归一化处理；
步骤 6）待步骤5）中归一化处理完毕后，计算评价函数的最终值，得到最小的评价函数值所对应的速度，作为机器人运行的最优速度；
步骤 7）执行步骤6）中的最优速度，并根据机器人当前位置判断机器人是否到达局部目标点
Figure 548948DEST_PATH_IMAGE003
，若到达则避障结束，否则返回步骤 3）继续循环计算。
2.根据权利要求1所述的一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法，其特征在于，步骤3）中，约束因素包括运动学约束、安全容许的速度约束、动力学约束及最小转弯半径约束。
CN202011464715.4A 2020-12-12 2020-12-12 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法 Active CN112506199B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011464715.4A CN112506199B (zh) 2020-12-12 2020-12-12 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011464715.4A CN112506199B (zh) 2020-12-12 2020-12-12 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法

Publications (2)

Publication Number Publication Date
CN112506199A CN112506199A (zh) 2021-03-16
CN112506199B true CN112506199B (zh) 2022-12-02

Family

ID=74972161

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011464715.4A Active CN112506199B (zh) 2020-12-12 2020-12-12 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法

Country Status (1)

Country Link
CN (1) CN112506199B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113534815A (zh) * 2021-08-05 2021-10-22 上海高仙自动化科技发展有限公司 一种避障路径确定方法、装置、电子设备、芯片及介质
CN113703448A (zh) * 2021-08-19 2021-11-26 深圳亿嘉和科技研发有限公司 一种基于超声波的阿克曼底盘避障控制方法
CN114995398A (zh) * 2022-05-16 2022-09-02 中国第一汽车股份有限公司 路径生成方法、装置、存储介质、处理器及电子装置
CN115016510A (zh) * 2022-08-08 2022-09-06 武汉工程大学 一种机器人导航避障方法、装置以及存储介质
CN115826588B (zh) * 2023-02-14 2023-05-12 北京国安广传网络科技有限公司 基于lbs数据对健康管理机器人路径进行修正的规划模块

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101738195A (zh) * 2009-12-24 2010-06-16 厦门大学 基于环境建模与自适应窗口的移动机器人路径规划方法
CN103064416A (zh) * 2012-12-10 2013-04-24 江西洪都航空工业集团有限责任公司 巡检机器人室内外自主导航系统
CN106406320A (zh) * 2016-11-29 2017-02-15 重庆重智机器人研究院有限公司 机器人路径规划方法及规划路线的机器人
CN106708058A (zh) * 2017-02-16 2017-05-24 上海大学 一种基于ros的机器人物品传送方法和控制系统
CN107102643A (zh) * 2017-06-19 2017-08-29 江西洪都航空工业集团有限责任公司 一种大型智能割草机器人p‑d型路径规划算法
CN107703948A (zh) * 2017-11-14 2018-02-16 上海理工大学 基于自适应动态窗口的移动机器人局部动态路径规划方法
WO2019060679A1 (en) * 2017-09-22 2019-03-28 Locus Robotics Corp. DYNAMIC WINDOW APPROACH USING AN OPTIMUM RECIPROCAL COLLISION AVIATION COST CRITICAL
CN109764886A (zh) * 2019-01-15 2019-05-17 成都信息工程大学 一种路径规划方法
CN111045433A (zh) * 2019-12-31 2020-04-21 深圳前海达闼云端智能科技有限公司 一种机器人的避障方法、机器人及计算机可读存储介质
CN111123947A (zh) * 2019-12-31 2020-05-08 深圳前海达闼云端智能科技有限公司 机器人行进的控制方法、装置、电子设备、介质和机器人
CN111665847A (zh) * 2020-06-24 2020-09-15 上海应用技术大学 一种基于模糊逻辑的动态窗口避障算法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8060306B2 (en) * 2007-10-04 2011-11-15 Deere & Company Method and system for obstacle avoidance for a vehicle

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101738195A (zh) * 2009-12-24 2010-06-16 厦门大学 基于环境建模与自适应窗口的移动机器人路径规划方法
CN103064416A (zh) * 2012-12-10 2013-04-24 江西洪都航空工业集团有限责任公司 巡检机器人室内外自主导航系统
CN106406320A (zh) * 2016-11-29 2017-02-15 重庆重智机器人研究院有限公司 机器人路径规划方法及规划路线的机器人
CN106708058A (zh) * 2017-02-16 2017-05-24 上海大学 一种基于ros的机器人物品传送方法和控制系统
CN107102643A (zh) * 2017-06-19 2017-08-29 江西洪都航空工业集团有限责任公司 一种大型智能割草机器人p‑d型路径规划算法
WO2019060679A1 (en) * 2017-09-22 2019-03-28 Locus Robotics Corp. DYNAMIC WINDOW APPROACH USING AN OPTIMUM RECIPROCAL COLLISION AVIATION COST CRITICAL
CN107703948A (zh) * 2017-11-14 2018-02-16 上海理工大学 基于自适应动态窗口的移动机器人局部动态路径规划方法
CN109764886A (zh) * 2019-01-15 2019-05-17 成都信息工程大学 一种路径规划方法
CN111045433A (zh) * 2019-12-31 2020-04-21 深圳前海达闼云端智能科技有限公司 一种机器人的避障方法、机器人及计算机可读存储介质
CN111123947A (zh) * 2019-12-31 2020-05-08 深圳前海达闼云端智能科技有限公司 机器人行进的控制方法、装置、电子设备、介质和机器人
CN111665847A (zh) * 2020-06-24 2020-09-15 上海应用技术大学 一种基于模糊逻辑的动态窗口避障算法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Liang Yao ; Junxiang Li ; Bohan Jiang ; Daxue Liu.Model-based trajectory prediction approach using an improved dynamic window approach and an interactive behaviour model.《2019 IEEE International Conference on Unmanned Systems (ICUS)》.2020, *
基于全局地图的移动机器人路径规划研究;高佳佳;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20200315;第I140-358页 *
基于动态窗口法的移动机器人自动避障导航研究;彭育强等;《自动化仪表》;20201031;第26-29、33页 *
基于多维激光雷达的智能车局部路径规划研究;陈铁桩;《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》;20200315;第C035-361页 *
改进动态窗口法的阿克曼移动机器人局部路径规划器;徐保来等;《工业自动化》;20161231;第21-26页 *

Also Published As

Publication number Publication date
CN112506199A (zh) 2021-03-16

Similar Documents

Publication Publication Date Title
CN112506199B (zh) 一种基于动态窗口法且适用于阿克曼模型机器人的局部路径规划方法
CN107703948B (zh) 基于自适应动态窗口的移动机器人局部动态路径规划方法
Stahl et al. Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios
CN107168305B (zh) 路口场景下基于Bezier和VFH的无人车轨迹规划方法
CN104933228B (zh) 基于速度障碍的无人车实时轨迹规划方法
KR101585504B1 (ko) 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치
Braunstingl et al. Fuzzy logic wall following of a mobile robot based on the concept of general perception
CN112639849A (zh) 路径选择方法和路径选择装置
CN113050646B (zh) 一种用于室内移动机器人动态环境路径规划方法
Ge et al. Sensor-based path planning for nonholonomic mobile robots subject to dynamic constraints
CN113895463B (zh) 一种适用于自动驾驶车辆掉头的路径规划方法
López et al. Obstacle avoidance in dynamic environments based on velocity space optimization
CN111665847A (zh) 一种基于模糊逻辑的动态窗口避障算法
Mouhagir et al. A markov decision process-based approach for trajectory planning with clothoid tentacles
Gong et al. VPH+: An enhanced vector polar histogram method for mobile robot obstacle avoidance
CN114237256B (zh) 一种适用于欠驱动机器人的三维路径规划与导航方法
Hamad et al. Path Planning of Mobile Robot Based on Modification of Vector Field Histogram using Neuro-Fuzzy Algorithm.
CN114428499A (zh) 一种融合Astar与DWA算法的移动小车路径规划方法
Kim et al. A heuristic obstacle avoidance algorithm using vanishing point and obstacle angle
Raiesdana A hybrid method for industrial robot navigation
Parasuraman Sensor fusion for mobile robot navigation: Fuzzy Associative Memory
CN115729238A (zh) 一种用于移动机器人自主避障的动态路径规划方法
CN116009530A (zh) 一种自适应切向避障的路径规划方法和系统
Chiu et al. Fuzzy obstacle avoidance control of a two-wheeled mobile robot
CN114167856A (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