CN114234968B - 一种基于a星算法的移动机器人自主导航方法 - Google Patents

Abstract

一种基于A星算法的移动机器人自主导航方法，首先在传统A星算法规划路径中选取符合条件的关键点用直线连接生成直线路径，有效减小路径长度；而后根据移动机器人运动学约束在相邻直线路径的转折角附近计算满足运动学约束的起始点和终止点，以生成平滑的圆弧路径，使得移动机器人更加平稳跟随路径；最后获取移动机器人当前状态和路径信息，判断移动机器人当前处于的区间后计算移动机器人的速度，从而控制移动机器人沿路径运动直至目标点，有效控制移动机器人安全，同时保证移动机器人高效沿路径运动至目标点。

Description

一种基于A星算法的移动机器人自主导航方法

技术领域

本发明涉及机器人导航技术领域，尤其涉及一种基于A星算法的移动机器人自主导航方法。

背景技术

随着科技的发展与人们生活水平的提高，移动机器人在军工、农业、医疗等各个领域得到广泛的应用，人们对移动机器人的要求也越来越高，而移动机器人的工作环境也越来越复杂，如何在复杂的室内环境完成自主导航是该领域的重要研究问题，特别是路径规划和运动控制是移动机器人实现自主导航的关键所在。

A星算法是在路径规划领域常用的一种启发搜索算法，其结合Dijkstar算法与BFS算法的优点，搜索路径时对每个可能的节点引入全局信息，再对当前节点距终点的距离做出估计，在A星算法中，定义了两个列表：OPENlist和CLOSElist，列表OPENlist中存放待检查的路径点，初始化时OPENlist中只包含一个路径起始点，列表CLOSElist中存放已经检查过的路径点，初始化时CLOSElist为空；在进行路径规划时，从候选节点中选取代价最小的点作为最优路径点，如果节点n是目标点，表示已经计算出最优路径，路径搜索结束，否则将节点n从OPENlist列表中去除，并将其添加到CLOSElist列表，同时将与节点n相邻不在OPENlist列表和CLOSElist列表的节点添加至OPENlist列表，A星算法利用这种方法进行循环搜索，直至寻找到最优路径。但传统的A星算法存在以下问题：(1)A星算法不能保证得到最优解，(2)A星算法未考虑机器人运动学约束，规划出的路径存在很多折线和拐角，不够平滑，机器人易受运动学约束造成无法到达。

发明内容

本发明所解决的技术问题在于提供一种基于A星算法的移动机器人自主导航方法，以解决上述背景技术中的问题。

本发明所解决的技术问题采用以下技术方案来实现：

一种基于A星算法的移动机器人自主导航方法，在传统A星算法规划路径中选取符合条件的关键点用直线连接生成直线路径，在相邻直线路径的转折角附近计算满足运动学约束的起始点和终止点以生成平滑的圆弧路径，而后获取移动机器人当前状态和路径信息，判断移动机器人当前处于的区间后计算移动机器人的速度，从而控制移动机器人沿路径运动直至目标点，具体步骤如下：

步骤1)设置各项参数，移动机器人运动速度为v，路径步长的阈值为D，p₁为起始点，p_g为目标点；

步骤2)在移动机器人所加载的环境地图中利用A星算法搜索路径，生成的初始路径点记为pathpoint＝{p₁,p₂…p_m,p_g}，同时剔除初始路径点中不符合要求的路径点，并生成新路径点集合记为pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}，依次用直线段连接pathopen列表中的关键路径点生成n+1条直线路径记为L₁,L₂…L_n+1；

步骤3)设定移动机器人沿直线路径运动进入下一条直线路径时的转弯半径r和转弯时线速度v₀，根据转弯半径和相邻两直线方程计算移动机器人按圆弧运动的角速度与旋转方向；

步骤4)根据移动机器人自带的传感器获取移动机器人当前位置坐标(x，y)与移动机器人航向角β；

