一种基于椭圆模型人工势场法的智能车寻路方法与流程

1.本发明涉及自动寻路技术领域，尤其涉及一种基于椭圆模型人工势场法的智能车寻路方法。


背景技术：

2.智能车是通过车载传感器感知道路环境，自动规划行车路线并控制车辆到达预定目标的智能汽车。集自动控制、体系结构、人工智能、视觉计算等众多技术于一体，是计算机科学、模式识别和智能控制技术高度发展的产物，也是衡量一个国家科研实力和工业水平的一个重要标志，在国防和国民经济领域有着广阔的应用前景。近年来，移动机器人以及各种无人飞行器在各行各业广泛发展应用起来，成为又一个研究热点，路径规划技术是机器人相关研究中最基本、最重要的技术之一。路径规划是指机器人根据一些特定的指标(如花费时间最少、行走路径最短等)，在其工作空间中搜索一条从起点到终点的避障路径。路径规划研究始于20世纪70年代，此后各机器人研发大国对其的研究从没有间断，至今已经是成果斐然。目前，常用的路径规划的方法分为传统算法和智能仿生算法。其中，传统的人工势场法因为计算简单方便而被广泛使用。
3.传统人工势场法是假设目标点对智能车产生引力，障碍物对智能车产生斥力，从而使智能车沿着势力下降的方向行走，就像是水往低处流。该算法的优点就是结构简单，有利于底层控制的实时性，可大大减少计算量和计算时间，并且生成相对光滑的路径，利于保持自动驾驶车辆的稳定性。该算法存在的问题也很明显，一是目标不可达的问题，若智能车与目标点的距离很远，引力将变得极大，斥力与之相比则较小，甚至可以忽略，此时智能车在前进的路径是上很有可能与障碍物相碰撞导致智能车的寻路失败，在目标点附近有大型障碍物时，智能车在靠近目标时会受到障碍物极大的斥力作用，导致无法到达终点。二是局部最小值问题，零势能域问题是人工势场法的瓶颈问题，产生零势能点的原因是智能车在工作区域存在多个障碍物点，且位置分布比较特殊，可能会在工作区域的某个点出现引力和斥力刚好大小相等，方向相反，智能车的受力为零，智能车就会在该点附近来回摆动无法到达目的地，严重影响智能车的运行效率。lee,j等人通过使用任意力算法来建立新的人工势场法函数来控制移动机器人的避障规划，同时解决了机器人、障碍物、目标点对称排序的问题。li.chunshu等人提出一种带记忆功能的“延边走”方法，该方法通过沿着障碍物的边缘行走跳出人工势场法的局部最小点，并记录和分析机器人走过的局部最小点来判断目标点是否被障碍物包围，以免机器人一直来回振荡或者位置目标点转圈。这些方法虽然在一定程度上解决了人工势场法存在的缺点，但是智能车在所处环境较为复杂的情况下往往无法取得很好的避障效果，不能得到最优路径，或者路径曲线曲折。
4.本发明在传统人工势场的基础上，通过改进斥力函数和建立椭圆模型设置虚拟目标点来解决势场法在规划路径过程中可能遇到的无法到达目标点和陷入局部最小值的问题。


技术实现要素：

