代码设计¶

  1. 构建地图: 获得障碍物信息，生成目标点(随机数)
  2. 聚类算法任务分配 使用 A* 算法(或者任意一个从A-->B规划路径的算法) 构建目标点的全连接图, 并用谱聚类实现任务分配
  3. 寻路算法获取 A-->B 路径 SOM 算法
  4. 样条法平滑路径
  5. (无人艇运动控制系统)

构建地图¶

scikit-image: Image processing in Python — scikit-image
Scikit image 中文开发手册 - 开发者手册 - 云 + 社区 - 腾讯云

In [6]:
import numpy as np
import matplotlib.pyplot as plt
from skimage import io, color, transform, morphology

%matplotlib inline
%config InlineBackend.figure_format = 'svg'

处理地图¶

In [10]:
# 导入图片 original map
omg = io.imread("../Assets/Resources/Environment/890c.png")
omg = np.flip(omg, 0) # 图片翻转
omg = transform.resize(omg, (500, 500)) # 大小
plt.axis("off"), plt.gray()
plt.xlim(0, omg.shape[1])
plt.ylim(0, omg.shape[0])
plt.imshow(omg)
Out[10]:
<matplotlib.image.AxesImage at 0x16b6d04c0a0>
2022-05-10T16:07:32.527417 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

关于坐标：

  • 图片矩阵是先 y 后 x, 对于坐标(x,y), 对应 img[y, x].
    • 访问图片坐标时倒过来就好, 其他正常.
    • 如果这一点不习惯的话，对于二维矩阵可以转置后再查找 img[y, x] == img.T[x, y] 高维矩阵可以交换维度。
  • 图片矩阵原点是图片的左上角，而一般习惯平面坐标系左下角是原点.
    • 所以显示图片的时候, imshow 的结果默认是左上角原点, 如果要按照习惯设左下角为原点，需要设置坐标轴(不过图片显示会颠倒).
    • 注意这里横坐标范围是 img.shape[1], 原因同上.
    • 有一种解决方式就是把 img 的 y 坐标(第一维度)上下翻转. 即 img=np.flip(img,0). 则等效为图片坐标原点转化为左下角.
In [11]:
# 转换为灰度图并二值化 (这里选的是另一个更方便的图)
img = io.imread("../Assets/Resources/Environment/map_original.png")

img = color.rgb2gray(img)
img = transform.resize(img, (500, 500))
img = img < 0.44

img = np.flip(img, 0)  # 图片翻转, 图片坐标原点转换到左下角

img = morphology.binary_closing(img)  # 消除小黑点
img = morphology.binary_opening(img, np.ones((6, 6)))

plt.axis("off"), plt.gray()
plt.xlim(0, img.shape[1])
plt.ylim(0, img.shape[0])
plt.imshow(img)  # 得到一个 500x500 大小的地图
Out[11]:
<matplotlib.image.AxesImage at 0x16b6d04c070>
2022-05-10T16:08:53.520711 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

获取障碍物信息：

  • 方法1: 输入一张地图图片，转化为灰度图像，设置合理阈值二值化处理，导出 0, 1 矩阵. (0 表示不可通行. 1 表示可通行.)
  • 方法2: 下载高程图, 截取需要的地方

地图大小 500x500; 0表示不可通行, 1表示可以

用于改进的 tips: 开闭运算消除细小区域; 腐蚀操作扩大障碍区域判定范围

生成目标点¶

随机数生成

In [12]:
import random

obstacle = morphology.binary_erosion(img, np.ones((10,10)))  # 扩大障碍物判定范围
target = []
while len(target) < 20:
    p = (random.randint(0, 499), random.randint(0, 499))
    if obstacle[p[1], p[0]]:
        target.append(p)
else:
    target = np.array(target)  # 最后转变为 ndarray