步骤5)判断移动机器人是否到达圆弧路径起始点p_c1，若是进入圆弧控制，否则进入步骤6)；

步骤6)计算移动机器人当前位置到直线路径的距离偏差e_error与偏转角Δθ；

步骤7)将移动机器人的距离偏差e_error划分为2个区间，A：|e_error|≤0.6，B：|e_error|＞0.6判断移动断机器人当前距离偏差e_error所属的区间，选择对应的角速度、线速度计算公式；

步骤8)通过步骤7)中获取移动机器人的角速度和线速度，判断移动机器人是否到达目标点，若是，则路径跟踪结束；否则跳入步骤4)重复执行，直至到达目标点。

在本发明中，所述步骤2)具体为：

Step2-1)定义列表pathopen，将起点p₁放入pathopen列表；

Step2-2)计算最新加入列表pathopen中的路径点p₁到其后的子节点p₂的距离d₁，若d₁＜D，继续计算p₂的子节点p₃到p₁的距离d₂，若d₂＜D，重复执行Step2-2)，直到找到一个点p_t满足：p_t到p₁的距离d_t-1≥D，或p_t为目标点，进入Step2-3)；

Step2-3)连接p₁、p_t点生成直线路径p₁p_t，计算直线路径p₁p_t上是否有障碍物，若有障碍物，查看p_t的父节点p_t-1，计算直线路径p₁p_t-1是否有障碍物，重复执行Step2-2)，直到找到一点p_t-m，满足直线路径p₁p_t-m上无障碍物，将p_t-m加入pathopen列表；

Step2-4)重复执行步骤Step2-2)～Step2-3)，直至将目标点p_g加入pathopen列表，得到一系列新的路径点：pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}。

在本发明中，所述步骤3)具体为：

假设p’₁p’₂为移动机器人当前跟踪的直线路径，p’₂p’₃为移动机器人将要跟踪的下一条直线路径，圆弧p_c1p_c2为移动机器人从直线路径p’₁p’₂过渡到直线路径p’₂p’₃的圆弧路径，O为圆弧路径所在圆的圆心，r为圆O的半径；

Step3-1)计算圆心o(x₀,y₀)的坐标，已知p’1,p’₂,p’₃的坐标可得直线p’₁p’₂，p’₂p’₃的方程分别为：

A₁x+B₁y+C₁＝0 (1)

A₂x+B₂y+C₂＝0 (2)

其中，A₁,B₁,C₁为p’₁p’₂的直线方程系数，A₂,B₂,C₂为p’₂p’₃的直线方程系数，圆心o(x₀,y₀)到p’₁p’₂和p’₂p’₃的距离均为r，可得：

由公式(1)～(4)可得圆心O坐标(x₀,y₀)；

Step3-2)计算圆弧起始点p_c1和圆弧终止点p_c2的坐标：

由几何关系可得op_c1⊥p₁’p’₂，op_c2⊥p₂’p’₃，因此op_c1和op_c2的直线方程分别为：

A₁y-B₁x+B₁x₀-A₁y₀＝0 (5)

A₂y-B₂x+B₂x₀-A₂y₀＝0 (6)

由公式(1)、(2)、(5)、(6)可计算得圆弧起始点p_c1(x_c1,y_c1)和圆弧终止点p_c2(x_c2,y_c2)的坐标；

Step3-3)计算移动机器人按圆弧运动的角速度

ω＝v/r (7)

Step3-4)计算圆弧的旋转方向

由p’₁(x₁,y₁),p’₂(x₂,y₂),p’₃(x₃,y₃)的坐标可得p’₁p’₂和p’₁p’₃与x轴夹角α,β分别为：

若α＞β，移动机器人顺时针旋转，移动机器人角速度小于0，即ω＝-v/r，若α＜β，移动机器人逆时针旋转，移动机器人角速度大于0，ω＝v/r。

在本发明中，所述步骤6)具体为：

假设移动机器人将要跟踪的直线路径为p’₁p’₂，直线路径起始点为p’₁(x₁,y₁)，终止点为p’₂(x₂,y₂)，机器人的坐标p(x,y)：

