灭火机器人路径规划matlab_【机器人路径规划】人工势场法

阅读本文需要的基础知识为：

理解机器人的构型空间。建议阅读：机器人运动规划中的C space怎样理解？为什么不直接在笛卡尔坐标系下运算呢？

本文的实现程序与使用说明见我的学习工具箱：小明工坊：【个人开源】机器人运动规划学习工具箱使用说明

基本原理

1.概述

我们打两个比方来说明人工势场法的作用机理。首先，我们把构型空间比作一个电势场平面，机器人（的当前构型）比作空间中一点。如果让机器人的起点和障碍物带正电荷，终点带负电荷，机器人带正电荷。由于同性电荷相斥，异性电荷相吸的原理，机器人将会在电场力的作用下沿着某条路径向终点移动，并避开带正电荷的障碍物，如图1所示。

图 1 电势场

类似的，我们也可以把构型空间比作一个有起伏地形的区域。其中，起点和障碍物位于较高的区域，终点位于较低的区域，机器人视作一个球体。那么在重力的作用下，机器人将沿着某条轨迹从较高的起点滑落到较低的终点，并避开较高的障碍物。如图2[2]所示。

图 2 重力势场 (图片来源于参考文献[2])

以上的两个例子其实就是电势场与重力势场的作用机制，电势场和重力势场都是自然势场。而人工势场法就是在已知起点、终点和障碍物位置的情况下，构建一个人工势场来模仿这种作用机制。人工势场法的优点在于，它其实是一种反馈控制策略，对控制和传感误差有一定的鲁棒性；缺点在于存在局部极小值问题，因此不能保证一定能找到问题的解。

2.引力势场和斥力势场（Additive Attractive/Repulsive Potential）

我们利用势函数

来建立人工势场。

势（场）函数是一种可微函数，空间中某点处势函数值的大小，代表了该点的势场强度。最简单的势函数是 引力/斥力势函数。其作用思路很简单：让目标对机器人产生吸引力，障碍物对机器人产生排斥力。某点

处的势函数

表达为引力势和斥力势之和：

其中，最常见的引力势函数表达式如下：

——引力增益

——当前点

到目标点

之间的距离

最常见的斥力势函数表达式如下：

——点

与其最近障碍物的距离

——斥力增益

——障碍物的作用距离阈值，大于此距离的障碍物不会产生斥力影响

当然，以上引力和斥力势函数的设计在某些情况下会产生问题，因此也有许多改进方法，这个我们以后再说。

3.梯度下降法（Gradient Descent）

如果把某点

处的势函数的取值

看作该点的能量大小，那么

梯度

则可以看作该点的力向量，其定义为：

可以看出，某点处梯度的方向即为势函数增长最快的方向。

而梯度下降法，就是让机器人从初始点开始，不停地沿着梯度的反方向行走，直到梯度为0。用伪代码表示如下：

输入：一种计算q点处梯度的方法U (q)
输出：一组轨迹序列{q(0), q(1), ..., q(i)}
q(0) = q_start
i = 0
while U (q(i)) ≠ 0 do
    q(i + 1) = q(i) + α(i)U (q(i))
    i = i + 1
end while

其中步长 α 的选择比较重要，如果太小，计算速度会变慢；太大，机器人可能会“跨进”障碍物中。

实现方法

下面以运动规划学习工具箱中的人工势场方法 pf_planning() 为例，说明人工势场法的实现步骤：

1.预处理

首先读入包含障碍物的构型空间的bmp图像，为了便于碰撞检测，将其二值化。并读入引力增益、斥力增益、障碍物作用范围、最大搜索次数等参数。

## 预处理
# 关键字参数处理
k_a = 50.0
k_r = 50.0
d_o = 3.0 
limit_try = 1000
# 地图灰度化
image_gray = cv2.cvtColor(self.map, cv2.COLOR_BGR2GRAY)
# 地图二值化
ret,img_binary = cv2.threshold(image_gray, 127, 255, cv2.THRESH_BINARY)

原图像

二值化图像

2.创建势场图

对于每一个像素点，按照前文所述势场函数建立势场图

