CN106843235A - 一种面向无人自行车的人工势场路径规划法 - Google Patents

Abstract

本发明提供了一种面向无人自行车的人工势场局部路径规划法，包括：(1)初始化环境信息，确定无人自行车的初始位置、目标点位置和障碍物信息；(2)获取周围障碍物的信息，计算障碍物对无人自行车的影响距离及障碍物与目标位置的距离；(3)计算无人自行车在当前位置所受的引力与斥力，改进斥力势场函数和斥力函数后，计算合力的大小与方向；(4)引导无人自行车行进到下一个地点；(5)判断是否陷入局部最小点，如是则转入步骤(6)，否则转入步骤(7)；(6)加入虚拟障碍物，斥力场为J_a，转入步骤(2)；(7)若有虚拟障碍物，将其删除，判断无人自行车是否到达目标位置或行走完规定距离，若是则结束，否则转到步骤(2)。

Description

一种面向无人自行车的人工势场路径规划法

技术领域

本发明涉及无人自行车技术，特别是一种面向无人自行车人工势场路径规划法。

背景技术

自20世纪60年代移动无人自行车诞生以来，研究人员一直梦想研究无人智能交通工具，作为智能交通系统的重要组成部分，无人自行车排除了人为不确定因素的影响，不仅可以提高驾驶安全性，而且可以解决交通拥堵，提高能源利用率，百度曾宣布开发复杂人工智能无人自行车，该产品是具备环境感知、规划和自平衡控制等复杂人工智能的无人自行车，主要集合了百度在人工智能、深度学习、大数据和云计算技术的成就，然而对技术细节没有任何披露。目前大多采用采用覆盖面广、成本低，且针对性强的运动干预服务系统，对无人自行车的运动进行符合实际情况的干预，有望解决自行车避障等问题。

作为无人自行车的智能核心，避障路径规划系统决定车辆如何在多种约束条件和路径障碍物条件下到达目标位置，这些约束包括体现为安全性的环境约束，体现可行性的系统运动学约束，体现平顺性和稳定性的系统动力学约束以及特定的优化指标约束，如最短时间或最短距离等。在无人自行车应用中，这些约束集中在全局路径规划中得到满足，全局路径规划问题等同于起点和终点间路径生成的问题，解决全局路径规划问题一般要求提前获知完成的典型道路及其数字化存储方式，也就是环境地图，当环境变化或其他因素导致规划结果不可行时，需要重启全局规划得到新的可行路径才能继续行使。

人工势场法结构简单，便于底层的实时控制，在实时避障和平滑的轨迹控制方面，得到广泛应用，但是存在局部最优解，容易产生死锁现象，因而可能使无人自行车在到达目标点之前就停留在局部最优点。针对人工势场法存在的局部极小点、目标不可达和振动问题，国内外学者作了大量研究，主要针对改进势场函数和将传统人工势场法和其他方法结合这几个方向，如沿墙走，模拟退火，粒子群算法等。

然而，这些方法虽然可以解决局部极小问题，然而沿墙走算法会增加无人自行车不必要的路程，大大增加无人自行车的运动时间，而模拟退火计算过程复杂，使得规划出来的路径不平滑，实时性差。

发明内容

本发明的目的在于提供一种面向无人自行车的人工势场局部路径规划法，包括如下步骤：

(1)初始化环境信息，确定无人自行车的初始位置、目标点位置和障碍物信息；

(2)无人自行车获取周围障碍物的信息，计算障碍物对无人自行车的影响距离及障碍物与目标位置的距离；

(3)计算无人自行车在当前位置所受的引力与斥力，改进斥力势场函数和斥力函数后，计算合力的大小与方向；

(4)根据步骤(3)求出的数据，按照一定的路径因子引导无人自行车行进到下一个地点；

(5)判断是否陷入局部最小点，如果陷入局部最小点则转入步骤(6)，否则，转入步骤(7)；

(6)在(x_a,y_a)点处加入虚拟障碍物，虚拟障碍物位置记为(x_oa,y_oa)，斥力场为J_a，转入步骤(2)；

(7)若有虚拟障碍物，将其删除，判断无人自行车是否到达目标位置或行走完规定距离，若到达目标位置或行走完规定距离，则结束本次算法的一个循环，否则，跳转到步骤(2)。

