A星路径平滑之三次样条插值
理论推导
三次样条插值理论推导
- 假设三次曲线如下
S i ( x ) = a i + b i ( x − x i ) + c i ( x − x i ) 2 + d i ( x − x i ) 3 , i = 0 , 1 , . . . n − 1. \begin{equation}S_i(x) = a_i + b_i(x - x_i) + c_i(x - x_i)^2 + d_i(x - x_i)^3, i = 0,1,...n-1.\end{equation} Si(x)=ai+bi(x−xi)+ci(x−xi)2+di(x−xi)3,i=0,1,...n−1.
一阶导
S i ′ ( x ) = b i + 2 c i ( x − x i ) + 3 d i ( x − x i ) 2 , i = 0 , 1 , . . . n − 1. \begin{equation}S_i'(x) = b_i + 2c_i(x - x_i) + 3d_i(x - x_i)^2, i = 0,1,...n-1.\end{equation} Si′(x)=bi+2ci(x−xi)+3di(x−xi)2,i=0,1,...n−1.
二阶导
S i ′ ′ ( x ) = 2 c i + 6 d i ( x − x i ) , i = 0 , 1 , . . . n − 1. \begin{equation}S_i''(x) = 2c_i + 6d_i(x - x_i), i = 0,1,...n-1.\end{equation} Si′′(x)=2ci+6di(x−xi),i=0,1,...n−1.
那么n个区间对应的表达式如下
S ( x ) = { S 0 ( x ) = a 0 + b 0 ( x − x 0 ) + c 0 ( x − x 0 ) 2 + d 0 ( x − x 0 ) 3 S 1 ( x ) = a 1 + b 1 ( x − x 1 ) + c 1 ( x − x 1 ) 2 + d 1 ( x − x 1 ) 3 . . S n − 1 ( x ) = a n − 1 + b n − 1 ( x − x n − 1 ) + c n − 1 ( x − x n − 1 ) 2 + d n − 1 ( x − x n − 1 ) 3 \begin{equation}S(x)= \begin{cases}S_0(x) = a_0 + b_0(x - x_0) + c_0(x - x_0)^2 + d_0(x - x_0)^3 \\ S_1(x) = a_1 + b_1(x - x_1) + c_1(x - x_1)^2 + d_1(x - x_1)^3 \\ .\\ .\\ S_{n-1}(x) = a_{n-1} + b_{n-1}(x - x_{n-1}) + c_{n-1}(x - x_{n-1})^2 + d_{n-1}(x - x_{n-1})^3 \\ \end{cases}\\ \\ \end{equation} S(x)=⎩ ⎨ ⎧S0(x)=a0+b0(x−x0)+c0(x−x0)2+d0(x−x0)3S1(x)=a1+b1(x−x1)+c1(x−x1)2+d1(x−x1)3..Sn−1(x)=an−1+bn−1(x−xn−1)+cn−1(x−xn−1)2+dn−1(x−xn−1)3
S ( x i ) = y i , i = 0 , 1 , . . . n S(x_i) = y_i ,i = 0,1,...n S(xi)=yi,i=0,1,...n
上面有n个方程，每个方程4个未知变量，则一共有4n个未知数，则至少需要4n个方程才能求解出所有未知变量
求解未知变量
条件1 起点条件:
假设给定一系列离散的点{[x0 y0],[x1 y1]…[x(n) y(n)]} 其中每一个点都是曲线的起点那么都满足
将每个点都代入到函数中可得:
y
i
=
S
i
(
x
i
)
=
a
i
+
b
i
(
x
i
−
x
i
)
+
c
i
(
x
i
−
x
i
)
2
+
d
i
(
x
i
−
x
i
)
3
,
i
=
0
,
1
,
.
.
.
n
−
1.
\begin{equation}y_i = S_i(x_i) = a_i + b_i(x_i - x_i) + c_i(x_i - x_i)^2 + d_i(x_i - x_i)^3, i = 0,1,...n-1.\end{equation}
yi=Si(xi)=ai+bi(xi−xi)+ci(xi−xi)2+di(xi−xi)3,i=0,1,...n−1.
从而可得
a
i
=
y
i
a_i = y_i
ai=yi即求得未知数中的所有
a
i
a_i
ai(或者可以理解为每个离散的点为曲线的起点)
条件2 终点条件：
当前点为上一段函数的终点即为下一段函数的起点(前一段方程在节点处的函数值和后一段方程在相同节点处的函数值相等)
暂时无法在飞书文档外展示此内容
S
i
(
x
i
+
1
)
=
S
i
+
1
(
x
i
+
1
)
S_i(x_{i+1}) = S_{i+1}(x_{i+1})
Si(xi+1)=Si+1(xi+1) （即
S
i
(
x
i
+
1
)
=
y
i
+
1
S_i(x_{i+1}) = y_{i+1}
Si(xi+1)=yi+1）
令
h
i
=
x
i
+
1
−
x
i
h_i = x_{i+1}-x_i
hi=xi+1−xi 可得（
x
i
+
1
x_{i+1}
xi+1可以理解为一段曲线的终点）
a
i
+
h
i
b
i
+
h
i
2
c
i
+
h
i
3
d
i
=
y
i
\begin{equation}a_i + h_ib_i + h^2_ic_i + h^3_id_i = y_i\end{equation}
ai+hibi+hi2ci+hi3di=yi
其中，i=0,1,2…n-2(共得到n-1个方程，因为n段三次函数之间共有n-1的衔接点)
条件3 一阶导相等：在所有节点（除了第一节点和最后一个节点）处1阶连续（保证节点处有相同的斜率,没有急转弯,原函数曲线上没有剧烈的跳变）
令 $$h_i = x_{i+1}-x_i$$ 可得（可以理解为一段曲线的终点）$$x_{i+1}$$
S
i
′
(
x
i
+
1
)
=
S
i
+
1
′
(
x
i
+
1
)
\begin{equation}S_i'(x_{i+1}) = S_{i + 1}'(x_{i+1})\end{equation}
Si′(xi+1)=Si+1′(xi+1)
S
i
′
(
x
)
=
b
i
+
2
c
i
(
x
−
x
i
)
+
3
d
i
(
x
−
x
i
)
2
,
i
=
0
,
1
,
.
.
.
n
−
1.
S_i'(x) = b_i + 2c_i(x - x_i) + 3d_i(x - x_i)^2, i = 0,1,...n-1.
Si′(x)=bi+2ci(x−xi)+3di(x−xi)2,i=0,1,...n−1.
S
i
′
(
x
i
+
1
)
=
b
i
+
2
c
i
h
i
+
3
d
i
h
i
2
S_i'(x_{i+1}) = b_i + 2c_ih_i + 3d_ih_i^2
Si′(xi+1)=bi+2cihi+3dihi2
下一段路径的起点
x
=
x
i
+
1
x = x_{i + 1}
x=xi+1同时在上一段的末尾
x
i
+
1
x_{i + 1}
xi+1进行求导 可得如下
S
i
′
(
x
i
+
1
)
=
b
i
+
1
+
2
c
i
+
1
(
x
i
+
1
−
x
i
+
1
)
+
3
d
i
+
1
(
x
i
+
1
−
x
i
+
1
)
2
=
b
i
+
1
,
i
=
0
,
1
,
.
.
.
n
−
1.
S_i'(x_{i+1}) = b_{i+1} + 2c_{i+1}(x_{i+1} - x_{i+1}) + 3d_{i+1}(x_{i+1} - x_{i+1})^2 = b_{i+1}, i = 0,1,...n-1.
Si′(xi+1)=bi+1+2ci+1(xi+1−xi+1)+3di+1(xi+1−xi+1)2=bi+1,i=0,1,...n−1.
i= 0,1,2…n-2 (共得到n-1个方程，n段三次函数之间共有n-1的衔接点)
因为
S
i
′
(
x
i
+
1
)
=
S
i
+
1
′
(
x
i
+
1
)
S_i'(x_{i+1}) = S_{i + 1}'(x_{i+1})
Si′(xi+1)=Si+1′(xi+1) 所以
b
i
+
1
=
b
i
+
2
c
i
h
i
+
3
d
i
h
i
2
\begin{equation}b_{i+1} = b_i + 2c_ih_i+3d_ih^2_i\end{equation}
bi+1=bi+2cihi+3dihi2
条件4 二阶导相等：在所有节点（除了第一节点和最后一个节点）处2阶连续（保证节点处有相同的曲率，即，相同的弯曲程度）
S
i
′
′
(
x
i
+
1
)
=
S
i
+
1
′
′
(
x
i
+
1
)
\begin{equation}S_i''(x_{i+1}) = S_{i + 1}''(x_{i+1})\end{equation}
Si′′(xi+1)=Si+1′′(xi+1)
S
i
′
′
(
x
i
+
1
)
=
2
c
i
+
6
d
i
(
x
i
+
1
−
x
i
)
=
2
c
i
+
6
d
i
h
i
S_i''(x_{i+1}) = 2c_i+6d_i(x_{i+1} - x_i) = 2c_i+6d_ih_i
Si′′(xi+1)=2ci+6di(xi+1−xi)=2ci+6dihi
S
i
+
1
′
′
(
x
i
+
1
)
=
2
C
i
+
1
+
6
d
i
+
1
(
x
i
+
1
−
x
i
+
1
)
=
2
c
i
+
1
S_{i + 1}''(x_{i+1}) = 2C_{i+1} + 6d_{i+1}(x_{i+1} - x_{i+1}) = 2c_{i+1}
Si+1′′(xi+1)=2Ci+1+6di+1(xi+1−xi+1)=2ci+1
得到
2
c
i
+
6
d
i
h
i
−
2
c
i
+
1
=
0
2c_i+ 6d_ih_i - 2c_{i+1} = 0
2ci+6dihi−2ci+1=0
令
m
i
=
S
i
′
′
(
x
i
)
=
2
c
i
m_i = S''_{i}(x_i) = 2c_i
mi=Si′′(xi)=2ci 可得出
d
i
=
m
i
+
1
−
m
i
6
h
i
\begin{equation}d_i= \cfrac{m_{i+1} - m_i}{6h_i}\end{equation}
di=6himi+1−mi
根据条件
2
将
c
i
和
d
i
代入下面的等式
根据条件2 将c_i 和 d_i 代入下面的等式
根据条件2将ci和di代入下面的等式
a
i
+
h
i
b
i
+
h
i
2
c
i
+
h
i
3
d
i
=
y
i
\begin{equation}a_i + h_ib_i + h^2_ic_i + h^3_id_i = y_i\end{equation}
ai+hibi+hi2ci+hi3di=yi
可得
b
i
=
y
i
+
1
−
y
i
h
i
−
h
i
2
m
i
−
h
i
6
(
m
i
+
1
−
m
i
)
\begin{equation}b_i = \cfrac{y_{i+1}-y_i}{h_i} - \cfrac{h_i}{2}m_i - \frac{h_i}{6}(m_{i+1}-m_i)\end{equation}
bi=hiyi+1−yi−2himi−6hi(mi+1−mi)
将
c
i
d
i
b
i
c_i d_i b_i
cidibi d代入
b
i
+
1
=
b
i
+
2
c
i
h
i
+
3
d
i
h
i
2
\begin{equation}b_{i+1} = b_i + 2c_ih_i+3d_ih^2_i\end{equation}
bi+1=bi+2cihi+3dihi2
得到
h
i
m
i
+
2
(
h
i
+
h
i
+
1
)
m
i
+
1
+
h
i
+
1
m
i
+
2
=
6
[
y
i
+
2
−
y
i
+
1
h
i
+
1
−
y
i
+
1
−
y
i
h
i
]
\begin{equation}h_im_i + 2(h_i+h_{i+1})m_{i+1} + h_{i+1}m_{i+2} = 6[\cfrac{y_{i+2}-y_{i+1}}{h_{i+1}}- \cfrac{y_{i+1}-y_i}{h_i} ]\end{equation}
himi+2(hi+hi+1)mi+1+hi+1mi+2=6[hi+1yi+2−yi+1−hiyi+1−yi]
i= 0,1,2…n-2 (共n-1个方程，n段三次函数之间共有n-1的衔接点)
条件总结
1.离散点从0-n 一共有n+1个点
2. 曲线个数比离散点少1 为n
3. 条件1: 每一个离散点的起点为函数的起点 所以一共有n+1个方程
4. 条件2，3，4 都是两条连续曲线的关系，所以方程都为n-1
5. 总的方程为4n -2
6. 有4n个变量 所以还需要两个条件
理解:曲线的一阶导及二阶导
比如 y = x 2 y = x^2 y=x2 一阶导为 y = 2 x y = 2x y=2x 二阶导为 y = 2 y = 2 y=2 一阶导表明当x大于0时，x越大y变化的越大，二阶导为常数表明曲线向下，即曲线的大方向变化
两个附加条件
1. 自由边界(Natural) Natural样条是柔软又有弹性的木杆经过所有数据点后形成的曲线，让端点的斜率自由的在某一位置保持平衡，使得曲线的摇摆最小。
S
0
′
′
(
x
0
)
=
2
c
0
+
6
d
0
(
x
0
−
x
0
)
=
2
c
0
=
m
0
=
0
S
n
−
1
′
′
(
x
n
)
=
2
c
n
−
1
+
6
d
n
−
1
(
x
n
−
x
n
−
1
)
=
m
n
−
1
+
m
n
−
m
n
−
1
=
m
n
=
0
S''_0(x_0) = 2c_0 + 6d_0(x_0 - x_0) = 2c_0 = m_0 = 0 \\ S''_{n-1}(x_n) = 2c_{n-1} + 6d_{n-1}(x_n - x_{n-1}) = m_{n-1}+m_n-m_{n-1} = m_n = 0
S0′′(x0)=2c0+6d0(x0−x0)=2c0=m0=0Sn−1′′(xn)=2cn−1+6dn−1(xn−xn−1)=mn−1+mn−mn−1=mn=0
7. 以矩阵的形式体现
h
i
m
i
+
2
(
h
i
+
h
i
+
1
)
m
i
+
1
+
h
i
+
1
m
i
+
2
=
6
[
y
i
+
2
−
y
i
+
1
h
i
+
1
−
y
i
+
1
−
y
i
h
i
]
\begin{equation}h_im_i + 2(h_i+h_{i+1})m_{i+1} + h_{i+1}m_{i+2} = 6[\cfrac{y_{i+2}-y_{i+1}}{h_{i+1}}- \cfrac{y_{i+1}-y_i}{h_i} ]\end{equation}
himi+2(hi+hi+1)mi+1+hi+1mi+2=6[hi+1yi+2−yi+1−hiyi+1−yi]
[
1
0
0
0
.
.
.
h
0
2
(
h
0
+
h
1
h
+
1
0
)
0
h
1
2
(
h
1
+
h
2
)
0
.
.
.
]
∗
[
m
0
(
0
)
m
1
m
2
m
3
.
.
.
m
n
(
0
)
]
=
\begin{bmatrix} 1&0&0&0&.&.&. \\ h_0&2(h_0+h_1&h+1&0)\\ 0&h_1&2(h_1+h_2)&0\\ .\\ .\\ .\end{bmatrix} * \begin{bmatrix}m_0(0)\\ m_1\\ m_2\\ m_3\\ .\\ .\\ .\\ m_n(0)\end{bmatrix} =
1h00...02(h0+h1h10h+12(h1+h2)00)0...
∗
m0(0)m1m2m3...mn(0)
=
6
∗
[
0
y
2
−
y
1
h
1
−
y
1
−
y
0
h
0
y
3
−
y
3
h
2
−
y
2
−
y
1
h
1
y
4
−
y
3
h
3
−
y
3
−
y
2
h
2
.
.
.
0
]
6 * \begin{bmatrix}0\\ \cfrac{y_2-y_1}{h_1} - \cfrac{y_1-y_0}{h_0}\\ \cfrac{y_3-y_3}{h_2} - \cfrac{y_2-y_1}{h_1}\\ \cfrac{y_4-y_3}{h_3} - \cfrac{y_3-y_2}{h_2}\\ .\\ .\\ .\\ 0 \end{bmatrix}
6∗
0h1y2−y1−h0y1−y0h2y3−y3−h1y2−y1h3y4−y3−h2y3−y2...0
上公式对应的代码如下
H(0, 0) = 1;
H(size - 1, size - 1) = 1;
for (int i = 1; i < size - 1; i++)
{
H(i, i - 1) = dx[i - 1];
H(i, i) = 2 * (dx[i - 1] + dx[i]);
H(i, i + 1) = dx[i];
Y(i) = 6 * (dy[i] / dx[i] - dy[i - 1] / dx[i - 1]);
//Y(i) = 3 * (dy[i] / dx[i] - dy[i - 1] / dx[i - 1]);
}
M = H.colPivHouseholderQr().solve(Y);
.
完整代码实现
void interpolateCubicSpline()
{
// const double min = 0;
// const double max = 15;
// double x[] = {0, 3, 5, 7, 9, 11, 12, 13, 14, 15};
// double y[] = {0, 1.2, 1.7, 2.0, 2.1, 2.0, 1.8, 1.2, 1.0, 1.6};
// 求解最小值
// 求解最小值
auto minElement = std::min_element(x.begin(), x.end());
double min = static_cast<double>(*minElement);
std::cout << "最小值: " << min << std::endl;
// 求解最大值
auto maxElement = std::max_element(x.begin(), x.end());
double max = static_cast<double>(*maxElement);
std::cout << "最大值: " << max << std::endl;
int size = x.size(); // sizeof(x) / sizeof(double);
vector<double> xx, yy;
double step = 0.1;
double value = x[0];
int num = (max - min) / step;
for (double i = 0; i <= num; i++)
{
xx.push_back(value);
value = value + step;
}
int size_xx = xx.size();
vector<double> dx;
vector<double> dy;
for (int i = 0; i < size - 1; i++)
{
double temp_dx = x[i + 1] - x[i];
dx.push_back(temp_dx);
double temp_dy = y[i + 1] - y[i];
dy.push_back(temp_dy);
}
MatrixXd H = MatrixXd::Random(size, size);
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size; j++)
{
H(i, j) = 0;
}
}
VectorXd Y(size);
for (int i = 0; i < size; i++)
{
Y(i) = 0;
}
VectorXd M(size);
for (int i = 0; i < size; i++)
{
M(i) = 0;
}
H(0, 0) = 1;
H(size - 1, size - 1) = 1;
for (int i = 1; i < size - 1; i++)
{
H(i, i - 1) = dx[i - 1];
H(i, i) = 2 * (dx[i - 1] + dx[i]);
H(i, i + 1) = dx[i];
Y(i) = 6 * (dy[i] / dx[i] - dy[i - 1] / dx[i - 1]);
//Y(i) = 3 * (dy[i] / dx[i] - dy[i - 1] / dx[i - 1]);
}
// std::cout << "Matrix Y:" << std::endl
// << Y << std::endl;
// std::cout << "Matrix H:" << std::endl
// << H << std::endl;
M = H.colPivHouseholderQr().solve(Y);
// std::cout << " M = " << M << std::endl;
vector<double> ai, bi, ci, di;
for (int i = 0; i < size - 1; i++)
{
ai.push_back(y[i]);
di.push_back((M(i + 1) - M(i)) / (6 * dx[i]));
// //bi.push_back(dy[i] / dx[i] - dx[i] * (2 * M(i) + M(i + 1)) / 3);
bi.push_back(dy[i] / dx[i] - dx[i] * M(i) / 2.0 - dx[i] *(M(i + 1) - M(i) ) / 6);
ci.push_back(M(i)/2.0);
// di.push_back((M(i + 1) - M(i)) / (3 * dx[i]));
// bi.push_back(dy[i] / dx[i] - dx[i] * (2 * M(i) + M(i + 1)) / 3);
// ci.push_back(M(i));
}
vector<int> x_, xx_;
for (int i = 0; i < size; i++)
{
int temp = x[i] / 0.1;
x_.push_back(temp);
}
for (int i = 0; i < size_xx; i++)
{
int temp = xx[i] / 0.1;
xx_.push_back(temp);
}
for (int i = 0; i < size_xx; i++)
{
int k = 0;
for (int j = 0; j < size - 1; j++)
{
if (xx_[i] >= x_[j] && xx_[i] < x_[j + 1])
{
k = j;
break;
}
else if (xx[i] == x[size - 1])
{
k = size - 1;
}
}
// yy(i) = y[i] + bi(k) * (xx[i] - x[k]) + 1 / 2.0 * M(i) * pow((xx[i] - x[k]) , 2) + di(k) * pow((xx[i] - x[k]),3);
double temp = ai[k] + bi[k] * (xx[i] - x[k]) + ci[k] * pow((xx[i] - x[k]), 2) + di[k] * pow((xx[i] - x[k]), 3);
std::cout << " x = " << xx[i] << " temp = " << temp << std::endl;
yy.push_back(temp);
geometry_msgs::PoseStamped pose;
pose.pose.position.x = xx[i];
pose.pose.position.y = yy[i];
sm_path_.push_back(pose);
}
std::ofstream output;
output.open("Spline.txt");
for (unsigned i = 0; i < size_xx; i++)
{
output << xx[i] << '\t' << yy[i] << std::endl;
}
output.close();
}