## 创建势场图
img_potential = np.mat(img_binary) * 0.0
for x in range(img_potential.shape[0]):
    for y in range(img_potential.shape[1]):
        # 目标点的引力场 ug
        ug = k_a * mpt.straight_distance(np.mat([x, y]), self.point_goal)[0, 0]
        # 障碍物的斥力场 uo
        # 最近障碍物的距离
        d_min = np.min(mpt.straight_distance(postion_obs, np.mat([x, y])), 0)
        if d_min < d_o:
            if d_min <= 0.5:
                d_min = 0.1 # 所有障碍物内部点都作为障碍物边缘点处理
            uo = k_r * (1.0 / d_min - 1.0 / d_o)**2 * mpt.straight_distance(np.mat([x, y]), self.point_goal)[0, 0] ** 0.25
        else:
            # 如果障碍物距离大于 d_o ,则不发生斥力作用
            uo = 0.0
        img_potential[x, y] = ug + uo

3.梯度下降法

在已经建立好的势场图中，按照前文所述的方法执行梯度下降法：

    point_current = self.point_strat
    potential_current = img_potential[point_current[0,0],point_current[0,1]]
    motion_direction = np.mat([[1, 0], [0,  1], [-1, 0], [0,  -1],
                               [1, 1], [1, -1], [-1, 1], [-1, -1]])
    point_road = point_current
    num_try = 0
    # 终点检测
    while mpt.straight_distance(point_current, self.point_goal)[0, 0] > 0.0:
        # 最大搜索次数检测
        if num_try < limit_try:
            # 梯度下降法
            potential_temp = potential_current
            point_temp = point_current
            for d in motion_direction:
                local_tag = True
                index_x = point_temp[0, 0] + d[0, 0]
                index_y = point_temp[0, 1] + d[0, 1]
                if ((index_x < 0) or (index_x >= img_potential.shape[0]) or 
                    (index_y < 0) or (index_y >= img_potential.shape[1])):
                    potential_next = float('inf')
                else:
                    potential_next = img_potential[index_x, index_y]
                if potential_current > potential_next:
                    potential_current = potential_next
                    point_current = np.mat([index_x, index_y])
            # 加入路径点集
            point_road = np.vstack((point_road, point_current))
            num_try = num_try + 1
        else:
            path_found = False
            break

4.绘图

在给出的示例图中分别执行人工势场法，得到结果如下：

问题与改进说明

本文仅仅演示了最简单的人工势场法的实现。要想在实际问题中进行应用，人工势场法往往会面临一些问题，需要进行相应的改进，下面对常见问题及其解决方法进行简要说明：

引力势函数的改进：现有引力势函数的取值，与

成正比。在这样的设计中，如果当前点与目标点的距离太远，会形成很大的引力势，使得机器人的运动速度过快。对此，我们可以采用分段的引力势函数解决问题，即在距离较远时降低

的幂。

斥力势函数的改进：现有斥力势函数的取值，取决于当前点与最近障碍物的距离。在这样的设计中，如果当前点与两个障碍物等距，可能会造成机器人在障碍物间的中线上来回跳动。对此，我们可以重新定义斥力势函数为当前点与最近的障碍物点之间的距离。即不考虑障碍物本身的坐标位置，只要障碍物上的某点离当前点最近，就以此距离定义斥力势函数。

距离的计算：一般来说，我们用直线距离来度量两点间的距离。但对于像素图或网格图，如何快速且合理地计算出两个像素点或网格之间的距离呢？参考文献[1]给出了一种 Brushfire算法 以供使用。

局部极小值问题：如图所示，人工势场法有时候会遇到局部极小值问题。在局部极小值点，虽然梯度为零，但并不是我们想要的终点。对这样的情况，我们一般引入抽样规划的思想，在局部最小值处加入一个扰动（随机行走）或回溯，以期跳出局部极小值。也可采用一种名为波前规划（wave-front plann）的方法，通过引入时间参数来消除局部最小值的问题。

人工势场法陷入局部极小值

参考文献

[1] Choset H, Lynch K, Hutchinson S, et al. Principles of robot motion: Theory, algorithms, and implementations. MIT Press, Cambridge, MA[J]. Proceedings of the Society for Experimental Biology & Medicine Society for Experimental Biology & Medicine, 2005, 147(1):512-512.

[2] 人工势场法（Artificial Potential Field Method）的学习