优选的，所述步骤(3)具体实施方法为：将无人自行车简化为一个质点，其在工作空间中的位置为X，引力势场函数定义为：

式中，k为引力势场常量，X＝(xy)^T为无人自行车的当前坐标，(X-X_g)为目标与无人自行车之间的相对位置；

引力函数与引力势场函数的负梯度：

F_att(X)＝k(X-X_g) (2)

改进后的斥力函数为：

式中，矢量F_rep1的方向从障碍物指向无人自行车，矢量F_rep2的方向从无人自行车指向目标点，

优选的，所述步骤(4)按照如下步骤进行：(4-1)对环境进行建模；(4-2)建立势场；(4-3)求解路径因子并选择最优路径。

优选的，所述步骤(4-1)对环境进行建模包括：以起始点为坐标原点，起始点和目标点的连线为X轴，建立直角坐标系，在起始点和目标点之间，选取相同间隔的一组点作为路径因子的横坐标，路径因子的连线就是无人自行车移动的路径，路径因子横坐标固定，只在Y方向运动，虚拟目标点纵坐标固定，只在X方向运动。

优选的，所述步骤(4-2)势场建立步骤为首先建立引力势场：(6)，式中，k为引力势场系数，L为路径因子相对于虚拟目标点的位置，其中引力定义为：F_att(X)＝kL(7)，路径因子受到的斥力来自障碍物，斥力势场函数采用电荷势场模型：式中，η为斥力势场系数，S为路径因子相对于障碍物的位置，斥力定义为

优选的，所述步骤(4-3)中求解路径因子并选择最优路径包括：建立路径因子在X方向和Y方向的受力分别为：

对于存在多个障碍物的情况，路径因子的受力表示为：

式中，L_gi为第i个路径因子相对于目标点的位置，L_di为第i个路径因子相对于虚拟目标点的位置，S_ki为第i个路径因子相对于第k个障碍物的位置。

由公式(11)求得路径因子的Y坐标和虚拟目标点的X坐标，求得多组解，从而无人自行车也就意味着无人自行车有多条路径可以到达目标区域。

优选的，，所述多条路径中选择一条路径的选取的原则有两个，一是无人自行车不能进入机动车道等禁止进入的区域，二是路径因子纵坐标的绝对值之和最小。

优选的，所述步骤(6)按照如下流程进行：当无人自行车陷入局部最小点时，以此极小点为中心，加入虚拟障碍物斥力场：

其中，P_oa为虚拟障碍物对无人自行车的影响距离；P_a为无人自行车到虚拟障碍物的距离，则虚拟斥力为：此时无人自行车所受到的合力为：此时无人自行车根据新合力的大小和方向运动以逃离局部极小点。

所述步骤(6)中还添加使用关联目标点使得无人自行车在目标点附近不受障碍物斥力场的影响，设障碍物1对无人自行车的影响距离为P_o1，障碍物2对无人自行车的影响距离为P_o2，…，障碍物n对无人自行车的影响距离为P_on，设目标点(x_goat,y_goat)与障碍物i(x_oi,y_oi)之间的距离为：若目标点位置不受障碍物i的斥力场影响，那么P_oi<P_i(i＝1,2,3...)，设障碍物i的影响范围距离P_oi与它跟目标点之间的距离成正比，关系定义为：其中，ω表示距离转换常量因子，k表示障碍物影响距离的权值，k越大，障碍物的影响距离越小。

采用本发明的避障局部路径规划方法，可使得自行车严格按照规划路径行驶，并且车速自动根据路径曲率调整，遇到移动或者固定障碍物的情况下，可以提前进行避障路径规划，而不会陷入极小点带来的意外停滞窘境。

根据下文结合附图对本发明具体实施例的详细描述，本领域技术人员将会更加明了本发明的上述以及其他目的、优点和特征。

附图说明

后文将参照附图以示例性而非限制性的方式详细描述本发明的一些具体实施例。附图中相同的附图标记标示了相同或类似的部件或部分。本领域技术人员应该理解，这些附图未必是按比例绘制的。本发明的目标及特征考虑到如下结合附图的描述将更加明显，附图中：

图1为根据本发明实施例的无人自行车受力分析示意图；

图2为根据本发明实施例的路径规划流程图；

图3为根据本发明实施例的仿真结果示意图。