plt.axis("off"), plt.gray()
plt.xlim(0, img.shape[1])
plt.ylim(0, img.shape[0])
plt.imshow(img)
plt.scatter(target[:, 0], target[:, 1], color="#6495ED", s=5, marker="x")
Out[12]:
<matplotlib.collections.PathCollection at 0x16b6d1cc6a0>
2022-05-10T16:09:29.617076 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

存档点¶

通用代码

In [13]:
import pickle
import time

# 数据保存在 assets 下面

def save(var, name="data"):
    time_str = time.strftime("%Y%m%d%H%M%S")
    filename = f"{time_str} {name}.pickle"
    filepath = f"assets/{filename}"
    with open(filepath, 'wb') as f:
        pickle.dump(var, f)

    print("save {} to \"{}\"".format(type(var), filepath))

def load(filename):
    with open(f"assets/{filename}", 'rb') as f:
        data = pickle.load(f)
    return data

保存到文件

In [14]:
# save(target, "targets")
# save(img, "map")
# save(omg, "original_map")

从文件读取

In [15]:
target = load("20220423164508 targets.pickle")
img = load("20220423164508 map.pickle")
omg = load("20220423171747 original_map.pickle")
plt.axis("off"), plt.gray()
plt.xlim(0, img.shape[1])
plt.ylim(0, img.shape[0])
plt.imshow(img)
plt.scatter(target[:, 0], target[:, 1], color="#6495ED", s=5, marker="x")
Out[15]:
<matplotlib.collections.PathCollection at 0x16b704c0be0>
2022-05-10T16:18:29.683333 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

分配任务点¶

这里的思路是用a星在500x500的网格中进行路径规划, 获得路径长度作为邻接矩阵对应位置的权重.

n个目标点就是规划 $1+2+...+n$ 个, 计算量比较大.

可以用一些更快的方法, 不需要获得精确的距离, 只需要获得的距离之间大小关系一致.

或者可以保存下来路径, 后续基于这个图用其他方法进行目标点排序, 排完再把路径拼起来

然后根据

构建邻接矩阵¶

距离是按照像素计算的.

WARNING: 计算时间比较久

  • 20 个目标点会计算 3min
  • 30 个目标点会计算 4-5min
  1. 输入 n 个数据
  2. 计算 n 个数据两两之间的距离，对自己的距离是 0
  3. 高斯函数将距离映射到 0-1 区间内
  4. 计算获得的数据组成了 nxn 的一个沿对角线对称的矩阵
In [106]:
from assets.astar import GridWithWeights, a_star_search, reconstruct_path

target_list = [(t[0],t[1]) for t in target]
graph = GridWithWeights(img)


def get_distance(graph, start, goal):
    came_from, cost_so_far = a_star_search(graph, start, goal)
    path = reconstruct_path(came_from, start, goal)
    return len(path) - 1


n = len(target_list)
adj = np.ones((n, n)) # adjacency matrix
for i, p1 in enumerate(target_list):
    for j, p2 in enumerate(target_list[i + 1:]):
        # 计算 P1,P2 之间的距离
        distance = get_distance(graph, p1, p2)
        # 矩阵对应位置赋值
        adj[[i, i + j + 1], [i + j + 1, i]] = distance

调参确定 gamma, gamma 越小, 数据越靠近1

In [115]:
gamma = 5e-6
affinity_matrix = np.exp(-gamma * adj**2)
for i in affinity_matrix:
    for index, j in enumerate(i):
        if j != 1.0:
            plt.scatter(index, j)
2022-04-23T17:14:20.139008 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

聚类¶

In [126]:
# from sklearn.cluster import KMeans
from sklearn.cluster import SpectralClustering

# 前面算好 affinity_matrix
cluster = 3
sc = SpectralClustering(n_clusters=cluster, affinity='precomputed')
result_sc = sc.fit_predict(affinity_matrix)