5.本发明的目的是为了解决现有技术中的问题，而提出的一种基于椭圆模型人工势场法的智能车寻路方法。
6.为了实现上述目的，本发明采用了如下技术方案：一种基于椭圆模型人工势场法的智能车寻路方法，包括以下步骤：
7.步骤1、根据智能车所在的区域构建相应的地图，将地图网格化并建立坐标系，确定地图中存在障碍物位置的坐标和目标点位置；
8.步骤2、判断智能车当前是否到达终点位置；
9.步骤3、若到达终点位置，则完成路径规划，生成最终路径；若未到达终点位置，则用人工势场法进行路径规划；
10.步骤4、计算智能车当前位置所受到的引力、斥力及合力大小及方向；
11.步骤5、判断是否陷入局部极小值问题；
12.步骤6、若未陷入局部最小值，则返回步骤2继续进行判断和路径规划，若陷入局部最小值问题，则根据局部最小值问题点构建椭圆模型，增设虚拟目标点，增大终点位置方向对智能车的吸引力，从而跳出局部最小值问题；
13.步骤7、再次使用椭圆模型人工势场法进行计算，直到行驶到终点位置；
14.步骤8、对规划后的路径在进行二次优化，从起点开始依次向后检测路径节点，判断是否能够直线到达，若能直线到达，则删除起点到该节点的路径；若不能直线到达，则将该节点位置当做起点位置继续做能否直线到达的判断，从而简化路径。
15.在上述的基于椭圆模型人工势场法的智能车寻路方法中，采用人工势场法，避免智能车在运动过程中出现目标不可达现象，在含有障碍物的工作环境中为所述智能车规划出一条从起始点到目标点的安全路径，并判断智能车是否陷入局部最小值问题，包括：
16.以地图建立x-o-y坐标系，假设机器人陷入局部平衡的坐标为p
min
(x
min
,y
min
)，终点位置坐标为p
goal
(x
goal
,y
goal
)，则这两点的中点坐标位置pb为：
[0017][0018]
以p
min
和p
goal
两点的连线作为新轴x’，以pb为原点，建立过原点垂直于x’的轴y’，新的坐标系是由原坐标系经过旋转平移得到，如下式：
[0019][0020]
其中是旋转矩阵，是平移矩阵；
[0021]
在新坐标系下建立椭圆方程如下：
[0022][0023]
焦距2c＝|p
min-p
goal
|,a＝ψc，ψ是长轴因子，a2＝b2 c2。
[0024]
以椭圆与x’轴的正交点为初始的虚拟目标点，增大终点位置对智能车的吸引力，
从而打破局部最小状态，智能车按此虚构目标点行走，若检测到再次陷入局部平衡时，则构建新的椭圆模型，寻找新的虚构目标点。
[0025]
在上述的基于椭圆模型人工势场法的智能车寻路方法中，人工势场法的引力函数如下：
[0026][0027]
式中xg表示目标点的位置(xg,yg)；λ1表示引力常数；ρ(x,xg)是智能车当前位置和终点位置的几何距离；
[0028]
对引力势力函数中的距离求一阶导所得到的即为引力：
[0029][0030]
同理，斥力势函数如下：
[0031][0032]
式中x0＝(x0,y0)表示障碍物位置；λ2表示斥力场常量；ρ0表示障碍物影响半径；ρ(x,x0)是智能车与障碍物之间的间距；
[0033]
对斥力函数对距离求一阶导数即斥力：
[0034][0035]
所以智能车在空间中所受到的合力f
total
(x)为：
[0036]ftotal
(x)＝f
att
(x) f
rep
(x)
[0037]
目标点位置无法抵达的问题是由于智能车在目标点周围的障碍物产生的斥力大于目标点的引力的原因；
[0038]
因此，在传统的斥力函数基础上，引入当前点到目标点之间的距离因子ρ(x,xg)，根据不同的情形降低斥力值的大小；改进的斥力函数如下所示：
[0039][0040]
当智能车到终点位置之间的间距ρ(x,xg)＜1时，且障碍物离机器人较近时，和ρ(x,xg)2均是小于1的数值，因此可以降低斥力值，防止在目标点周围因斥力过大引起目标点无法抵达现象。
[0041]
在上述的基于椭圆模型人工势场法的智能车寻路方法中，在步骤3中，人工势场法算法改进其斥力函数，根据智能车当前位置到目标点之间的距离因子ρ(x,xg)，设置三种不同的情况，智能车的位置出现在障碍物影响半径值的一半以内时，智能车已经十分靠近障碍物，考虑到安全避障的因素，不能够将斥力削减的过多，此处取距离因子的开平方系数，另一方面，机器人的位置在障碍物影响半径值的一半位置到最大值之间时，智能车刚开始接近障碍物，为了防止引力小于斥力从而无法接近目标点，当距离因子大于障碍物影响半径时，智能车不受影响。
[0042]
在上述的基于椭圆模型人工势场法的智能车寻路方法中，在步骤6中，以局部极小值点位置和目标点位置构建椭圆模型，以指向目标点的方向为正方向建立x’轴，在其两点的中点位置作垂直于x’轴的y’轴，从而建立新的坐标系x
’‑
o-y’,设置虚拟目标点为x’轴正方向与椭圆模型的交点，从而增加目标点对智能车在引力方向上的力，从而打破局部极小值状态，跳出局部平衡。
[0043]
与现有的技术相比，本发明的优点在于：
[0044]
1、在庞杂环境中，采用椭圆模型增设虚拟目标点，虚构目标点产生的引力将改变机器人在局部最小值的状态，通过在椭圆模型上不断修正虚构目标点的位置，从而使智能车跳出局部平衡区域，同时大大降低了算法的时间成本；
[0045]
2、根据智能车当前位置与终点位置之间的距离，并根据设置距离影响因子设置三种不同的情况，更好的保证了智能车规划路径的安全性。
附图说明
[0046]
图1为本发明提出的一种基于椭圆模型人工势场法的智能车寻路方法中人工势场法的基本原理图；
[0047]
图2为本发明提出的一种基于椭圆模型人工势场法的智能车寻路方法中椭圆模型建立虚拟目标点的方法图；
[0048]
图3为本发明提出的一种基于椭圆模型人工势场法的智能车寻路方法中椭圆模型人工势场法的具体流程图；
[0049]
图4为本发明提出的一种基于椭圆模型人工势场法的智能车寻路方法中椭圆模型人工势场法规划路径的效果图。
具体实施方式
[0050]
以下实施例仅处于说明性目的，而不是想要限制本发明的范围。
[0051]
实施例
[0052]
参照图1-4，一种基于椭圆模型人工势场法的智能车寻路方法，包括以下步骤：
[0053]
步骤1、根据智能车所在的区域构建相应的地图，将地图网格化并建立坐标系，确定地图中存在障碍物位置的坐标和目标点位置；
[0054]
步骤2、判断智能车当前是否到达终点位置；
[0055]
步骤3、若到达终点位置，则完成路径规划，生成最终路径；若未到达终点位置，则用人工势场法进行路径规划；
[0056]
步骤4、计算智能车当前位置所受到的引力、斥力及合力大小及方向；
[0057]
步骤5、判断是否陷入局部极小值问题；
[0058]
步骤6、若未陷入局部最小值，则返回步骤2继续进行判断和路径规划，若陷入局部最小值问题，则根据局部最小值问题点构建椭圆模型，增设虚拟目标点，增大终点位置方向对智能车的吸引力，从而跳出局部最小值问题；
[0059]
步骤7、再次使用椭圆模型人工势场法进行计算，直到行驶到终点位置；
[0060]
步骤8、对规划后的路径在进行二次优化，从起点开始依次向后检测路径节点，判断是否能够直线到达，若能直线到达，则删除起点到该节点的路径；若不能直线到达，则将该节点位置当做起点位置继续做能否直线到达的判断，从而简化路径。
[0061]
采用人工势场法，避免智能车在运动过程中出现目标不可达现象，在含有障碍物的工作环境中为所述智能车规划出一条从起始点到目标点的安全路径，并判断智能车是否陷入局部极小点，包括：
[0062]
以地图建立x-o-y坐标系，假设机器人陷入局部平衡的坐标为p
min
(x
min
,y
min
)，终点位置坐标为p
goal
(x
goal
,y
goal
)，则这两点的中点坐标位置pb为：
[0063][0064]
以p
min
和p
goal
两点的连线作为新轴x’，以pb为原点，建立过原点垂直于x’的轴y’，新的坐标系是由原坐标系经过旋转平移得到，如下式：
[0065][0066]
其中是旋转矩阵，是平移矩阵；
[0067]
在新坐标系下建立椭圆方程如下：
[0068][0069]
焦距2c＝|p
min-p
goal
|,a＝ψc，ψ是长轴因子，a2＝b2 c2；
[0070]
以椭圆与x’轴的正交点为初始的虚拟目标点，增大终点位置对智能车的吸引力，从而打破局部最小状态，智能车按此虚构目标点行走，若检测到再次陷入局部平衡时，则构建新的椭圆模型，寻找新的虚构目标点。
[0071]
人工势场法的引力函数如下：
[0072][0073]
式中xg表示目标点的位置(xg,yg)；λ1表示引力常数；ρ(x,xg)是智能车当前位置和终点位置的几何距离；
[0074]
对引力势力函数中的距离求一阶导所得到的即为引力：
[0075][0076]
同理，斥力势函数如下：
[0077][0078]
式中x0＝(x0,y0)表示障碍物位置；λ2表示斥力场常量；ρ0表示障碍物影响半径；ρ(x,x0)是智能车与障碍物之间的间距；
[0079]
对斥力函数对距离求一阶导数即斥力：
[0080][0081]
所以智能车在空间中所受到的合力f
total
(x)为：
[0082]ftotal
(x)＝f
att
(x) f
rep
(x)
[0083]
目标点位置无法抵达的问题是由于智能车在目标点周围的障碍物产生的斥力大于目标点的引力的原因；
[0084]
因此，在传统的斥力函数基础上，引入当前点到目标点之间的距离因子ρ(x,xg)，根据不同的情形降低斥力值的大小；改进的斥力函数如下所示：
[0085][0086]
当智能车到终点位置之间的间距ρ(x,xg)＜1时，且障碍物离机器人较近时，和ρ(x,xg)2均是小于1的数值，因此可以降低斥力值，防止在目标点周围因斥力过大引起目标点无法抵达现象。
[0087]
一方面，机器人的位置出现在障碍物影响半径值的一半以内时，智能车已经十分靠近障碍物，考虑到安全避障的因素，不能够将斥力削减的过多，此处取距离因子的开平方系数。另一方面，机器人的位置在障碍物影响半径值的一半位置到最大值之间时，智能车刚开始接近障碍物，为了防止引力小于斥力从而无法接近目标点，应该尽可能的减小斥力的大小，故此处取距离因子的平方系数。
[0088]
在较为复杂的环境中，有可能出现智能车在未抵达目标点位置时，智能车所受到的引力和斥力的大小一样，方向相反，合力为零的情况，这将导致智能车在目前位置来回的振荡。
[0089]
本发明提出了建立椭圆模型来设置虚拟目标点的方法，虚构目标点产生的引力将改变机器人在局部最小值的状态，通过在椭圆模型上不断修正虚构目标点的位置，从而使智能车跳出局部平衡区域。
[0090]
在步骤3中，人工势场法算法改进其斥力函数，根据智能车当前位置到目标点之间
的距离因子ρ(x,xg)，设置三种不同的情况，智能车的位置出现在障碍物影响半径值的一半以内时，智能车已经十分靠近障碍物，考虑到安全避障的因素，不能够将斥力削减的过多，此处取距离因子的开平方系数，另一方面，机器人的位置在障碍物影响半径值的一半位置到最大值之间时，智能车刚开始接近障碍物，为了防止引力小于斥力从而无法接近目标点，当距离因子大于障碍物影响半径时，智能车不受影响。
[0091]
在步骤6中，以局部极小值点位置和目标点位置构建椭圆模型，以指向目标点的方向为正方向建立x’轴，在其两点的中点位置作垂直于x’轴的y’轴，从而建立新的坐标系x
’‑
o-y’,设置虚拟目标点为x’轴正方向与椭圆模型的交点，从而增加目标点对智能车在引力方向上的力，从而打破局部极小值状态，跳出局部平衡。