若则移动机器人的位置p在直线路径/>的右侧，移动机器人到直线路径的距离偏差：

若则移动机器人的位置p在直线路径/>的左侧，移动机器人到直线路径的距离偏差：

直线路径p’₁p’₂与x轴夹角为

由移动机器人自带的传感器可得移动机器人航向与x轴夹角为因此移动机器人航向与路径的偏转角/>

在本发明中，所述步骤7)具体为：

1)当e_error∈A，移动机器人偏离路径的误差较小，移动机器人以较小的角速度和线速度运动，其角速度计算公式为：

角速度：ω＝λ₁*e_error+λ₂*Δθ

线速度：V＝k*(0.6-|e_error|)+v/2

其中，λ₁，λ₂，k为系数；

2)当e_error∈B，移动机器人偏离路径较远，移动机器人的运动分两段，先以零速，即线速度为0转过一定的角度，而后再以较大的线速度和角速度向直线路径运动：

第一段运动：

线速度v＝0，角速度ω＝ω_max，运动时间t，t满足：

t＝(Δθ+π/4)/ω_max

第二段运动：

角速度：ω＝2λ₁*e_error+2λ₂*Δθ

线速度：V＝k*|e_error|+v

其中，λ₁，λ₂，k为系数。

有益效果：

1、本发明在传统的A星算法规划路径中选取符合条件的关键点用直线连接生成直线路径，有效减小路径长度；

2、本发明在相邻直线路径的转折角附近计算满足运动学约束的起始点和终止点，生成平滑的圆弧路径，使得机器人更加平稳跟随路径；

3、本发明优化路径，有效控制移动机器人安全，同时保证移动机器人高效沿路径运动至目标点。

附图说明

图1为本发明的较佳实施例的路径列表生成示意图。

图2为本发明的较佳实施例中的移动机器人圆弧路径示意图。

图3为本发明的较佳实施例中的移动机器人跟随直线路径示意图。

具体实施方式

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明。

一种基于A星算法的移动机器人自主导航方法，在传统A星算法规划路径中选取符合条件的关键点用直线连接生成直线路径，在相邻直线路径的转折角附近计算满足运动学约束的起始点和终止点生成平滑的圆弧路径，而后获取移动机器人当前状态和路径信息，以判断移动机器人当前处于的区间后计算移动机器人的速度，从而控制移动机器人沿路径运动直至目标点，具体步骤如下：

步骤1)设置各项参数，移动机器人运动速度为v，路径步长的阈值为D，p₁为起始点，p_g为目标点；

步骤2)在移动机器人所加载的环境地图中利用A星算法搜索路径，生成的初始路径点记为pathpoint＝{p₁,p₂…p_m,p_g}，同时剔除初始路径点中不符合要求的路径点，并生成新路径点集合记为pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}，依次用直线段连接pathopen列表中的关键路径点生成n+1条直线路径记为L₁,L₂…L_n+1，如图1所示，其中，圆点为经过优化后生成的pathopen列表中的点，三角点为不符合要求被剔除的点；

步骤3)设定移动机器人沿直线路径运动进入下一条直线路径时的转弯半径r和转弯时线速度v₀，根据转弯半径和相邻两直线方程计算机器人按圆弧运动的角速度和旋转方向；

步骤4)根据移动机器人自带的传感器获取移动机器人当前位置坐标(x，y)与移动机器人航向角β；

步骤5)判断移动机器人是否到达圆弧路径起始点p_c1，若是进入圆弧控制，否则进入步骤6)；

步骤6)计算移动机器人当前位置到直线路径的距离偏差e_error与偏转角Δθ；

步骤7)将移动机器人的距离偏差e_error划分为2个区间，A：|e_error|≤0.6，B：|e_error|＞0.6判断移动断机器人当前距离偏差e_error所属的区间，选择对应的角速度、线速度计算公式；

