路径转角正态分布概率重叠求解：如何实现路径中心概率均匀且边缘低？

我正在实现路径中心分配高概率、边缘分配低概率的功能，当前使用如下代码基于正态分布计算路径概率密度函数（PDF）：

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

gridPath = "resources/myGrid3/"
pathFile = "pathNorm.csv"
myFile = "myPath8.csv"

pathDf = pd.read_csv(gridPath + pathFile)
myDf = pd.read_csv(gridPath + myFile)

pathDf = pathDf.drop_duplicates(["x"])

maxXGrid = pathDf.x.max() + 2
maxYGrid = pathDf.y.max() + 2
minXGrid = pathDf.x.min() - 2
minYGrid = pathDf.y.min() - 2

dx1 = 0.1
dx2 = 0.1

x1 = np.arange(minXGrid, maxXGrid, dx1)
x2 = np.arange(minYGrid, maxYGrid, dx2)
X1, X2 = np.meshgrid(x1, x2)
X = np.column_stack((X1.flatten(), X2.flatten()))

sigma_p = 0.7

path_pdf = np.sum((1 / (np.sqrt(2 * np.pi) * sigma_p)) * np.exp(-0.5 * (1 / (sigma_p ** 2)) * (np.sqrt((X1[:, :, None] - pathDf.x.values) ** 2 + (X2[:, :, None] - pathDf.y.values) ** 2)) ** 2), axis=2)
path_pdf /= np.sum(path_pdf)  # Normalize the pdf

fig = plt.figure(3)
ax = fig.add_subplot(111)
f = ax.pcolor(X1, X2, path_pdf)
plt.colorbar(f, ax=ax)
ax.scatter(myDf.x, myDf.y, color='white', label="Path", marker="x", s=0.1)
ax.set_xlabel('$x$-axis (m)', fontsize=12)
ax.set_ylabel('$y$-axis (m)', fontsize=12)
ax.scatter(pathDf.x.iloc[0], pathDf.y.iloc[0], c='b', marker='o', label='Start')
ax.set_aspect('equal')
ax.set_title('PDF of Path')
ax.legend()

plt.show()

当前结果中，转角处因路径点密集导致概率值叠加过高，我希望路径上所有点的概率值一致，请问有没有办法避免转角处的概率重叠？

解决方案 #

方法1：基于路径距离场生成PDF（最直接有效） #

放弃逐个路径点叠加正态分布的方式，改为计算每个网格点到整条路径的最短距离，再将距离转换为概率。这样不管路径点密度如何，概率只和到路径的距离相关，自然避免转角处的叠加问题。

修改后的核心代码如下：

import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

gridPath = "resources/myGrid3/"
pathFile = "pathNorm.csv"
myFile = "myPath8.csv"

pathDf = pd.read_csv(gridPath + pathFile)
myDf = pd.read_csv(gridPath + myFile)

pathDf = pathDf.drop_duplicates(["x"])
path_points = pathDf[['x', 'y']].values

maxXGrid = pathDf.x.max() + 2
maxYGrid = pathDf.y.max() + 2
minXGrid = pathDf.x.min() - 2
minYGrid = pathDf.y.min() - 2

dx1 = 0.1
dx2 = 0.1

x1 = np.arange(minXGrid, maxXGrid, dx1)
x2 = np.arange(minYGrid, maxYGrid, dx2)
X1, X2 = np.meshgrid(x1, x2)
grid_points = np.column_stack((X1.flatten(), X2.flatten()))

sigma_p = 0.7

# 计算每个网格点到路径的最短距离
def distance_to_path(grid_points, path_points):
    dists = []
    for i in range(len(path_points)-1):
        p1 = path_points[i]
        p2 = path_points[i+1]
        vec = p2 - p1
        len_sq = np.dot(vec, vec)
        if len_sq == 0:
            dist = np.linalg.norm(grid_points - p1, axis=1)
        else:
            t = np.clip(np.dot(grid_points - p1, vec) / len_sq, 0, 1)
            proj = p1 + t[:, np.newaxis] * vec
            dist = np.linalg.norm(grid_points - proj, axis=1)
        dists.append(dist)
    return np.min(np.array(dists), axis=0)

