一种基于PSO参数整定的移动机器人平滑轨迹规划方法
文献发布时间：2023-06-19 16:06:26
技术领域
本发明属于机器人轨迹规划技术领域，具体提供一种基于PSO参数整定的移动机器人平滑轨迹规划方法。在不同环境下为移动机器人全局规划出一条到目标点的无碰撞参考路径基础上，利用PSO参数整定的纯跟踪算法对参考路径跟踪生成符合机器人自身模型特点的平滑可行轨迹，且具有针对不同环境的适应性。
背景技术
随着全球技术及时代的飞速发展，智能移动机器人的应用领域的日益扩大，移动机器人在诸多生活领域发挥着越来越重要的作用，人们期望智能移动机器人能在更多的领域为人类服务，代替人类完成更多更复杂的工作。为了实现生产更可靠、更熟练的机器人的有效方法，以及解决不同领域的问题和优化现有解决方案，在提高机器人的任务性能方面，机器人导航是移动机器人领域的重要课题。机器人路径规划是帮助机器人在环境中导航的主要任务之一，在过去几十年中受到了广泛关注。机器人的路径规划技术，其实是参照某一个参数的指标(如工作代价值最低，选择路径最短，运算时间消耗最短等)，在任务区域选择出一条可从起点连接到终点的最优或次优的避障路径。其本质是在几个约束条件下得到最优或可行解的问题。路径规划结果的优劣，将直观地对机器人完成任务的实时性及结果优劣造成影响。
路径规划算法可以分为两类：全局规划和局部规划。目前应用较多的有遗传算法、蚁群算法、Dijkstra算法、A*算法、D*Lite算法、人工势场法及此类改进算法等。其中A*算法的核心思想是利用启发函数对算法遍历的方向进行引导，避免盲目搜索，提高算法的收敛效率，但该算法不适用于存在动态障碍物的环境。动态窗口算法(DWA:Dynamic WindowApproach)可以预估一定范围内动态障碍物在一段时间内的运动轨迹，规划路径以避开动态障碍物，但该算法对全局路径规划有维数爆炸增长特性。而且传统A*算法存在规划的路径搜索时间长、冗余节点多、“锯齿”形状的问题，并不符合实际需求，无法直接用于轮式移动机器人的跟踪，需要进行平滑处理。目前常用的路径平滑处理方法有贝塞尔曲线、多项式拟合、B样条曲线等，但是针对复杂多拐点的路径，一般拟合曲线的多项式阶数都会很高，容易震荡且高次多项式又会带来计算上的困难并且贝塞尔曲线也不能作局部修改。
轨迹跟踪控制通过跟踪算法解析运动航向角和跟踪速度，实现移动机器人的轨迹跟踪控制。其中纯跟踪控制是基于当前的后轮中心位置，在参考路径上向Ld(自定义)的距离匹配一个预瞄点作为跟踪点，通过调节轮式移动机器人的转向角来控制车辆当前姿态和目标路点在横向上的误差。但是该算法的跟踪效果完全依赖调试的参数，效率低下。尤其在动态变化环境下需要参数的不断调试，比较繁琐耗时，容易导致跟踪控制效果不佳。
发明内容
为了克服现有技术的不足，本发明提供了一种基于PSO参数整定的移动机器人平滑轨迹规划方法，在静态环境下为移动机器人全局规划出一条到目标点的无碰撞参考路径基础之上，利用PSO参数整定的纯跟踪算法对参考路径跟踪生成符合机器人自身模型特点并满足曲率约束的可行轨迹，且具有针对不同环境的适应性。
为实现上述目的，本申请所采取的技术方案为：
一种基于PSO参数整定的移动机器人平滑轨迹规划方法，包括以下步骤：
步骤S1、获取室内场景的全局静态栅格地图，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点,建立移动机器人工作环境的二维地图模型，确定起始点、目标点和障碍物坐标；
步骤S2、在二维地图模型中，为避免路径规划中出现对角线方向上的路径与障碍物发生碰撞采用改进A*算法进行领域搜索，以提高生成路径的合理性；同时，通过引入障碍物比重信息改进代价函数，进而在保证搜索效率的前提下得到参考路径；
在使用改进后A*路径搜索算法搜索出从起点到目标点的一条最短路径，由于路径搜索只考虑机器人起点与目标点的位置，并不会考虑诸如当前速度、加速度以及移动机器人的运动学约束，这会导致找出的路径不符合移动机器人的运动学约束，如超过了机器人的最大速度及路径曲率不连续导致底层控制器无法进行轨迹跟踪。因此需要后续的轨迹优化工作；
步骤S3、生成可行轨迹：根据上述的改进A*算法路径规划后得到的路径中的控制节点利用纯跟踪算法实现参考轨迹跟踪控制，其控制算法的参数通过PSO依据不同环境智能化调试从而对轨迹进行平滑优化处理，即生成一条适应环境下的符合机器人自身模型特点并满足曲率约束的可行轨迹。
进一步，所述步骤S2中，具体评价函数设计如下：
在扩展当前节点的子节点时舍弃与上一个节点生成对角线和障碍物接触的子节点，以此避免路径规划中出现对角线方向上的路径与障碍物发生碰撞从而保证机器人与障碍物节点之间的安全距离；
引入障碍物占比p(n)来影响启发函数h(n)在不同环境下的权重，假设一局部区域中障碍物的数量为b，所有栅格数量为B，设当前节点的坐标为(n
B＝[1+abs(n
则p(n)如式(2)表示
P(n)＝b/B (2)
初始评价函数如式(3)所示：
f(n)＝g(n)+h(n) (3)
改进后的启发式函数为：
其中n代表当前节点,f(n)表示移动机器人在当前节点的评价函数,g(n)表示移动机器人从起始点到当前点的实际代价，h(n)为启发函数；w
再进一步，所述步骤S3的过程如下：
步骤S3.1、取全段路径规划得到的全路段路径中的控制节点；
步骤S3.2、纯跟踪算法轨迹跟踪拟合，
g点为预瞄点，l
化简上式，得到：
其中路径曲率
根据轮式移动机器人的运动学模型，有
则
另外，定义横向误差为机器人当前姿态和预瞄点在横向上的误差，并考虑小角度误差：
所以纯跟踪本质上是一个P控制器，且由式(10)可得前视距离与轨迹跟踪效果密切相关，是纯跟踪算法的重要参数，前视距离过大会降低控制精度，但跟踪产生的轨迹会更平滑；前视距离小，追踪拟合更精确，但容易产生振荡。通常定义前视距离为关于速度的一次多项式，如下式所示：
l
计算加速度：
a＝k
其中设v
步骤S3.3、基于PSO算法的纯跟踪参数整定优化，包括：
根据式(11)，(12)，(13)可知需要调试的参数有：k
PSO算法公式如式(14)(15)所示：
v
x
其中，i为粒子序号；x
在笛卡尔坐标系中，参考点与真实点间在X、Y轴方向的误差：
d
计算横向误差：
f(k)＝dy×cosθ
设计适应度函数如下式：
其中k表示实际跟踪的序列点，r表示该条轨迹序列点的平均曲率；
所述PSO算法流程如下：
Step1：迭代次数为n＝1,在搜索空间随机初始化粒子群，第i个粒子的位置向量为
Step2:将每个粒子的位置向量元素依次作为参数k
Step3:用每个粒子的当前适应度值
Step4:用每个粒子的当前适应度值
Step5:按照式子(14)和(15)更新粒子的位置
Step6:迭代次数n＝n+1,返回Step2，直至达到最大迭代次数N；
按照以上算法进行迭代，求得纯跟踪控制的最优参数k
本发明提供了一种基于PSO参数整定的移动机器人平滑轨迹规划方法，在静态环境下为移动机器人全局规划出一条到目标点的无碰撞参考路径基础之上，该方法极大的降低了初始路径的曲率，基于几何跟踪优化所生成的机器人轨迹符合了机器人自身模型特点并满足了曲率约束，并且智能化的参数调试提高了在不同环境下的适应性，更符合实际的应用场景需求。
与现有的一类机器人路径规划与轨迹平滑算法，主要有遗传算法、蚁群算法、Dijkstra算法、A*算法、D*Lite算法、人工势场法、贝塞尔曲线、多项式拟合、B样条曲线等方法相比，本发明的有益效果表现在于：应用PSO算法对跟踪算法的智能化的参数调试，极大地节省了繁琐的参数调试的时间，且其调试出的参数使得轨迹优化后的平均曲率小。同时该方法可适用于不同环境下的智能化参数调试。
附图说明
图1是基于PSO参数整定的移动机器人平滑轨迹规划方法的流程示意图。
图2是静态环境下移动机器人路径规划图。
图3是初始规划路径曲率图。
图4是纯跟踪算法示意图。
图5是横向误差示意图。
图6是基于PSO算法的迭代过程图
图7是静态环境下移动机器人轨迹优化图。
图8是轨迹优化后曲率图。
图9是轨迹拟合误差对比图。
具体1实施方式
为使本发明的目的、技术方案和优点更加清晰，下面结合附图和具体实施例对本发明的技术方案作进一步详细说明。
参照图1～图9，一种基于PSO参数整定的移动机器人平滑轨迹规划方法，包括以下步骤：
S1、获取室内场景的全局静态栅格地图，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点；
S2、设计改进A*算法评价函数，并基于改进A*全局路径规划算法进行寻路，得到参考路径；
S3、根据路径规划后得到的路径中的控制节点，利用经验参数的纯跟踪算法实现参考轨迹跟踪控制；
S4、基于PSO算法对纯跟踪参数的智能化调试从而生成一条可行轨迹。
在本发明的一个实施例中，栅格地图中包括8个邻居节点，根据栅格地图中的对称原则，可以将栅格地图中的移动分为直线移动和对角线移动。
步骤S1、获取室内场景的全局静态栅格地图，栅格地图的每个节点代表当前位置是障碍物或可通行区域，并确认起始点和目标点。建立25×22的栅格地图，每个单元大小为1×1m，并初始设定起始点坐标(2，3)、障碍物若干个和目标点坐标(22,20)。
步骤S2、设计改进A*算法评价函数，并基于改进A*全局路径规划算法进行寻路，得到参考路径。
考虑轮式移动机器人的运动路径的连续、转弯角度、能量消耗等适应度指标。对传统A*算法进行了安全设计，在扩展当前节点的子节点时舍弃与上一个节点生成对角线和障碍物接触的子节点，以此避免路径规划中出现对角线方向上的路径与障碍物发生碰撞从而保证机器人与障碍物节点之间的安全距离。同时引入障碍物占比p(n)来影响启发函数h(n)在不同环境下的权重，改进A*评价函数如下所示：
其中n代表当前节点,f(n)表示移动机器人在当前节点的评价函数,g(n)表示移动机器人从起始点到当前点的实际代价，h(n)为启发函数；w
在栅格地图上采用该A*算法进行路径规划获得的路径如图2所示，且该初始规划路径曲率见图3。
步骤S3、根据路径规划后得到的路径中的控制节点，利用经验参数的纯跟踪算法实现参考轨迹跟踪控制。
将轮式移动机器人的模型简化为阿克曼转向的单车模型，纯跟踪算法示意图如图4所示：
其中g点为预瞄点，l
化简上式，得到：
其中路径曲率
根据轮式移动机器人的运动学模型，有
则
另外，定义横向误差为机器人当前姿态和预瞄点在横向上的误差，并考虑小角度误差：
所以纯跟踪本质上是一个P控制器，且由式(10)可得前视距离与轨迹跟踪效果密切相关，是纯跟踪算法的重要参数，前视距离过大会降低控制精度，但跟踪产生的轨迹会更平滑；前视距离小，追踪拟合更精确，但容易产生振荡。通常定义前视距离为关于速度的一次多项式，如下式所示：
l
计算加速度：
a＝k
其中设v
步骤S4、基于PSO算法对纯跟踪参数的智能化调试从而生成一条可行轨迹。
采用PSO算法对纯跟踪算法的参数整定优化，过程如下：
根据式(7)，(8)，(9)可知需要调试的参数有：k
PSO算法公式如式(10)(11)所示：
v
x
其中，i为粒子序号；x
横向误差示意图如图5所示：
首先，在笛卡尔坐标系中，参考点与真实点间在X、Y轴方向的误差：
d
计算横向误差：
f(k)＝dy×cosθ
设计适应度函数如下式：
其中k表示实际跟踪的序列点，r表示该条轨迹序列点的平均曲率；
PSO算法流程具体如下：
Step1：迭代次数为n＝1,在搜索空间随机初始化粒子群，第i个粒子的位置向量为
Step2:将每个粒子的位置向量元素依次作为参数k
Step3:用每个粒子的当前适应度值
Step4:用每个粒子的当前适应度值
Step5:按照式子(14)和(15)更新粒子的位置
Step6:迭代次数n＝n+1,返回Step2，直至达到最大迭代次数N＝50；
按照以上算法流程进行迭代50次，求得适应度函数最小时的纯跟踪控制最优参数为：
k
迭代过程中适应度函数值得变化见图6，基于PSO算法的纯跟踪控制参数整定后轨迹跟踪优化见图7，其轨迹优化后曲率图见图8所示。同时基于经验给定参数和基于PSO参数整定的的纯跟踪算法轨迹跟踪的跟踪误差对比见图9。
从实验结果(图4-9)可以看出，本发明从轮式移动机器人的实际运动学约束角度出发，在静态环境下准确的规划出一条从起始点到目标点的无碰撞平滑小曲率轨迹，并且初始路径长度为31.15m,经过上述方法轨迹优化后路径长度为30.5m,缩短了2.03％的路径长度，且轨迹曲率总和由207.68降低到86.13，轨迹曲率大小的总和降低了58.5％，使得机器人跟踪轨迹更加的平滑。该方法应用PSO算法对跟踪算法的智能化的参数调试，极大地节省了繁琐的参数调试时间，且基于PSO调试出的参数使得轨迹优化后的平均曲率更小。满足移动机器人运动学约束。结果可以满足实际应用的精度与实时要求。
以上结合附图详细说明和陈述了本发明的实施方式，但并不局限于上述方式。在本领域的技术人员所具备的知识范围内，只要以本发明的构思为基础，还可以做出多种变化和改进。