# SplineTrajectory

**Repository Path**: byronartest/SplineTrajectory

## Basic Information

- **Project Name**: SplineTrajectory
- **Description**: C 语言实现PVT三次样条插值
- **Primary Language**: C
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 3
- **Created**: 2024-08-05
- **Last Updated**: 2024-08-05

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# Cubic Spline 说明

三次样条插值公式为：
$$
P(t) = a(t-t_0)^3 + b(t-t_0)^2 + c(t-t_0) + d
$$
$$
V(t) = \dot{P(t)} = 3a(t-t_0)^2 + 2b(t-t_0) + c
$$
其中，P(t)表示t时刻的位置，V(t)表示该位置点的速度, a, b, c, d分别为多项式的4个系数。
给定 $t_0$时刻的$P(t_0)$和$V(t_0)$, 以及$t = t_x$时刻的$P(t_x)$和$V(t_x)$可以插值得到$t_0, t_x$中间任意时刻点的位置和速度。
求解方法如下：
给定$P(t_0) = P0$, $V(t_0) = V0$,则：
$$
d=P0
$$
$$
c=V0
$$
给定$P(t_0 + T) = PT$, $V(t_0+T) = VT$,则:
$$
PT = aT^3+bT^2+cT+d
$$
$$
VT = 3aT^2+2bT+c
$$
可得:
$$
b=(3 \cdot PT - VT \cdot T - 2V0 \cdot T - 3P0)/(T^2)
$$
$$
a=(-2 \cdot PT + VT \cdot T + V0 \cdot T + 2P0)/(T^3)
$$

得到a, b, c, d四个系数以后，可以有文档开头的第一个公式得到时刻t的P(t)和V(t)

测试效果如下：

![img](DataAnalysis/spline_test.png)