【机器人】无人车-运动规划-高阶多项式曲线方程求解

高阶多项式曲线拟合方程求解

已知参数（2个位置点）

起点状态（3组已知值）

$$(p_0,x_0),(p_0,x_0^{\prime }),(p_0,x_0^{\prime \prime })$$

终点状态（3组已知值）

$$(p_1,x_1),(p_1,x_1^{\prime }),(p_1,x_1^{\prime \prime })$$

n阶多项式曲线（n+1个未知参数）

基本形式

• 输入：$p$
• 输出：$x$
• 系数：$w_0,w_1\dots \dots w_n$
• 方程：$x=w_n{p}^n+w_{n-1}{p}^{n-1}+w_{n-2}{p}^{n-2}+\dots \dots +w_1{p}^1+w_0$

基本特性

• n阶多项式共有n+1个参数，要想求的唯一解需要n+1组已知量才能求得。
• 对于超过5阶的多项式，仅知道两个点的0、1、2阶导数（共6组参数）是无法求的唯一解的。

高阶曲线拟合示意图

3阶多项式曲线（4个未知参数）

需要4组已知值，才能求得唯一解

曲线方程

$$\begin{array}{rl}x& =a{p}^3+b{p}^2+cp+d\end{array}$$

导数方程

$$\begin{array}{rl}{x}^{\prime }& =3a{p}^2+2bp+c\\ x^{\prime \prime }& =6ap+2b\end{array}$$

构建方程组

$$\{\begin{array}{ll}{x}_0& =a{p}_0^3+b{p}_0^2+c{p}_0+d\\ x_0^{\prime }& =3a{p}_0^2+2b{p}_0+c\\ x_0^{\prime \prime }& =6a{p}_0+2b\\ x_1& =a{p}_1^3+b{p}_1^2+c{p}_1+d\\ x_1^{\prime }& =3a{p}_1^2+2b{p}_1+c\\ x_1^{\prime \prime }& =6a{p}_1+2b\end{array}$$

系数求解

• 方程组中有4个未知参数，只需要4个方程即可获得唯一解，
• 此处采用 $(p_0,x_0),(p_0,x_0^{\prime }),(p_0,x_0^{\prime \prime }),(p_1,x_1)$ 4个已知条件进行求解，
• 因此只需要选择1、2、3、4共4子方程构造新的方程组

$$\{\begin{array}{ll}{x}_0& =a{p}_0^3+b{p}_0^2+c{p}_0+d\\ x_0^{\prime }& =3a{p}_0^2+2b{p}_0+c\\ x_0^{\prime \prime }& =6a{p}_0+2b\\ x_1& =a{p}_1^3+b{p}_1^2+c{p}_1+d\end{array}$$
方程组整理可得：
$$\begin{array}{rl}d& =x_0-(a{p}_0^3+b{p}_0^2+c{p}_0)\\ c& =x_0^{\prime }-(3a{p}_0^2+2b{p}_0)\\ b& =\frac{x_0^{\prime \prime }-6a{p}_0}{2}\\ a& =\frac{x_1}{p_1^3}\end{array}$$