一种基于PSO参数整定的移动机器人平滑轨迹规划方法技术

一种基于PSO参数整定的移动机器人平滑轨迹规划方法，包括以下步骤：步骤1，获取室内场景的全局静态栅格地图，建立移动机器人工作环境的二维地图模型，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点；步骤2、设计改进A*算法的评价函数，并基于改进A*全局路径规划算法进行寻路，得到参考路径；步骤3、根据路径规划后得到的路径中的控制节点，利用经验参数的纯跟踪算法实现参考轨迹跟踪控制；步骤4、基于PSO算法对纯跟踪参数的智能化调试，选择迭代过程中适应度最优的轨迹，从而生成一条可行轨迹。从而生成一条可行轨迹。

A smooth trajectory planning method for mobile robot based on PSO parameter tuning

【技术实现步骤摘要】
一种基于PSO参数整定的移动机器人平滑轨迹规划方法


[0001]本专利技术属于机器人轨迹规划
，具体提供一种基于PSO参数整定的移动机器人平滑轨迹规划方法。在不同环境下为移动机器人全局规划出一条到目标点的无碰撞参考路径基础上，利用PSO参数整定的纯跟踪算法对参考路径跟踪生成符合机器人自身模型特点的平滑可行轨迹，且具有针对不同环境的适应性。

技术介绍

[0002]随着全球技术及时代的飞速发展，智能移动机器人的应用领域的日益扩大，移动机器人在诸多生活领域发挥着越来越重要的作用，人们期望智能移动机器人能在更多的领域为人类服务，代替人类完成更多更复杂的工作。为了实现生产更可靠、更熟练的机器人的有效方法，以及解决不同领域的问题和优化现有解决方案，在提高机器人的任务性能方面，机器人导航是移动机器人领域的重要课题。机器人路径规划是帮助机器人在环境中导航的主要任务之一，在过去几十年中受到了广泛关注。机器人的路径规划技术，其实是参照某一个参数的指标(如工作代价值最低，选择路径最短，运算时间消耗最短等)，在任务区域选择出一条可从起点连接到终点的最优或次优的避障路径。其本质是在几个约束条件下得到最优或可行解的问题。路径规划结果的优劣，将直观地对机器人完成任务的实时性及结果优劣造成影响。
[0003]路径规划算法可以分为两类：全局规划和局部规划。目前应用较多的有遗传算法、蚁群算法、Dijkstra算法、A*算法、D*Lite算法、人工势场法及此类改进算法等。其中A*算法的核心思想是利用启发函数对算法遍历的方向进行引导，避免盲目搜索，提高算法的收敛效率，但该算法不适用于存在动态障碍物的环境。动态窗口算法(DWA:Dynamic Window Approach)可以预估一定范围内动态障碍物在一段时间内的运动轨迹，规划路径以避开动态障碍物，但该算法对全局路径规划有维数爆炸增长特性。而且传统A*算法存在规划的路径搜索时间长、冗余节点多、“锯齿”形状的问题，并不符合实际需求，无法直接用于轮式移动机器人的跟踪，需要进行平滑处理。目前常用的路径平滑处理方法有贝塞尔曲线、多项式拟合、B样条曲线等，但是针对复杂多拐点的路径，一般拟合曲线的多项式阶数都会很高，容易震荡且高次多项式又会带来计算上的困难并且贝塞尔曲线也不能作局部修改。
[0004]轨迹跟踪控制通过跟踪算法解析运动航向角和跟踪速度，实现移动机器人的轨迹跟踪控制。其中纯跟踪控制是基于当前的后轮中心位置，在参考路径上向Ld(自定义)的距离匹配一个预瞄点作为跟踪点，通过调节轮式移动机器人的转向角来控制车辆当前姿态和目标路点在横向上的误差。但是该算法的跟踪效果完全依赖调试的参数，效率低下。尤其在动态变化环境下需要参数的不断调试，比较繁琐耗时，容易导致跟踪控制效果不佳。

技术实现思路

