前言
移动机器人有一个非常重要的任务,轨迹规划。轨迹规划需要满足运动学原理,即在路径规划给出路点后,必须把路点平滑成光滑的轨迹,才能让机器人循迹移动。
平滑的方法可以采用数值计算中的插值法或者拟合法。
常用的插值方法可分为多项式插值法和分段插值法,还有一种三角插值法,适用于具有周期的函数插值,但是比较少见。
什么是插值
插值:假设某个表达式未知的函数
f
(
x
)
f(x)
f(x),知道它在某些点的取值,求一个有具体表达式的函数
P
(
x
)
P(x)
P(x),使得
P
(
x
)
P(x)
P(x)在这些点上的取值与
f
(
x
)
f(x)
f(x)相同:
u
n
k
n
o
w
n
:
f
(
x
)
,
x
∈
[
a
,
b
]
k
n
o
w
n
:
y
1
=
f
(
x
1
)
,
y
2
=
f
(
x
2
)
,
…
,
y
n
=
f
(
x
n
)
a
≤
x
1
<
x
2
<
⋯
<
x
n
≤
b
f
i
n
d
:
P
(
x
)
,
x
∈
[
a
.
b
]
s
.
t
.
y
1
=
P
(
x
1
)
,
y
2
=
P
(
x
2
)
,
…
,
y
n
=
P
(
x
n
)
unknown: \quad f(x),x\in[a,b] \\ \quad \\ known: \quad y_1=f(x_1),y_2=f(x_2),\dots,y_n=f(x_n) \\ \\ \quad \\ a\le x_1<x_2<\dots < x_n\le b \\ \quad \\ find: \quad P(x),x\in [a.b] \\ \quad \\ s. t. \quad y_1=P(x_1),y_2=P(x_2),\dots,y_n=P(x_n)
unknown:f(x),x∈[a,b]known:y1=f(x1),y2=f(x2),…,yn=f(xn)a≤x1<x2<⋯<xn≤bfind:P(x),x∈[a.b]s.t.y1=P(x1),y2=P(x2),…,yn=P(xn)
多项式插值法
使用多阶多项式来对函数进行插值,多项式的阶数可以由需要插值的节点数来确定,一般形式为:
P
(
x
)
=
a
0
+
a
1
x
+
a
2
x
2
+
⋯
+
a
n
x
n
(
1
)
P(x)=a_0+a_1x+a_2x^2+\dots+a_nx^n \quad(1)
P(x)=a0+a1x+a2x2+⋯+anxn(1)
我们的目标就是通过n+1个节点求出多项式的系数。
多项式插值原理:n阶多项式能够对n+1个不同节点进行插值,并且插值结果是唯一的。这个结论将放在多项式插值的总结里证明。
常用的多项式插值法有拉格朗日插值法和牛顿插值法,也可以通过公式(1)与节点值直接求解线性方程组,但是这么做的计算量很大。
拉格朗日插值法
观察公式(1),发现多项式
P
(
x
)
P(x)
P(x)其实可以表示为n+1个同类多项式的和:
P
(
x
)
=
a
0
+
a
1
x
+
a
2
x
2
+
⋯
+
a
n
x
n
P
0
(
x
)
=
a
00
+
a
10
x
+
a
20
x
2
+
⋯
+
a
n
0
x
n
P
1
(
x
)
=
a
01
+
a
11
x
+
a
21
x
2
+
⋯
+
a
n
1
x
n
…
P
n
(
x
)
=
a
0
n
+
a
1
n
x
+
a
2
n
x
2
+
⋯
+
a
n
n
x
n
P
=
P
1
+
P
2
+
⋯
+
P
n
=
∑
i
=
0
n
P
i
(
x
)
P(x)=a_0+a_1x+a_2x^2+\dots+a_nx^n \\ \quad \\ P_0(x)=a_{00}+a_{10}x+a_{20}x^2+\dots+a_{n0}x^n \\ P_1(x)=a_{01}+a_{11}x+a_{21}x^2+\dots+a_{n1}x^n \\ \dots \\ P_n(x)=a_{0n}+a_{1n}x+a_{2n}x^2+\dots+a_{nn}x^n \\ \quad \\ P=P_1+P_2+\dots+P_n=\sum_{i=0}^n P_i(x)
P(x)=a0+a1x+a2x2+⋯+anxnP0(x)=a00+a10x+a20x2+⋯+an0xnP1(x)=a01+a11x+a21x2+⋯+an1xn…Pn(x)=a0n+a1nx+a2nx2+⋯+annxnP=P1+P2+⋯+Pn=i=0∑nPi(x)
假定
P
i
(
x
i
)
=
f
(
x
i
)
,
P
i
(
x
j
)
=
0
,
i
≠
j
P_i(x_i)=f(x_i),P_i(x_j)=0,i\ne j
Pi(xi)=f(xi),Pi(xj)=0,i=j,则有:
P
(
x
0
)
=
P
1
(
x
0
)
+
P
2
(
x
0
)
+
⋯
+
P
n
(
x
0
)
=
f
(
x
0
)
P
(
x
1
)
=
P
1
(
x
1
)
+
P
2
(
x
1
)
+
⋯
+
P
n
(
x
1
)
=
f
(
x
1
)
…
P
(
x
n
)
=
P
1
(
x
n
)
+
P
2
(
x
n
)
+
⋯
+
P
n
(
x
n
)
=
f
(
x
n
)
P(x_0)=P_1(x_0)+P_2(x_0)+\dots+P_n(x_0) = f(x_0) \\ P(x_1)=P_1(x_1)+P_2(x_1)+\dots+P_n(x_1) = f(x_1) \\ \dots \\ P(x_n)=P_1(x_n)+P_2(x_n)+\dots+P_n(x_n) = f(x_n) \\
P(x0)=P1(x0)+P2(x0)+⋯+Pn(x0)=f(x0)P(x1)=P1(x1)+P2(x1)+⋯+Pn(x1)=f(x1)…P(xn)=P1(xn)+P2(xn)+⋯+Pn(xn)=f(xn)
于是多项式
P
(
x
)
P(x)
P(x)就可以拆分为满足
P
i
(
x
i
)
=
f
(
x
i
)
,
P
i
(
x
j
)
=
0
,
i
≠
j
P_i(x_i)=f(x_i),P_i(x_j)=0,i\ne j
Pi(xi)=f(xi),Pi(xj)=0,i=j的n+1个多项式之和,即:
P
i
(
x
)
=
f
(
x
i
)
p
i
(
x
)
p
i
(
x
)
=
{
1
,
x
=
x
i
0
,
x
≠
x
i
P_i(x)=f(x_i)p_i(x) \\ \quad \\ p_i(x)= \begin{cases} 1, & x=x_i \\ 0, & x\ne x_i \\ \end{cases}
Pi(x)=f(xi)pi(x)pi(x)={1,0,x=xix=xi
那么问题就转化为了构造n+1个
p
i
(
x
)
p_i(x)
pi(x),一种构造方法是:
p
i
(
x
)
=
(
x
−
x
0
)
(
x
−
x
1
)
…
(
x
−
x
i
−
1
)
(
x
−
x
i
+
1
)
…
(
x
−
x
n
)
(
x
i
−
x
0
)
(
x
i
−
x
1
)
…
(
x
i
−
x
i
−
1
)
(
x
i
−
x
i
+
1
)
…
(
x
i
−
x
n
)
=
∏
i
≠
j
0
≤
j
≤
n
x
−
x
j
x
i
−
x
j
p_i(x)=\frac {(x-x_0)(x-x_1)\dots(x-x_{i-1})(x-x_{i+1})\dots(x-x_n)}{(x_i-x_0)(x_i-x_1)\dots(x_i-x_{i-1})(x_i-x_{i+1})\dots(x_i-x_n)} \\ \quad \\ = \prod_{i\ne j}^{0\le j \le n} \frac{x-x_j}{x_i-x_j}
pi(x)=(xi−x0)(xi−x1)…(xi−xi−1)(xi−xi+1)…(xi−xn)(x−x0)(x−x1)…(x−xi−1)(x−xi+1)…(x−xn)=i=j∏0≤j≤nxi−xjx−xj
上面这个
p
i
(
x
)
p_i(x)
pi(x)就称为拉格朗日插值的基函数。
最终,拉格朗日插值的结果就可以写成:
P
(
x
)
=
∑
i
=
0
n
P
i
(
x
)
=
∑
i
=
0
n
f
(
x
i
)
p
i
(
x
)
=
∑
i
=
0
n
(
f
(
x
i
)
∏
i
≠
j
0
≤
j
≤
n
x
−
x
j
x
i
−
x
j
)
P(x)=\sum_{i=0}^n P_i(x) \\ \quad \\ = \sum_{i=0}^n f(x_i)p_i(x) \\ \quad \\ = \sum_{i=0}^n (f(x_i)\prod_{i\ne j}^{0\le j \le n} \frac{x-x_j}{x_i-x_j})
P(x)=i=0∑nPi(x)=i=0∑nf(xi)pi(x)=i=0∑n(f(xi)i=j∏0≤j≤nxi−xjx−xj)
总结
拉格朗日插值法的思路实际上是将插值多项式看作是多个多项式的组合,每个多项式在某采样点上取节点值,在其它采样点上取0值。
当采样点增加时,拉格朗日插值法需要重新计算多项式,因此计算量也比较大。(但是比直接使用方程组求解要快的多)