步骤8)获取移动机器人的角速度和线速度，发送给移动机器人，判断移动机器人是否到达目标点，若是，则路径跟踪结束；否则跳入步骤4)重复执行，直至到达目标点。

在本实施例中，所述步骤2)具体为：

Step2-1)定义列表pathopen，将起点p₁放入pathopen列表；

Step2-2)计算最新加入列表pathopen中的路径点p₁到其后的子节点p₂的距离d₁，若d₁＜D，继续计算p₂的子节点p₃到p₁的距离d₂，若d₂＜D，重复执行Step2-2)，直到找到一个点p_t满足：p_t到p₁的距离d_t-1≥D，或p_t为目标点，进入Step2-3)；

Step2-3)连接p₁、p_t点生成直线路径p₁p_t，计算直线路径p₁p_t上是否有障碍物，若有障碍物，查看p_t的父节点p_t-1，计算直线路径p₁p_t-1是否有障碍物，重复执行Step2-2)，直到找到一点p_t-m，满足直线路径p₁p_t-m上无障碍物，将p_t-m加入pathopen列表；

Step2-4)重复执行步骤Step2-2)～Step2-3)，直至将目标点p_g加入pathopen列表，得到一系列新的路径点：pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}。

在本实施例中，所述步骤3)具体为：

如图2所示，p’₁p’₂为移动机器人当前跟踪的直线路径，p’₂p’₃为移动机器人将要跟踪的下一条直线路径，圆弧p_c1p_c2为移动机器人从直线路径p’₁p’₂过渡到直线路径p’₂p’₃的圆弧路径，O为圆弧路径所在圆的圆心，r为圆O的半径；

Step3-1)计算圆心o(x₀,y₀)的坐标，已知p’₁,p’₂,p’₃的坐标可得直线p’₁p’₂，p’₂p’₃的方程分别为：

A₁x+B₁y+C₁＝0 (1)

A₂x+B₂y+C₂＝0 (2)

其中，A₁,B₁,C₁为p’₁p’₂的直线方程系数，A₂,B₂,C₂为p’₂p’₃的直线方程系数，圆心o(x₀,y₀)到p’₁p’₂和p’₂p’₃的距离均为r，可得：

由公式(1)～(4)可得圆心O坐标；

Step3-2)计算圆弧起始点p_c1和圆弧终止点p_c2的坐标；

由图2中几何关系可得op_c1⊥p’₁p’₂，op_c2⊥p’₂p’₃，因此op_c1和op_c2的直线方程分别为：

A₁y-B₁x+B₁x₀-A₁y₀＝0 (5)

A₂y-B₂x+B₂x₀-A₂y₀＝0 (6)

由公式(1)、(2)、(5)、(6)可计算得圆弧起始点p_c1(x_c1,y_c1)和圆弧终止点p_c2(x_c2,y_c2)的坐标；

Step3-3)计算移动机器人按圆弧运动的角速度

ω＝v/r (7)

Step3-4)计算圆弧的旋转方向

由p’₁(x₁,y₁),p’₂(x₂,y₂),p’₃(x₃,y₃)的坐标可得p’₁p’₂和p’₁p’₃与x轴夹角α,β分别为：

若α＞β，移动机器人顺时针旋转，移动机器人角速度小于0，即ω＝-v/r，若α＜β，移动机器人逆时针旋转，移动机器人角速度大于0，ω＝v/r。

在本实施例中，所述步骤6)具体为：

如图3所示，移动机器人将要跟踪的直线路径为p’₁p’₂，直线路径起始点为p’₁(x₁,y₁)，终止点为p’₂(x₂,y₂)，机器人的坐标p(x,y)：

若则移动机器人的位置p在直线路径/>的右侧，移动机器人到直线路径的距离偏差：

若则移动机器人的位置p在直线路径/>的左侧，移动机器人到直线路径的距离偏差：

直线路径p’₁p’₂与x轴夹角为

由移动机器人自带的传感器可得移动机器人航向与x轴夹角为因此移动机器人航向与路径的偏转角/>