具体实施方式

结合附图如下详细说明本发明的一种面向无人自行车的人工势场局部路径规划法，包括如下步骤：(1)初始化环境信息，确定无人自行车的初始位置、目标点位置和障碍物信息；(2)无人自行车获取周围障碍物的信息，计算障碍物对无人自行车的影响距离及障碍物与目标位置的距离；(3)计算无人自行车在当前位置所受的引力与斥力，改进斥力势场函数和斥力函数后，计算合力的大小与方向；(4)根据步骤(3)求出的数据，按照一定的路径因子引导无人自行车行进到下一个地点；(5)判断是否陷入局部最小点，如果陷入局部最小点则转入步骤(6)，否则，转入步骤(7)；(6)在(x_a,y_a)点处加入虚拟障碍物，虚拟障碍物位置记为(x_oa,y_oa)，斥力场为J_a，转入步骤(2)；(7)若有虚拟障碍物，将其删除，判断无人自行车是否到达目标位置或行走完规定距离，若到达目标位置或行走完规定距离，则结束本次算法的一个循环，否则，跳转到步骤(2)。所述步骤(3)具体实施方法为：将无人自行车简化为一个质点，其在工作空间中的位置为X，引力势场函数定义为：

式中，k为引力势场常量，X＝(xy)^T为无人自行车的当前坐标，(X-X_g)为目标与无人自行车之间的相对位置；

引力函数与引力势场函数的负梯度：

F_att(X)＝k(X-X_g) (2)

改进后的斥力函数为：

式中，矢量F_rep1的方向从障碍物指向无人自行车，矢量F_rep2的方向从无人自行车指向目标点，

步骤(4)按照如下步骤进行：(4-1)对环境进行建模；(4-2)建立势场；(4-3)求解路径因子并选择最优路径。

其中，步骤(4-1)对环境进行建模包括：以起始点为坐标原点，起始点和目标点的连线为X轴，建立直角坐标系，在起始点和目标点之间，选取相同间隔的一组点作为路径因子的横坐标，路径因子的连线就是无人自行车移动的路径，路径因子越多，连线越光滑，路径因子横坐标固定，只在Y方向运动，虚拟目标点的存在是为了构造势能极小点，也就是路径因子所在的位置，虚拟目标点纵坐标固定，只在X方向运动。

而步骤(4-2)势场建立步骤为首先建立引力势场：

式中，k为引力势场系数，L为路径因子相对于虚拟目标点的位置，

其中引力定义为：

F_att(X)＝kL (7)

路径因子受到的斥力来自障碍物；

斥力势场函数采用电荷势场模型：

式中，η为斥力势场系数，S为路径因子相对于障碍物的位置，

斥力定义为

另外，所述步骤(4-3)中求解路径因子并选择最优路径包括：建立路径因子在X方向和Y方向的受力分别为：

对于存在多个障碍物的情况，路径因子的受力表示为：

式中，L_gi为第i个路径因子相对于目标点的位置，L_di为第i个路径因子相对于虚拟目标点的位置，S_ki为第i个路径因子相对于第k个障碍物的位置；如图1所示。

由公式(11)求得路径因子的Y坐标和虚拟目标点的X坐标，路径因子Y坐标的求解可以通过在前一个目标因子Y坐标值的领域内寻求数值解，以提高计算效率，由公式(11)可以求得多组解，也就意味着无人自行车有多条路径可以到达目标区域，路径选取的原则有两个，一是无人自行车不能进入机动车道等禁止进入的区域，二是路径因子纵坐标的绝对值之和最小，其中该势场法路径规划流程如图2所示。

在虚拟人工势场中的无人自行车，其运动取决于在势场中所受的目标点产生的引力与障碍物产生的斥力的合力，若无人自行车运动到某个点的时候，多个障碍物形成的斥力的合力与目标点产生的引力大小相等，方向相反，则无人自行车所受的合力为零，无人自行车会停止运动，步骤(6)按照如下流程进行：当无人自行车陷入局部最小点时，以此极小点为中心，加入虚拟障碍物斥力场：

其中，P_oa为虚拟障碍物对无人自行车的影响距离；P_a为无人自行车到虚拟障碍物的距离，

则虚拟斥力为：

此时无人自行车所受到的合力为：