plt.axis("off")
plt.xlim(0, omg.shape[1])
plt.ylim(0, omg.shape[0])
plt.imshow(omg)
# plt.scatter(target[:, 0], target[:, 1], color="#6495ED", s=6)  # 画上目标点

plt.scatter(target[:, 0],
            target[:, 1],
            c=result_sc,
            marker='x',
            s=5,
            cmap="tab10")  # 画上目标点
Out[126]:
<matplotlib.collections.PathCollection at 0x279ee4dbd60>
2022-04-23T17:18:23.792329 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

存档点¶

In [151]:
readme = """
img: 20220423164508 map.pickle
omg: 20220423171747 original_map.pickle
target: 20 个目标点 ndarry 20220423164508 targets.pickle
cluster: 3 个聚类结果
affinity_matrix: 邻接矩阵
"""

MAP = {
    "omg": omg,  # 底图
    "img": img,  # 二值化后障碍信息
    "target": target,  # 目标点
    "cluster": result_sc,  # 目标点聚类结果(聚类数自己看)
    "affinity_matrix": affinity_matrix,  # 邻接矩阵, 以免想要重新聚类
    "readme": readme
}

# save(MAP,"MAP")
MAP = load("20220423172802 MAP.pickle")
# MAP = load("MAP 20220212162020.pickle")
omg = MAP["omg"]
img = MAP["img"]
target = MAP["target"]
cluster = MAP["cluster"]

路径规划¶

工具¶

In [149]:
from assets.som import Network
# 画图

def plot_path(img, targets, network):
    fig = plt.figure()
    axis = fig.subplots()
    axis.set_xlim(0, img.shape[1])
    axis.set_ylim(0, img.shape[0])
    plt.axis("off")
    axis.imshow(img)  # 画地图

    axis.scatter(targets[:, 0], targets[:, 1], marker='x', s=2.5,
                 c="r")  # 画任务点
    axis.plot(network[:, 0], network[:, 1])  # 画路线
    return plt.axis

# 单线 SOM 的话, 确定起点和终点


def get_start_end(array):
    # (maximum - minimum) along an axis.
    index = array.ptp(axis=0).argmax()  # 获得相距最远的轴 (x or y)
    start_index = array.argmin(axis=0)[index]
    end_index = array.argmax(axis=0)[index]
    start = array[start_index]
    end = array[end_index]
    return start, end


# start, end = get_start_end(target) # 确定起始位置
# net = Network(target, img, start, end)
# net.training()
# path_lenghth = net.route_distance()
# print(path_lenghth, "km")
# plot_path(omg, target, net.network)
# plt.scatter(start[0], start[1], marker='o', c="r", s=5)  # 画起点
# plt.scatter(end[0], end[1], marker='o', c="r", s=5)  # 画终点

SOM 路径规划¶

In [150]:
result = cluster
distances = []
routes = []

for i in range(result.max() + 1):
    subt = target[result == i]
    start, end = get_start_end(subt)
    net = Network(subt, img, start, end)
    net.training()
    routes.append(net.network)
    distances.append(net.route_distance())
plt.xlim(0, omg.shape[1])
plt.ylim(0, omg.shape[0])
plt.axis("off")
plt.imshow(omg)  # 画地图

plt.scatter(target[:, 0],
            target[:, 1],
            c=cluster,
            marker='x',
            s=5,
            cmap="tab10")  # 画任务点
for path in routes:  # 画路线
    plt.plot(path[:, 0], path[:, 1])
    # axis.scatter(path[:,0],path[:,1],s=1)
2022-04-23T17:54:11.170506 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

存档点¶

In [6]:
readme = """
20220423172802 MAP.pickle, 30 个目标点, 3组分类 (全部都是 numpy 矩阵)
"""

# PATH = {
#     "routes": routes,  # list of paths array
#     "distances": distances,
#     "readme": readme
# }