在本实施例中，所述步骤7)具体为：

1)当e_error∈A，移动机器人偏离路径的误差较小，移动机器人以较小的角速度和线速度运动，其角速度计算公式为：

角速度：ω＝λ₁*e_error+λ₂*Δθ

线速度：V＝k*(0.6-|e_error|)+v/2

其中，λ₁，λ₂，k为系数；

2)当e_error∈B，移动机器人偏离路径较远，移动机器人的运动分两段，先以零速，即线速度为0转过一定的角度，而后再以较大的线速度和角速度向直线路径运动：

第一段运动：

线速度v＝0，角速度ω＝ω_max，运动时间t，t满足：

t＝(Δθ+π/4)/ω_max

第二段运动：

角速度：ω＝2λ₁*e_error+2λ₂*Δθ

线速度：V＝k*|e_error|+v

其中，λ₁，λ₂，k为系数。

Claims (2)

1.一种基于A星算法的移动机器人自主导航方法，其特征在于，首先在传统A星算法规划路径中选取符合条件的关键点用直线连接生成直线路径，而后根据移动机器人运动学约束在相邻直线路径的转折角附近计算满足运动学约束的起始点和终止点，以生成平滑的圆弧路径；最后获取移动机器人当前状态和路径信息，判断移动机器人当前处于的区间后计算移动机器人的速度，从而控制移动机器人沿路径运动直至目标点；具体步骤如下：

步骤1)设置参数

移动机器人运动速度为v，路径步长的阈值为D，p₁为起始点，p_g为目标点；

步骤2)生成直线路径

在移动机器人所加载的环境地图中利用A星算法搜索路径，生成的初始路径点记为pathpoint＝{p₁,p₂…p_m,p_g}，同时剔除初始路径点中不符合要求的路径点，并生成新路径点集合记为pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}，依次用直线段连接pathopen列表中的关键路径点生成n+1条直线路径记为L₁,L₂…L_n+1；

步骤3)计算圆弧路径

设定移动机器人沿直线路径运动进入下一条直线路径时的转弯半径r和转弯时线速度v₀，根据转弯半径和相邻两直线方程计算机器人按圆弧运动的角速度和旋转方向；具体为：

假设p’₁p’₂为移动机器人当前跟踪的直线路径，p’₂p’₃为移动机器人将要跟踪的下一条直线路径，圆弧p_c1p_c2为移动机器人从直线路径p’₁p’₂过渡到直线路径p’₂p’₃的圆弧路径，O为圆弧路径所在圆的圆心，r为圆O的半径；

Step3-1)计算圆心o(x₀,y₀)的坐标，已知p’₁,p’₂,p’₃的坐标可得直线p’₁p’₂，p’₂p’₃的方程分别为：

A₁x+B₁y+C₁＝0 (1)

A₂x+B₂y+C₂＝0 (2)

其中，A₁,B₁,C₁为p’₁p’₂的直线方程系数，A₂,B₂,C₂为p’₂p’₃的直线方程系数，圆心o(x₀,y₀)到p’₁p’₂和p’₂p’₃的距离均为r，可得：

由公式(1)～(4)可得圆心O坐标(x₀,y₀)；

Step3-2)计算圆弧起始点p_c1和圆弧终止点p_c2的坐标：

由几何关系可得op_c1⊥p’₁p’₂，op_c2⊥p’₂p’₃，因此op_c1和op_c2的直线方程分别为：

A₁y-B₁x+B₁x₀-A₁y₀＝0 (5)

A₂y-B₂x+B₂x₀-A₂y₀＝0 (6)

由公式(1)、(2)、(5)、(6)可计算得圆弧起始点p_c1(x_c1,y_c1)和圆弧终止点p_c2(x_c2,y_c2)的坐标；

Step3-3)计算移动机器人按圆弧运动的角速度

ω＝v/r (7)

Step3-4)计算圆弧的旋转方向