此时无人自行车根据新合力的大小和方向运动以逃离局部极小点。

另外，无人自行车在环境中能够自由行走而不碰到障碍物，障碍物对无人自行车的影响范围距离P_o起着重要作用，若目标点距离障碍物较近，即在某障碍物的影响范围内，无人自行车靠近目标点时，斥力没有消失反而增加，在引力没有变为零的时候，斥力会在一点与引力形成大小相等，方向相反的合力，无人自行车不能趋向目标点，因此步骤(6)中采用关联目标点使得无人自行车在目标点附近不受障碍物斥力场的影响。

设障碍物1对无人自行车的影响距离为P_o1，障碍物2对无人自行车的影响距离为P_o2，…，障碍物n对无人自行车的影响距离为P_on，

设目标点(x_goat,y_goat)与障碍物i(x_oi,y_oi)之间的距离为：

若目标点位置不受障碍物i的斥力场影响，那么

P_oi<P_i(i＝1,2,3...)，

设障碍物i的影响范围距离P_oi与它跟目标点之间的距离成正比，关系定义为：

其中，ω表示距离转换常量因子，k表示障碍物影响距离的权值，k越大，障碍物的影响距离越小，这样无人自行车在靠近目标点的过程中，可以不受离目标点距离很近的障碍物的影响，从而达到目标点。

将无人自行车视为质点，在MATLAB仿真环境下进行仿真研究，仿真参数位：仿真环境尺寸为200cm*200cm，无人自行车起始位置为：x轴为5cm，y轴为0cm，障碍物设定由三个圆形障碍物及一个四边形障碍物组成，均匀分布，算法参数取值为：ω＝1.0,λ₁＝10,λ₂＝0.001,P₀＝10,k＝500，仿真结果如图3所示，增加虚拟障碍物后，无人自行车可以成功逃离局部最小点，采用关联目标点方法后，通过不断调整k值可以实现对障碍物影响范围的修改，无人自行车可以顺利到达离障碍物很近的目标点，将两种方法结合并结合修正的斥力势场函数和斥力函数，获得了满意的避障局部路线规划结果，如图3所示。

虽然本发明已经参考特定的说明性实施例进行了描述，但是不会受到这些实施例的限定而仅仅受到附加权利要求的限定。本领域技术人员应当理解可以在不偏离本发明的保护范围和精神的情况下对本发明的实施例能够进行改动和修改。

Claims (8)

1.一种面向无人自行车的人工势场局部路径规划法，其特征在于包括如下步骤：

(1)初始化环境信息，确定无人自行车的初始位置、目标点位置和障碍物信息；

(2)无人自行车获取周围障碍物的信息，计算障碍物对无人自行车的影响距离及障碍物与目标位置的距离；

(3)计算无人自行车在当前位置所受的引力与斥力，改进斥力势场函数和斥力函数后，计算合力的大小与方向；

(4)根据步骤(3)求出的数据，按照一定的路径因子引导无人自行车行进到下一个地点；

(5)判断是否陷入局部最小点，如果陷入局部最小点则转入步骤(6)，否则，转入步骤(7)；

(6)在(x_a,y_a)点处加入虚拟障碍物，虚拟障碍物位置记为(x_oa,y_oa)，斥力场为J_a，转入步骤(2)；

(7)若有虚拟障碍物，将其删除，判断无人自行车是否到达目标位置或行走完规定距离，若到达目标位置或行走完规定距离，则结束本次算法的一个循环，否则，跳转到步骤(2)。

2.根据权利要求1的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述步骤(3)具体实施方法为将无人自行车简化为一个质点，其在工作空间中的位置为X，引力势场函数定义为：

$U_{att}\left(X\right)=\frac{1}{2}k{(X-X_g)}^2---\left(1\right)$

式中，k为引力势场常量，X＝(xy)^T为无人自行车的当前坐标，(X-X_g)为目标与无人自行车之间的相对位置；

引力函数与引力势场函数的负梯度：

F_att(X)＝k(X-X_g) (2)

改进后的斥力函数为：

