如何使用Python和matplotlib绘制机器人运动偏差路径图——实用教程与代码解析

前言

科研论文中需要绘制一个精美的机器人运动路径图（其中包含了机器人的期望运动路径和实际运动路径）。我的期望路径是是一个方形，用Python代码是这样表示的：

n_list = [n1,n2,n3,n4]
e_list = [e1,e2,e3,e4] #n_list和e_list的各点分别是这个方形期望路径的四个顶点
plt.plot(e_list,n_list)

现在我已经拥有了数据，但是我发现使用Python中matplotlib.plt.plot来直接绘制不够精美，我有以下要求：

1. 绘制机器人运动路径图代码由较高美观程度
2. 要在图中表现偏离程度

基本思路

要在图中表现偏离程度，需要计算每个实际路径点到期望路径的最近点的距离。考虑到期望路径是由一系列顶点定义的，我们可以使用这些顶点来构造一系列的线段，然后计算实际路径点到这些线段的最短距离。

本博客提供了一个方法，基本思路是，通过遍历每个实际路径点，对于每一个点都遍历4条线段，通过计算到这个线段的垂直距离来获取距离该点最近的线段和相应距离（即偏离程度），然后，使用颜色或者透明度来表现这个偏离程度。

在下面的代码中，首先定义了一个函数来计算点到线段的最短距离，然后遍历所有的实际路径点来计算它们的偏离程度。最后，将这些偏离程度用颜色表现出来。

代码

下面提供实例，代码作为模板

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.collections import LineCollection

# 四个顶点具体化，这里构造一个简单的正方形
n1, e1 = (0, 0)  # 第一个顶点
n2, e2 = (0, 10)  # 第二个顶点
n3, e3 = (10, 10)  # 第三个顶点
n4, e4 = (10, 0)  # 第四个顶点

# 创建期望路径的顶点数组
vertices = np.array([[e1, n1], [e2, n2], [e3, n3], [e4, n4], [e1, n1]])  # 最后回到第一个点形成闭合

# 生成实际路径的数据（在期望路径上添加一些随机噪声）
np.random.seed(0)
# actual_path = vertices + np.random.normal(0, 0.5, size=vertices.shape)  # 给顶点添加噪声模拟实际路径
expected_x = np.concatenate([np.linspace(0, 10, 25), np.ones(25)*10, np.linspace(10, 0, 25), np.zeros(25)])
expected_y = np.concatenate([np.zeros(25), np.linspace(0, 10, 25), np.ones(25)*10, np.linspace(10, 0, 25)])

# 生成实际路径的数据（在期望路径上添加一些随机噪声）
actual_x = expected_x + np.random.normal(0, 0.1, size=expected_x.shape)
actual_y = expected_y + np.random.normal(0, 0.1, size=expected_y.shape)
actual_path = np.array(list(zip(actual_x,actual_y)))

# 创建期望路径的线段（每对相邻的顶点）
segments = np.concatenate([vertices[:-1], vertices[1:]], axis=1).reshape(-1, 2, 2)

# 函数计算点到线段的最短距离
def point_to_line_dist(point, line_points):
    p1, p2 = line_points
    p1_to_point = np.array(point) - np.array(p1)
    p2_to_p1 = np.array(p2) - np.array(p1)
    
    # 使用外积计算叉乘结果的模（对于二维向量，外积可以通过计算向量的行列式得到）
    cross_product_mod = np.abs(p1_to_point[0]*p2_to_p1[1] - p1_to_point[1]*p2_to_p1[0])
    
    # 计算线段p2_to_p1的模长的平方
    p2_to_p1_mod_squared = np.dot(p2_to_p1, p2_to_p1)
    
    # 根据公式计算点到线段的垂直距离的平方
    dis = cross_product_mod/ np.sqrt(p2_to_p1_mod_squared)
    return dis

# 计算实际路径点到期望路径的最短距离
deviations = np.array([min([point_to_line_dist(point, segment) for segment in segments]) for point in actual_path])

# 设定颜色映射根据偏离程度
norm = plt.Normalize(deviations.min(), deviations.max())
colors = plt.cm.viridis(norm(deviations))

# 绘制期望路径和实际路径
plt.figure(figsize=(8, 8))
plt.plot(vertices[:, 0], vertices[:, 1], label='Expected Path', color='blue', linewidth=2)

# 使用 LineCollection 绘制带有颜色映射的实际路径
lc = LineCollection(zip(actual_path[:-1], actual_path[1:]), colors=colors, linewidth=2, cmap='viridis', norm=norm)
plt.gca().add_collection(lc)

plt.colorbar(lc, label='Deviation Distance')

# 添加图表标题和坐标轴标签
plt.title('Robot Motion Path with Deviation')
plt.xlabel('E position')
plt.ylabel('N position')
plt.legend()
plt.axis('equal')  # 设置坐标轴比例相同
plt.show()

部分代码解释

point_to_line_dist函数的作用？

point_to_line_dist函数通过叉乘计算了点到线段的垂直距离。在二维空间中，叉乘的模可以通过计算向量的外积得到。然后，让这个值除以线段向量的模，以获得垂直距离。

zip(actual_path[:-1], actual_path[1:]) 的意思是？

在Python中，zip()函数是一个内置函数，它接受一系列的可迭代对象作为参数，将它们“压缩”成一个个元组，然后返回由这些元组组成的一个迭代器。每个元组包含来自每个可迭代对象的一个元素，元素的索引相同。当用在两个列表上时，它会把两个列表对应位置的元素配对。当使用zip()来处理两个相同列表的不同切片时，它可以用来创建一系列相邻元素的配对。

在代码段中：

zip(actual_path[:-1], actual_path[1:])

这段代码正在处理actual_path列表。actual_path[:-1]表示actual_path列表中的第一个到倒数第二个元素，actual_path[1:]表示从第二个元素到最后一个元素。因此，zip(actual_path[:-1], actual_path[1:])将创建一个元组的序列，其中每个元组包含一对相邻的点（即列表中当前点和下一个点），形如：(actual_path[i], actual_path[i+1])。

举个例子：

如果 actual_path 是 [A, B, C, D]，那么 zip(actual_path[:-1], actual_path[1:]) 将产生：

[(A, B), (B, C), (C, D)]

结果

亲测即便点的数量达到9256个时，计算速度仍然很快（几乎仍然是瞬间完成的）。

延伸阅读