一组点上的速度剖面

如何解决一组点上的速度剖面

目前我正在研究自动驾驶汽车，我的问题是改善车辆沿曲线的最大速度曲线。换句话说：汽车在直线上可以开得更快，但当附近有弯道时必须减速。我的目标是了解它应该以何种速度驱动曲线。

特别是，车辆每秒钟都会收到一些在 x,y 坐标 (Here the points) 中定义车辆轨迹的规划路径点。每一秒它都会收到类似的东西 Path planned，蓝色表示计划中的点，红色表示车辆的位置。

到目前为止，我所做的是沿曲线定义弧长 (s)，并根据弧长写出 x 和 y 点，例如：x(s) 和 y(s)。 然后我将整个曲线分成更小的间隔，每个间隔包含 55 个点，然后我使用 Python 的 5 阶多项式对这些点进行插值（WPsTmp），并计算 x 和 y 的一阶和二阶导数：

c_path_x = np.polyfit(sInterp,WPsTmp[:,0],5)     #interpolation of the x points
      
c_path_x_d = np.polyder(c_path_x)                  #1st derivative

c_path_x_dd = np.polyder(c_path_x_d)               #2nd derivative

知道了 x 和 y，有了导数，我就可以使用 following formula 计算每个点的曲率，这为我提供了路径 (K_path) 中每个点的曲率 K 值。

之后，根据对车辆的要求，我知道必须限制向心加速度 (ac) 以保证乘客的舒适度，并且使用向心加速度方程我能够计算出速度： V_max_tmp = np.sqrt(ac/abs(K_path))

V_max_tmp 是一个数组，用于保存路径每个点的速度值。取这个数组的最小值让我限制曲线中的速度值。 对汽车模拟时间的每一秒都这样做，我能够随时间构建汽车的速度曲线： Here 特别是在曲线中获得了振荡值，我的算法在这些曲线中表现不佳。我想改进它并有一个更平滑的趋势。 我想问你是否知道如何提高该算法在曲线中的性能，或者是否有其他方法可以计算给定点上的最大速度（和曲率）。

            `