# save(PATH, "PATHS")
PATH = load("20220423175003 PATHS.pickle")
routes = PATH["routes"]
distances = PATH["distances"]
MAP = load("20220423172802 MAP.pickle")
omg = MAP["omg"]
img = MAP["img"]
target = MAP["target"]
cluster = MAP["cluster"]

plt.xlim(0, omg.shape[1])
plt.ylim(0, omg.shape[0])
plt.axis("off")
plt.imshow(omg)  # 画地图

plt.scatter(target[:, 0],
            target[:, 1],
            c=cluster,
            marker='x',
            s=50,
            cmap="tab10")  # 画任务点
for path in routes:  # 画路线
    plt.plot(path[:, 0], path[:, 1])
    # axis.scatter(path[:,0],path[:,1],s=1)
2022-04-24T16:41:58.400875 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

路线平滑处理¶

In [11]:
from scipy.interpolate import make_interp_spline


def Bspline(path):
    """考虑路径长度进行圆滑处理
    默认输入路径 15m 一个路径点, 输出 0.7m 1个路径点
    """
    di_vec = np.diff(path, n=1, axis=0)  # 沿行一阶差分
    di_dis = np.linalg.norm(di_vec, axis=1)  # 相邻路径点之间的欧氏距离
    di_dis = np.insert(di_dis, 0, 0, axis=0)  # 开头插入一个 0
    di_overall = sum(di_dis)  # 路线总长度
    di_index = np.cumsum(di_dis) / di_overall  # 先累积距离，再除以总长度，变成0-1的索引

    # 初次平滑, 隔 20m 输出一个路径点 "降采样"
    spl1 = make_interp_spline(di_index, path, bc_type="clamped")
    t1 = np.linspace(0, 1, int(di_overall // 20))
    splined_routes1 = spl1(t1)

    # 二次平滑, 每隔 1m 输出一个路径点
    spl2 = make_interp_spline(t1, splined_routes1, bc_type="clamped")
    t2 = np.linspace(0, 1, int(di_overall // 1))
    splined_routes2 = spl2(t2)
    return splined_routes2


# # test code
# path=routes[0]
# distance=distances[0]

# splined_routes = Bspline(path)
# plt.plot(path[:, 0], path[:, 1], '-', alpha=0.25, c="gray",linewidth=3)
# plt.plot(splined_routes[:, 0], splined_routes[:, 1], '-',alpha=0.5)
# plt.scatter(splined_routes[:, 0], splined_routes[:, 1],s=1, c="red")
In [12]:
splined_routes = []
for path in routes:
    splined_routes.append(Bspline(path))

plt.axis("off")
plt.xlim(0, omg.shape[1])
plt.ylim(0, omg.shape[0])
plt.imshow(omg)
# plt.scatter(target[:, 0], target[:, 1], color="#6495ED", s=6)  # 画上目标点

plt.scatter(target[:, 0],
            target[:, 1],
            c=cluster,
            marker='x',
            s=5,
            cmap="tab10")  # 画上目标点

# 绘图比较 灰色的是原始, 彩色的是平滑过后的
for i, j in zip(routes, splined_routes):
    plt.plot(i[:, 0], i[:, 1], '-', alpha=0.75, c="black", linewidth=2)
    plt.plot(j[:, 0], j[:, 1], '-', linewidth=1)
2022-04-24T16:42:43.685770 image/svg+xml Matplotlib v3.4.3, https://matplotlib.org/

最终结果输出¶

输出一组 targets & path

matlab 沿 path.csv 仿真

In [13]:
# splined_route = splined_routes[0]
# plt.plot(splined_route[:, 0], splined_route[:, 1], '-', linewidth=1)
# st = target[cluster==0]
# plt.scatter(st[:,0],st[:,1],marker="x")

for index, path in enumerate(splined_routes):
    np.savetxt(f"path{index}.csv", path, delimiter=',', fmt='%.3f')
np.savetxt("all_targets.csv", target, delimiter=',', fmt='%.3f')