$F_{rep}\left(X\right)=\left\{\begin{array}{c}{F}_{rep1}+F_{rep2},\mathrm{\&rho;}\&le;{\mathrm{\&rho;}}_0\\ 0,\mathrm{\&rho;}>{\mathrm{\&rho;}}_0\end{array}\right.---\left(3\right)$

式中，矢量F_rep1的方向从障碍物指向无人自行车，矢量F_rep2的方向从无人自行车指向目标点，

$F_{rep1}=\mathrm{\&eta;}{(\frac{1}{\mathrm{\&rho;}}-\frac{1}{{\mathrm{\&rho;}}_0})}^2\frac{1}{{\mathrm{\&rho;}}^4}{(X-X_g)}^n---\left(4\right)$

$F_{rep2}=\frac{n}{2}\mathrm{\&eta;}{(\frac{1}{\mathrm{\&rho;}}-\frac{1}{{\mathrm{\&rho;}}_0})}^2{(X-X_g)}^{n-1}---\left(5\right)$

3.根据权利要求1的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述步骤(4)按照如下步骤进行：(4-1)对环境进行建模；(4-2)建立势场；(4-3)求解路径因子并选择最优路径。

4.根据权利要求3的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述步骤(4-1)对环境进行建模包括：以起始点为坐标原点，起始点和目标点的连线为X轴，建立直角坐标系，在起始点和目标点之间，选取相同间隔的一组点作为路径因子的横坐标，路径因子的连线就是无人自行车移动的路径，路径因子横坐标固定，只在Y方向运动，虚拟目标点纵坐标固定，只在X方向运动。

5.根据权利要求3的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：优选的，所述步骤(4-2)势场建立步骤为首先建立引力势场：式中，k为引力势场系数，L为路径因子相对于虚拟目标点的位置，其中引力定义为：F_att(X)＝kL(7)，路径因子受到的斥力来自障碍物，斥力势场函数采用电荷势场模型：式中，η为斥力势场系数，S为路径因子相对于障碍物的位置，斥力定义为

6.根据权利要求3的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述步骤(4-3)中求解路径因子并选择最优路径包括：建立路径因子在X方向和Y方向的受力分别为：

$\left\{\begin{array}{c}{F}_{att}{\mathrm{sin\&theta;}}_g+F_d{\mathrm{sin\&theta;}}_d=F_{rep}{\mathrm{sin\&theta;}}_b\\ F_{att}{\mathrm{cos\&theta;}}_g=F_d{\mathrm{cos\&theta;}}_d+F_{rep}{\mathrm{cos\&theta;}}_b\end{array}\right.---\left(10\right)$

对于存在多个障碍物的情况，路径因子的受力表示为：

$\left\{\begin{array}{c}{\mathrm{kL}}_{gi}{\mathrm{sin\&theta;}}_{gi}+{\mathrm{kL}}_{di}{\mathrm{sin\&theta;}}_{di}=\mathrm{\&Sigma;}\mathrm{\&eta;}\frac{{\mathrm{sin\&theta;}}_{kd}}{{S_{ki}}^2}\\ {\mathrm{kL}}_{gi}{\mathrm{cos\&theta;}}_{gi}={\mathrm{kL}}_{di}{\mathrm{cos\&theta;}}_{di}+\mathrm{\&Sigma;}\mathrm{\&eta;}\frac{{\mathrm{cos\&theta;}}_{kb}}{{S_{ki}}^2}\end{array}\right.---\left(11\right)$

式中，L_gi为第i个路径因子相对于目标点的位置，L_di为第i个路径因子相对于虚拟目标点的位置，S_ki为第i个路径因子相对于第k个障碍物的位置。

由公式(11)求得路径因子的Y坐标和虚拟目标点的X坐标，求得多组解，从而无人自行车也就意味着无人自行车有多条路径可以到达目标区域。

7.根据权利要求6的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述多条路径中选择一条路径的选取的原则有两个，一是无人自行车不能进入机动车道等禁止进入的区域，二是路径因子纵坐标的绝对值之和最小。

8.根据权利要求1的一种面向无人自行车的人工势场局部路径规划法优选的，其特征在于：所述步骤(6)按照如下流程进行：当无人自行车陷入局部最小点时，以此极小点为中心，加入虚拟障碍物斥力场：

其中，Po_a为虚拟障碍物对无人自行车的影响距离；P_a为无人自行车到虚拟障碍物的距离，则虚拟斥力为：此时无人自行车所受到的合力为：此时无人自行车根据新合力的大小和方向运动以逃离局部极小点。