由p’₁(x₁,y₁),p’₂(x₂,y₂),p’₃(x₃,y₃)的坐标可得p’₁p’₂和p’₁p’₃与x轴夹角α,β分别为：

若α＞β，移动机器人顺时针旋转，移动机器人角速度小于0，即ω＝-v/r，若α＜β，移动机器人逆时针旋转，移动机器人角速度大于0，ω＝v/r；

步骤4)获取当前状态和路径信息

根据移动机器人自带的传感器获取移动机器人当前位置坐标(x，y)与移动机器人航向角β；

步骤5)判断移动机器人当前处于的区间

判断移动机器人是否到达圆弧路径起始点p_c1，若是进入圆弧控制，否则进入步骤6)；

步骤6)计算移动机器人距离偏差

计算移动机器人当前位置到直线路径的距离偏差e_error与偏转角Δθ；具体为：

假设移动机器人将要跟踪的直线路径为p’₁p’₂，直线路径起始点为p’₁(x₁,y₁)，终止点为p’₂(x₂,y₂)，机器人的坐标p(x,y)：

若则移动机器人的位置p在直线路径/>的右侧，移动机器人到直线路径的距离偏差：

若则移动机器人的位置p在直线路径/>的左侧，移动机器人到直线路径的距离偏差：

直线路径p’₁p’₂与x轴夹角为

由移动机器人自带的传感器可得移动机器人航向与x轴夹角为因此移动机器人航向与路径的夹角/>

步骤7)选择计算公式

将移动机器人的距离偏差e_error划分为2个区间，A：|e_error|≤0.6，B：|e_error|＞0.6，判断移动断机器人当前距离偏差e_error所属的区间，选择对应的角速度、线速度计算公式；具体为：

1)当e_error∈A，移动机器人偏离路径的误差较小，移动机器人以较小的角速度和线速度运动，其角速度计算公式为：

角速度：ω＝λ₁*e_error+λ₂*Δθ

线速度：V＝k*(0.6-|e_error|)+v/2

其中，λ₁，λ₂，k为系数；

2)当e_error∈B，移动机器人偏离路径较远，移动机器人的运动分两段，先以零速，即线速度为0转过一定的角度，而后再以较大的线速度和角速度向直线路径运动：

第一段运动：

线速度v＝0，角速度ω＝ω_max，运动时间t，t满足：

t＝(Δθ+π/4)/ω_max；

第二段运动：

角速度：ω＝2λ₁*e_error+2λ₂*Δθ

线速度：V＝k*|e_error|+v

其中，λ₁，λ₂，k为系数；

步骤8)执行沿路径运动

通过步骤7)中获取移动机器人的角速度和线速度，判断移动机器人是否到达目标点，若是，则路径跟踪结束；否则跳入步骤4)重复执行，直至到达目标点。

2.根据权利要求1所述的一种基于A星算法的移动机器人自主导航方法，其特征在于，步骤2)中，具体为：

Step2-1)定义列表pathopen，将起点p₁放入pathopen列表；

Step2-2)计算最新加入列表pathopen中的路径点p₁到其后的子节点p₂的距离d₁，若d₁＜D，继续计算p₂的子节点p₃到p₁的距离d₂，若d₂＜D，重复执行Step2-2)，直到找到一个点p_t满足：p_t到p₁的距离d_t-1≥D，或p_t为目标点，进入Step2-3)；

Step2-3)连接p₁、p_t点生成直线路径p₁p_t，计算直线路径p₁p_t上是否有障碍物，若有障碍物，查看p_t的父节点p_t-1，计算直线路径p₁p_t-1是否有障碍物，重复执行Step2-2)，直到找到一点p_t-m，满足直线路径p₁p_t-m上无障碍物，将p_t-m加入pathopen列表；

Step2-4)重复执行步骤Step2-2)～Step2-3)，直至将目标点p_g加入pathopen列表，得到一系列新的路径点：pathopen＝{p₁,p_t1,p_t2,…p_tn,p_g}。