求满足两点航向的平滑曲线计算方法及SciPy实现方案

一、数学方法：三次贝塞尔曲线（带航向约束） #

图中红色曲线适合用带端点导数约束的三次贝塞尔曲线实现，它可以精确控制起点、终点位置，以及两端的切线方向（对应航向要求），同时保证曲线平滑连续。

核心公式 #

三次贝塞尔曲线的参数方程为：

B(t) = (1-t)³P₀ + 3(1-t)²tP₁ + 3(1-t)t²P₂ + t³P₃,  t∈[0,1]

参数说明：

• P₀：起点坐标，P₃：终点坐标
• P₁、P₂：由航向约束推导的控制点：
  • 起点航向θ₀对应切线向量(cosθ₀, sinθ₀)，因此P₁ = P₀ + k*(cosθ₀, sinθ₀)，k为张力参数（可按两点距离的1/4~1/3取值，调整曲线弯曲程度）
  • 终点航向θ₃对应切线向量(cosθ₃, sinθ₃)，因此P₂ = P₃ - k*(cosθ₃, sinθ₃)

如果需要更高的曲率连续性，可改用五次贝塞尔曲线，但三次曲线已能满足多数平滑需求。

二、SciPy实现方案 #

方法1：带导数约束的三次样条插值 #

利用scipy.interpolate.make_interp_spline，直接传入端点航向对应的导数约束生成曲线：

import numpy as np
from scipy.interpolate import make_interp_spline
import matplotlib.pyplot as plt

# 定义基础参数
start_point = np.array([0, 0])
end_point = np.array([5, 3])
start_heading = np.pi/4  # 起点航向45度（弧度）
end_heading = np.pi*3/4  # 终点航向135度（弧度）

# 转换航向为导数（切线分量）
dx_start, dy_start = np.cos(start_heading), np.sin(start_heading)
dx_end, dy_end = np.cos(end_heading), np.sin(end_heading)

# 创建带边界导数约束的三次样条
spl_x = make_interp_spline([0, 1], [start_point[0], end_point[0]], 
                           k=3, bc_type=((1, dx_start), (1, dx_end)))
spl_y = make_interp_spline([0, 1], [start_point[1], end_point[1]], 
                           k=3, bc_type=((1, dy_start), (1, dy_end)))

# 生成曲线上的采样点
t = np.linspace(0, 1, 100)
curve_x, curve_y = spl_x(t), spl_y(t)

# 可视化
plt.plot(curve_x, curve_y, 'r-', label='平滑曲线')
plt.plot(start_point[0], start_point[1], 'bo', label='起点')
plt.plot(end_point[0], end_point[1], 'go', label='终点')
# 绘制航向箭头
plt.arrow(start_point[0], start_point[1], dx_start*0.5, dy_start*0.5, head_width=0.1, color='b')
plt.arrow(end_point[0], end_point[1], dx_end*0.5, dy_end*0.5, head_width=0.1, color='g')
plt.legend()
plt.axis('equal')
plt.show()

方法2：手动实现三次贝塞尔曲线 #

直接按公式计算控制点与曲线点，可控性更强：

import numpy as np
import matplotlib.pyplot as plt

def cubic_bezier(t, P0, P1, P2, P3):
    return (1-t)**3 * P0 + 3*(1-t)**2 * t * P1 + 3*(1-t)*t**2 * P2 + t**3 * P3

# 定义基础参数
start_point = np.array([0, 0])
end_point = np.array([5, 3])
start_heading = np.pi/4
end_heading = np.pi*3/4

# 计算控制点，k取两点距离的1/4
k = np.linalg.norm(end_point - start_point) / 4
P1 = start_point + k * np.array([np.cos(start_heading), np.sin(start_heading)])
P2 = end_point - k * np.array([np.cos(end_heading), np.sin(end_heading)])

# 生成曲线采样点
t = np.linspace(0, 1, 100)
curve = np.array([cubic_bezier(ti, start_point, P1, P2, end_point) for ti in t])

# 可视化
plt.plot(curve[:,0], curve[:,1], 'r-', label='贝塞尔平滑曲线')
plt.plot(start_point[0], start_point[1], 'bo')
plt.plot(end_point[0], end_point[1], 'go')
plt.arrow(start_point[0], start_point[1], np.cos(start_heading)*0.5, np.sin(start_heading)*0.5, head_width=0.1, color='b')
plt.arrow(end_point[0], end_point[1], np.cos(end_heading)*0.5, np.sin(end_heading)*0.5, head_width=0.1, color='g')
plt.legend()
plt.axis('equal')
plt.show()

三、方案说明 #

• 三次贝塞尔曲线：计算简单，调整k值可灵活改变曲线弯曲程度，k越大曲线偏离直线越明显
• 带约束的样条插值：自动生成曲率更均匀的曲线，适合对平滑度要求更高的场景

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