min_dists = distance_to_path(grid_points, path_points)
min_dists = min_dists.reshape(X1.shape)

# 转换为概率密度函数
path_pdf = (1 / (np.sqrt(2 * np.pi) * sigma_p)) * np.exp(-0.5 * (min_dists / sigma_p) ** 2)
path_pdf /= np.sum(path_pdf)  # 归一化

# 绘图部分和原代码一致
fig = plt.figure(3)
ax = fig.add_subplot(111)
f = ax.pcolor(X1, X2, path_pdf)
plt.colorbar(f, ax=ax)
ax.scatter(myDf.x, myDf.y, color='white', label="Path", marker="x", s=0.1)
ax.set_xlabel('$x$-axis (m)', fontsize=12)
ax.set_ylabel('$y$-axis (m)', fontsize=12)
ax.scatter(pathDf.x.iloc[0], pathDf.y.iloc[0], c='b', marker='o', label='Start')
ax.set_aspect('equal')
ax.set_title('PDF of Path (Distance Field Based)')
ax.legend()

plt.show()

方法2：对路径点进行弧长均匀采样 #

如果坚持用点叠加的方式，可以先对原始路径按弧长重新采样，让每个采样点之间的弧长一致，避免转角处点过于密集。

核心代码示例：

# 计算路径累计弧长
path_points = pathDf[['x', 'y']].values
diffs = np.diff(path_points, axis=0)
seg_lengths = np.linalg.norm(diffs, axis=1)
cum_lengths = np.concatenate([[0], np.cumsum(seg_lengths)])
total_length = cum_lengths[-1]

# 按固定间隔采样（比如每0.5米一个点）
sample_interval = 0.5
sample_lengths = np.arange(0, total_length, sample_interval)
sampled_points = []

for l in sample_lengths:
    seg_idx = np.searchsorted(cum_lengths, l) - 1
    if seg_idx >= len(seg_lengths):
        sampled_points.append(path_points[-1])
        continue
    seg_start = cum_lengths[seg_idx]
    seg_rel_pos = (l - seg_start) / seg_lengths[seg_idx]
    sampled_point = path_points[seg_idx] + seg_rel_pos * diffs[seg_idx]
    sampled_points.append(sampled_point)

sampled_points = np.array(sampled_points)

# 后续用采样点计算叠加PDF
path_pdf = np.sum((1 / (np.sqrt(2 * np.pi) * sigma_p)) * np.exp(-0.5 * (1 / (sigma_p ** 2)) * (np.sqrt((X1[:, :, None] - sampled_points[:,0]) ** 2 + (X2[:, :, None] - sampled_points[:,1]) ** 2)) ** 2), axis=2)
path_pdf /= np.sum(path_pdf)

方法3：路径点权重归一化 #

给每个原始路径点分配权重，权重与该点和相邻点的距离成反比（密集点权重低，稀疏点权重高），这样叠加后总概率均匀。

核心代码示例：

path_points = pathDf[['x', 'y']].values
# 计算每个点的权重
weights = np.ones(len(path_points))
# 首尾点权重取相邻点距离的一半
weights[0] = np.linalg.norm(path_points[0] - path_points[1]) / 2
weights[-1] = np.linalg.norm(path_points[-1] - path_points[-2]) / 2
# 中间点权重取左右相邻点距离的平均值的一半
for i in range(1, len(path_points)-1):
    left_dist = np.linalg.norm(path_points[i] - path_points[i-1])
    right_dist = np.linalg.norm(path_points[i] - path_points[i+1])
    weights[i] = (left_dist + right_dist) / 4
# 归一化权重
weights /= np.sum(weights)

# 计算PDF时乘以权重
path_pdf = np.sum(weights[None, None, :] * (1 / (np.sqrt(2 * np.pi) * sigma_p)) * np.exp(-0.5 * (1 / (sigma_p ** 2)) * (np.sqrt((X1[:, :, None] - pathDf.x.values) ** 2 + (X2[:, :, None] - pathDf.y.values) ** 2)) ** 2), axis=2)
path_pdf /= np.sum(path_pdf)

内容的提问来源于stack exchange，提问作者Park Bo