[0005]为了克服现有技术的不足，本专利技术提供了一种基于PSO参数整定的移动机器人平滑轨迹规划方法，在静态环境下为移动机器人全局规划出一条到目标点的无碰撞参考路径
基础之上，利用PSO参数整定的纯跟踪算法对参考路径跟踪生成符合机器人自身模型特点并满足曲率约束的可行轨迹，且具有针对不同环境的适应性。
[0006]为实现上述目的，本申请所采取的技术方案为：
[0007]一种基于PSO参数整定的移动机器人平滑轨迹规划方法，包括以下步骤：
[0008]步骤S1、获取室内场景的全局静态栅格地图，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点,建立移动机器人工作环境的二维地图模型，确定起始点、目标点和障碍物坐标；
[0009]步骤S2、在二维地图模型中，为避免路径规划中出现对角线方向上的路径与障碍物发生碰撞采用改进A*算法进行领域搜索，以提高生成路径的合理性；同时，通过引入障碍物比重信息改进代价函数，进而在保证搜索效率的前提下得到参考路径；
[0010]在使用改进后A*路径搜索算法搜索出从起点到目标点的一条最短路径，由于路径搜索只考虑机器人起点与目标点的位置，并不会考虑诸如当前速度、加速度以及移动机器人的运动学约束，这会导致找出的路径不符合移动机器人的运动学约束，如超过了机器人的最大速度及路径曲率不连续导致底层控制器无法进行轨迹跟踪。因此需要后续的轨迹优化工作；
[0011]步骤S3、生成可行轨迹：根据上述的改进A*算法路径规划后得到的路径中的控制节点利用纯跟踪算法实现参考轨迹跟踪控制，其控制算法的参数通过PSO依据不同环境智能化调试从而对轨迹进行平滑优化处理，即生成一条适应环境下的符合机器人自身模型特点并满足曲率约束的可行轨迹。
[0012]进一步，所述步骤S2中，具体评价函数设计如下：
[0013]在扩展当前节点的子节点时舍弃与上一个节点生成对角线和障碍物接触的子节点，以此避免路径规划中出现对角线方向上的路径与障碍物发生碰撞从而保证机器人与障碍物节点之间的安全距离；
[0014]引入障碍物占比p(n)来影响启发函数h(n)在不同环境下的权重，假设一局部区域中障碍物的数量为b，所有栅格数量为B，设当前节点的坐标为(n
x
,n
y
)，目标点的坐标为(g
x
,g
y
)，则B由式(1)表示
[0015]B＝[1+abs(n
x
‑
g
y
)]×
[1+abs(n
y
‑
g
y
)]ꢀꢀ
(1)
[0016]则p(n)如式(2)表示
[0017]P(n)＝b/B
ꢀꢀ
(2)
[0018]初始评价函数如式(3)所示：
[0019]f(n)＝g(n)+h(n)
ꢀꢀ
(3)
[0020]改进后的启发式函数为：
[0021][0022]其中n代表当前节点,f(n)表示移动机器人在当前节点的评价函数,g(n)表示移动机器人从起始点到当前点的实际代价，h(n)为启发函数；w1,w2分别表示为距离函数权重和障碍物占比权重。
[0023]再进一步，所述步骤S3的过程如下：
[0024]步骤S3.1、取全段路径规划得到的全路段路径中的控制节点；
是[0,1]范围内的随机数；P
i
是到i时粒子搜索到的最优位置；G
i
是到i时粒子群搜索到的最优位置；
[0049]在笛卡尔坐标系中，参考点与真实点间在X、Y轴方向的误差：
[0050]d
x
＝x
‑
x
des
,d
y
＝y
‑
y
des
ꢀꢀ
(15)
[0051]计算横向误差：
[0052]f(k)＝dy
×
cosθ
des
‑
dx
×
sinθ
des
ꢀꢀ
(16)<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于PSO参数整定的移动机器人平滑轨迹规划方法，其特征在于，所述方法包括以下步骤：步骤S1、获取室内场景的全局静态栅格地图，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点,建立移动机器人工作环境的二维地图模型，确定起始点、目标点和障碍物坐标；步骤S2、在二维地图模型中，为避免路径规划中出现对角线方向上的路径与障碍物发生碰撞采用改进A*算法进行领域搜索，以提高生成路径的合理性；同时，通过引入障碍物比重信息改进代价函数，进而在保证搜索效率的前提下得到参考路径；步骤S3、生成可行轨迹：根据上述的改进A*算法路径规划后得到的路径中的控制节点利用纯跟踪算法实现参考轨迹跟踪控制，其控制算法的参数通过PSO依据不同环境智能化调试从而对轨迹进行平滑优化处理，即生成一条适应环境下的符合机器人自身模型特点并满足曲率约束的可行轨迹。2.如权利要求1所述的基于PSO参数整定的移动机器人平滑轨迹规划方法，其特征在于，所述步骤S2中，评价函数设计如下：在扩展当前节点的子节点时舍弃与上一个节点生成对角线和障碍物接触的子节点，以此避免路径规划中出现对角线方向上的路径与障碍物发生碰撞从而保证机器人与障碍物节点之间的安全距离。；引入障碍物占比p(n)来影响启发函数h(n)在不同环境下的权重，假设一局部区域中障碍物的数量为b，所有栅格数量为B，设当前节点的坐标为(n
x
,n
y
)，目标点的坐标为(g
x
,g
y
)，则B由式(1)表示B＝[1+abs(n
x
‑
g
y
)]
×
[1+abs(n
y
‑
g
y
)]
ꢀꢀꢀꢀ
(1)则p(n)如式(2)表示P(n)＝b/B
ꢀꢀꢀꢀ
(2)初始评价函数如式(3)所示：f(n)＝g(n)+h(n)
ꢀꢀꢀꢀꢀ
(3)改进后的启发式函数为：其中n代表当前节点,f(n)表示移动机器人在当前节点的评价函数,g(n)表示移动机器人从起始点到当前点的实际代价，h(n)为启发函数；w1,w2分别表示为距离函数权重和障碍物占比权重。3.如权利要求2所述的基于PSO参数整定的移动机器人平滑轨迹规划方法，其特征在于，所述步骤S3的过程如下：步骤S3.1、取全段路径规划得到的全路段路径中的控制节点；步骤S3.2、纯跟踪算法轨迹跟踪拟合，g点为预瞄点，l
d
为车辆后轴中心至预瞄点的距离，即前视距离，α为车身姿态与后轴中心和预瞄点之间连线的方向之间的夹角，δ
f
为前轮偏角，l为轴距，根据正弦定理，有下式成立：
化简上式，得到：其中路径曲率所以上式写为：根据轮式移动机器人的运动学模型，有则另外，定义横向误差为机器人当前姿态和预瞄点在横向上的误差，并考虑小角度误差：所以纯跟踪本质上是一个P控制器，且由式(10)得前视距离与轨迹跟踪效果密切相关，是纯跟踪算法的重要参数，前视距离过大会降低控制精度，但跟踪产生的轨迹会更平滑；前视距离小，追踪拟合更精确，但容易产生振荡；定义前视距离为关于速度的一次多项式，如下式所示：l...

【专利技术属性】
技术研发人员：朱俊威，应良焕，胡公翰，黄怡宁，翁得鱼，
申请(专利权)人：浙江工业大学，
类型：发明
国别省市：