分段五次螺旋线非线性优化拟合(基于Apollo)
[TOC]
分段五次螺旋线非线性优化拟合(基于 Apollo)
螺旋线介绍
曲线方程可以表示为以弧长为参数的参数表达式, 好处是在使用弧长参数表达式下的 $C^n$ 连续性转换成任何参数形式仍然能保证 $C^n$ 连续, 同时如果表达为 $\theta(s)$ 即切线的弧长参数表达式可以很方便地求得曲率的 $n$ 阶导.
问题模型建立
如下图
给定 $n$ 个离散点 $P_0(x_0,y_0)$~$P_{n-1}(x_{n-1},y_{n-1})$, 被分为 $n-1$ 段, 每段长度为 $\Delta s_i$.
每段曲线的参数表达式如下:
其中 $s\in[0,\Delta{s_i}]$.
解析求得两点确定的一条曲线(第 $i$ 段)过程如下:
未知参数向量 $\vec{p_i}$ 为 6 行向量: $a_i,b_i,c_i,d_i,e_i,f_i$, 需要 6 个方程求解得出.
为了保证相邻段曲线之间的连续性 $(C^2)$ 连续, 可得如下方程:
$$ \begin{aligned} & x_{i+1}=x_i+\int_{0}^{\Delta s_i}\cos(\theta(s))ds\\ & y_{i+1}=y_i+\int_{0}^{\Delta s_i}\sin(\theta(s))ds\\ & \theta_{i+1}=\theta_i(\Delta{s_i}) \\ & \dot{\theta_{i+1} }=\dot{\theta_i}(\Delta{s_i}) \\ & \ddot{\theta_{i+1} }=\ddot{\theta_i}(\Delta{s_i}) \end{aligned} $$优化模型
优化变量
优化变量设置为 $\vec{q}$ 拟合所需要的结果, 这样不需要再用系数求一遍, 同时如果使用系数作为优化变量的话, 无法直接在约束中添加曲率/曲率变化率的 bounds, 需要构造 constraints 或者是间接添加 bounds. 如下每个元素均为 $n$ 行的向量.
$$ \vec{q} =[\vec{\theta},\vec{\dot{\theta} },\vec{\ddot{\theta} },\vec{x},\vec{y},\vec{\Delta{s} }] $$ 例如 $$ \vec{\Delta{s} }= [\Delta{s}_0 \quad \Delta{s}_1 \quad \cdots \quad \Delta{s}_{n-1}] $$ 其中 $$ \Delta{s}_0=0 $$目标方程
$$ f_{cost}=w_{length} \sum_{i=0}^{n-1} \Delta{s_i} + w_{kppa} \sum_{i=0}^{n-1} \Bigg( \sum_{j=0}^{m-1} (\dot{\theta_{ij} }(s))^2 \Bigg) + w_{dkppa} \sum_{i=0}^{n-1} \Bigg(\sum_{j=0}^{m-1} (\ddot{\theta_{ij} }(s))^2 \Bigg) $$分别代表长度, 曲率, 曲率变化率, $m$ 表示对第 $i$ 段曲线长度等分为 $m$ 子段(分段的原因, 初步理解为减小数值积分误差).
约束条件
包含 bounds(其实也是 constraints, 为了尽可能地方便理解, 专门独立出来讲解) 与 constraints, 分别用 $B$ 与 $C$ 序号表示, 下表带 $i$ 表示共 $n$ 个.
起点等式约束(bounds)
$$ \begin{aligned} & \theta_0 = \theta_{start} \quad \cdots (B0) \\ & \kappa_0=\kappa_{start} \quad \cdots (B1)\\ & \dot{\kappa_0} =\dot{\kappa_{start} } \quad \cdots (B2)\\ & x_0 = x_{ref_0} \quad \cdots (B3)\\ & y_0 = y_{ref_0} \quad \cdots (B4) \end{aligned} $$终点等式约束(bounds)
$$ \begin{aligned} & \theta_{n-1} = \theta_{end} \quad \cdots (B5)\\ & \kappa_{n-1}=\kappa_{end} \quad \cdots (B6)\\ & \dot{\kappa_{n-1} } =\dot{\kappa_{end} } \quad \cdots (B7)\\ & x_{n-1} = x_{ref_{n-1} } \quad \cdots (B8)\\ & y_{n-1} = y_{ref_{n-1} } \quad \cdots (B9) \end{aligned} $$物理模型约束(bounds)
最大转角/转角变化率/转角变化率的变化率( Apollo 中标定值).
$$ \begin{aligned} & \theta_{i-1} - \frac{\pi}{2} \leq \theta_{i} \leq \theta_{i-1} + \frac{\pi}{2} \quad \cdots (B10_i)\\ & -0.25 \leq \kappa \leq +0.25 \quad \cdots (B11_i)\\ & -0.02 \leq \dot{\kappa} \leq +0.02 \quad \cdots (B12_i)\\ \end{aligned} $$中间点范围约束(bounds)
规定连续两个点之间最大转向为四分之一圆弧.
$$ \begin{aligned} & x_{ref_i} -r_i \leq x_i \leq x_{ref_i} +r_i \quad \cdots (B13_i)\\ & y_{ref_i} -r_i \leq y_i \leq y_{ref_i} +r_i \quad \cdots (B14_i)\\ & D_i -2r_i\leq \Delta{s_i} \leq D_i \frac{\pi}{2} \quad \cdots (B15_i) \\ & 其中, D_i = \sqrt{(x_{ref_i}-x_{ref_{i+1} })^2+(y_{ref_i}-y_{ref_{i+1} })^2} \end{aligned} $$连接点等式约束(bounds+constraints)
$$ \begin{aligned} & x_{i+1}=x_i+\int_{0}^{\Delta s_i}\cos(\theta(s))ds \quad \cdots (C0_i)\\ & y_{i+1}=y_i+\int_{0}^{\Delta s_i}\sin(\theta(s))ds \quad \cdots (C1_i)\\ & \theta_{i+1}=\theta_i(\Delta{s_i}) \quad \cdots (B16_i)\\ & \dot{\theta_{i+1} }=\dot{\theta_i}(\Delta{s_i}) \quad \cdots (B17_i)\\ & \ddot{\theta_{i+1} }=\ddot{\theta_i}(\Delta{s_i}) \quad \cdots (B18_i) \end{aligned} $$位置平移非等式约束(constraints)
$$
(x_{i}-x_{ref_{i} })^2+(y_{i}-y_{ref_{i} })^2 \leq r_i^2 \quad \cdots (C2_i)
$$
数值积分求解弧长
连接点等式约束中出现了积分, 其中存在 Fresnel 积分, 求不出解析解, Apollo 中采用 Gauss-Legendre 积分方法. 采用此积分方法的好处是计算效率较高, 只需计算特定几个点处的函数值加权和即可逼近积分值, 并且能保证较满意的精度. Apollo 中采用的是五点高斯积分.
下面为 Gauss-Legendre 积分公式
$$
\int_{-1}^{1}f(x)dx \approx \sum_{k=0}^{n}A_kf(x_k)
$$
下图为 Gauss-Legendre 积分积分点与权重系数表:
具体步骤:
- 需要将自变量区间 $[0,\Delta{s_i}]$ 线性变换为 $t*[-1,1]$.
- 在 $n=4$ 处的 $x_i$ 点处求函数值的权重 $A_i$ 和.
具体过程参考 Apollo 代码:modules/common/math/interal.cc(math 里面有很多小工具值得借鉴一下.)
求解库
Apollo 采用 IPOPT 非线性求解器求解此问题, IPOPT 的使用指南可以参考博文. 只需要将接口所需的参数/表达式传递进去即可. 下面的部分为具体的操作, Apollo 的代码会在后面逐行解析头文件.
起点: Ipopt::TNLP::get_starting_point
把 $\theta_0 = \theta_{start} , \kappa_0=\kappa_{start} , \dot{\kappa_0} =\dot{\kappa_{start} }, x_0 = x_{ref_0} , y_0 = y_{ref_0}$ 传进去.
目标方程: Ipopt::TNLP::eval_f
把 $f_{cost}$ 传进去.
目标方程 Gradient 向量: Ipopt::TNLP::eval_grad_f
这个是整个求解过程中较为复杂的一部分, 此部分求偏导的结果可以用于约束方程的偏导求解. 优化变量为 $6n$ 向量, 因此目标方程的 Gradient 向量也为 $6n$ 向量, 拿任意相邻 2 组 12 行为例, $[\theta_{i},\dot{\theta_{i} },\ddot{\theta_{i} },x_{i},y_{i},\Delta s_{i},\theta_{i+1},\dot{\theta_{i+1} },\ddot{\theta_{i+1} },x_{i+1},y_{i+1},\Delta s_{i+1}]^T$. $f_{cost}$ 中只需考虑求和公式中的第 $i,i+1$ 项即可, 其余项对优化变量的偏导均为 0. 注, 后面有时会省略下标 $i$, 偷懒一下 :-). 同时对 $\Delta s$ 与 $s$ 的求导结果一致.
即:
对 $\vec{x},\vec{y}$ 的偏导为 0.
$$ \frac{\partial {\dot{\theta_i} }^2 / \partial {\dot{\theta_{i+1} }}^2 / \partial {\ddot{\theta_i} }^2 / \partial {\ddot{\theta_{i+1} }}^2 / \partial \Delta s_i} {\partial x_i / \partial y_i / \partial x_{i+1} / \partial y_{i+1} } = \frac{\partial f_{cost(i,i+1)} }{\partial x_i / \partial y_i / \partial x_{i+1} / \partial y_{i+1} } = 0 $$对 $\theta_{i},\dot{\theta_{i} },\ddot{\theta_{i} },\theta_{i+1},\dot{\theta_{i+1} },\ddot{\theta_{i+1} }$ 求偏导可以参考如下例子:
$$ \frac{\partial {\dot{\theta_i(\vec{q})} }^2}{\partial \theta_i} = \\ \sum_{r=0}^{m} 2\dot{\theta_i(s)} * \frac{\partial (a_is^5+b_is^4+c_is^3+d_is^2+e_is+f_i)}{\partial \theta_i} \bigg|_{s=r_j}= \\ \sum_{r=0}^{m} 2\dot{\theta_i(s)} * \frac{\partial [(\frac{-6 \theta_i} { {\Delta s}^5} - \frac{3 \dot{\theta_i} } { {\Delta s}^4} - \frac{ \ddot{\theta_{i} }} {2{\Delta s}^3} + \frac{6 \theta_{i+1} } { {\Delta s}^5} - \frac{3 \dot{\theta_{i+1} }} { {\Delta s}^4} + \frac{\ddot{\theta_{i+1} }} {2{\Delta s}^3})s^5+b_is^4+c_is^3+d_is^2+e_is+f_i]}{\partial \theta_i} \bigg|_{s=r_j} =\\ \sum_{r=0}^{m} 2\dot{\theta_i(r_j)} *(5\frac{-6}{ {\Delta s}^5}r_j^4 + 4\frac{15}{ {\Delta s}^4}r_j^3+3\frac{-10}{ {\Delta s}^3}r_j^2) $$ 其中 $$ r_j=ratio_j * \Delta s \\ ratio_j=j/m $$同理将参数向量 $\vec{q}_i$ 带入可得 $\ddot{\theta_i(\vec{q})}^2$ 的偏导.
$$ \frac{\partial \Delta s}{\partial \theta / \partial \dot{\theta} / \partial \ddot{\theta} } = 0 $$
另外对 $\Delta s_{i},\Delta s_{i+1}$ 求偏导
$$ \frac{\partial \Delta s_i(\vec{q})}{\partial \Delta s_i} = 1 \\ \frac{\partial \dot{\theta_i(\vec{q})}^2}{\partial \Delta s_i} = \\ \sum_{r=0}^{m} 2\dot{\theta_i(s)} * \Bigg[ \frac{\partial [(\frac{-6 \theta_i} { {\Delta s}^5} - \frac{3 \dot{\theta_i} } { {\Delta s}^4} - \frac{ \ddot{\theta_{i} }} {2{\Delta s}^3} + \frac{6 \theta_{i+1} } { {\Delta s}^5} - \frac{3 \dot{\theta_{i+1} }} { {\Delta s}^4} + \frac{\ddot{\theta_{i+1} }} {2{\Delta s}^3})s^5+b_is^4+c_is^3+d_is^2+e_is+f_i]}{\partial \Delta s} + \frac{\partial r_j}{\partial \Delta s} \Bigg] \bigg|_{s=r_j} = \\ \sum_{r=0}^{m} 2\dot{\theta_i(s)} * \bigg[ (\frac{30 \theta_i} { {\Delta s}^6} + \frac{12 \dot{\theta_i} } { {\Delta s}^5} + \frac{ \ddot{3\theta_{i} }} {2{\Delta s}^3} + \frac{-30 \theta_{i+1} } { {\Delta s}^6} + \frac{12 \dot{\theta_{i+1} }} { {\Delta s}^5} + \frac{-3 \ddot{\theta_{i+1} }} {2{\Delta s}^4})s^5 + \frac{ {\partial (b_is^4+c_is^3+d_is^2+e_is+f_i)+ {\partial r_j} }}{\partial \Delta s} \bigg] \bigg|_{s=r_j} $$ 其中 $$ \frac{\partial r_j}{\partial \Delta s} = \frac{\partial \dot{\theta(ratio_j*s)} }{\partial \Delta s} \bigg|_{s=r_j}= ratio_j \frac{\partial \dot{\theta(s)} }{\partial s} \bigg|_{s=r_j}= ratio_j \ddot{\theta(s)}\bigg|_{s=r_j} $$其余处偏导均为 0. 此处对 $\Delta s$ 求偏导出现了加法, 为 Apollo 中计算公式, 笔者认为是不是应该采取乘法.下面的约束方程对 $\Delta s$ 求偏导也是如此.
边界: Ipopt::TNLP::get_bounds_info
共 $9n+10$ 个
起终点边界: B0-B9,10 个.
物理模型约束边界: B10-B12,3n 个.
中间点范围边界: B13-B15,3n 个.
连接点等式约束: B16-B18,3n 个.
约束方程: Ipopt::TNLP::eval_g
连接点等式约束:
$$ g_{1i}(\vec{p}) = \bigg ( x_{i+1} - x_i - \int_{0}^{\Delta s_i}\cos(\theta(s))ds \bigg) ^2 = 0 \\ g_{2i}(\vec{p}) = \bigg ( y_{i+1} - y_i- \int_{0}^{\Delta s_i}\sin(\theta(s))ds \bigg) ^2 =0 $$ 位置平移非等式约束: $$ g_{3i}(\vec{p}) = (x_{i}-x_{ref_{i} })^2+(y_{i}-y_{ref_{i} })^2 - r_i^2 \leq 0 $$共 $3n$ 个.
约束方程: Ipopt::TNLP::eval_jac_g
令 $x_{r_j} = \bigg (x_{i+1} - x_i - \int_{0}^{\Delta s_i}\cos(\theta(s))ds\bigg) ^2 \bigg |_{s=r_j}$
将积分空间 $[0,\Delta s_i]$ 转换到 $[-1,1]$, 自变量变为 $(\frac{\Delta s_i}{2}r+\frac{\Delta s_i}{2})$, 以便进行数值积分求解.
对于 $g_{1i}$:
$$
\frac{\partial g_{1i} }{\partial \theta_i} =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \theta_i}) \bigg |_{s=r_j} \\
\frac{\partial g_{1i} }{\partial \dot{\theta_i} } =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \dot{\theta_i} }) \bigg |_{s=r_j}\\
\frac{\partial g_{1i} }{\partial \dot{\theta_i} } =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \ddot{\theta_i} }) \bigg |_{s=r_j}\\
\frac{\partial g_{1i} }{\partial \theta_{i+1} } =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \theta_{i+1} }) \bigg |_{s=r_j}\\
\frac{\partial g_{1i} }{\partial \dot{\theta_{i+1} }} =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \dot{\theta_{i+1} }}) \bigg |_{s=r_j}\\
\frac{\partial g_{1i} }{\partial \dot{\theta_{i+1} }} =
-2x_{r_j}
\frac{\Delta s_i}{2}
(\int_{-1}^{1}\sin \theta \frac{\partial {\theta_i(\vec{q})} }{\partial \ddot{\theta_{i+1} }}) \bigg |_{s=r_j} \\
\frac{\partial g_{1i} }{\partial x_i} = -2x_{r_j}*(-1) \\
\frac{\partial g_{1i} }{\partial x_{i+1} } = -2x_{r_j}*(1) \\
\frac{\partial g_{1i} }{\partial y_i} = 0 \\
\frac{\partial g_{1i} }{\partial y_{i+1} } = 0 \\
\frac{\partial g_{1i} }{\partial \Delta s_i} =
\frac{\partial g_{1i} }{\partial \theta_i}+(-2x_{r_j}) \frac{1}{2} \int_{-1}^{1}\cos(\theta)ds \bigg |_{s=r_j}\\
\frac{\partial g_{1i} }{\partial \Delta s_{i+1} } =0
$$
相同地可以得到 $g_{2i}$ 的偏导.
对 $g_{3i}$ 仅在 $x_i$ 与 $y_i$ 处有偏导.
$$
\frac{\partial g_{3i} }{\partial x_i} = 2(x_{i}-x_{ref_{i} }) \\
\frac{\partial g_{3i} }{\partial y_i} = 2(y_{i}-y_{ref_{i} })
$$
目标方程 Hessian 矩阵: Ipopt::TNLP::eval_h
Hessain 矩阵计算公式如下:
$$ \mathbf{H} = \sigma_f \nabla^2 f(\vec{p}) + \sum_{i=1}^{3n}\lambda_i\nabla^2 g_i(\vec{p}) $$ Apollo 中将 Hessian 矩阵设置为 0, 实际理论上不为零,下面举出 2 个例子: - 例1: $$ \frac{\partial^2 {\dot{\theta_i(\vec{q})} }}{\partial \theta_i \partial \Delta s_i} = \sum_{r=0}^{m} \frac{ \partial (5\frac{-6}{ {\Delta s}^5}r_j^4 + 4\frac{15}{ {\Delta s}^4}r_j^3+3\frac{-10}{ {\Delta s}^3}r_j^2)}{\partial \Delta s} = \\ \sum_{r=0}^{m} (\frac{150}{ {\Delta s}^6}r_j^4 + \frac{-240}{ {\Delta s}^5}r_j^3+\frac{90}{ {\Delta s}^4}r_j^2) $$ - 例2: $$ \frac{\partial^2 g_{3i} }{\partial^2 x_i} = 2 \\ \frac{\partial^2 g_{3i} }{\partial^2 y_i} = 2 $$设置为 0 可能是考虑到求解性.
Apollo(v6.0)源码解析
对所有相关代码逐行解析后, 主要类的 .h 文件分析如下
curve1d基类, 纯虚类1
2
3
4
5
6
7
8
9
10
11
12
13
14/*FILE:planning/math/curve1d/curve1d.h */
class Curve1d {
public:
Curve1d() = default;
virtual ~Curve1d() = defaul
virtual double Evaluate(const std::uint32_t order,
const double param) const = 0;
virtual double ParamLength() const = 0;
virtual std::string ToString() const = 0;
};polynomial_curve1d, 继承 1 级类, 纯虚类1
2
3
4
5
6
7
8
9
10
11
12/*FILE:planning/math/curve1d/polynomial_curve1d.h*/
class PolynomialCurve1d : public Curve1d {
public:
PolynomialCurve1d() = default;
virtual ~PolynomialCurve1d() = default;
virtual double Coef(const size_t order) const = 0;
virtual size_t Order() const = 0;
protected:
double param_ = 0.0;
};quintic_polynomial_curve1d, 继承 2 级类, 五次多项式曲线1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48/* FILE:planning/math/curve1d/quintic_polynomial_curve1d.h*/
// 1D quintic polynomial curve:
// (x0, dx0, ddx0) -- [0, param] --> (x1, dx1, ddx1)
//param_为曲线长度
//吐槽长度为啥不用length/l这种一看就理解的命名,非要用param_/p/delta_s乱七八糟的不统一命名
class QuinticPolynomialCurve1d : public PolynomialCurve1d {
public:
QuinticPolynomialCurve1d() = default;
QuinticPolynomialCurve1d(const std::array<double, 3>& start,
const std::array<double, 3>& end,
const double param);
QuinticPolynomialCurve1d(const double x0, const double dx0, const double ddx0,
const double x1, const double dx1, const double ddx1,
const double param);
//拷贝构造函数,复制另一个类的param_与coef
QuinticPolynomialCurve1d(const QuinticPolynomialCurve1d& other);
//根据首末点的信息求解五次多项式系数,使用公式解析(调用protected成员函数ComputeCoefficients)
void SetParam(const double x0, const double dx0, const double ddx0,
const double x1, const double dx1, const double ddx1,
const double param);
//从四次多项式积分到五次多项式
void IntegratedFromQuarticCurve(const PolynomialCurve1d& other,
const double init_value);
virtual ~QuinticPolynomialCurve1d() = default;
//求曲线在p处第order阶的导数
double Evaluate(const std::uint32_t order, const double p) const override;
double ParamLength() const override { return param_; }
//输出五次多项式spec字符串
std::string ToString() const override;
//get coef中某一个元素
double Coef(const size_t order) const override;
size_t Order() const override { return 5; }
protected:
void ComputeCoefficients(const double x0, const double dx0, const double ddx0,
const double x1, const double dx1, const double ddx1,
const double param);
// f = sum(coef_[i] * x^i), i from 0 to 5
std::array<double, 6> coef_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0} };
std::array<double, 3> start_condition_{{0.0, 0.0, 0.0} };
std::array<double, 3> end_condition_{{0.0, 0.0, 0.0} };
};quintic_spiral_path, 继承 3 级类, 五次螺旋线1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94/* FILE:planning/math/curve1d/quintic_polynomial_curve1d.h*/
/**
* Describe a quintic spiral path
* Map (theta0, kappa0, dkappa0) ----- delta_s -----> (theta1, kappa1, dkappa1)
*/
class QuinticSpiralPath : public QuinticPolynomialCurve1d {
public:
QuinticSpiralPath() = default;
//构造函数,调用第二个构造函数构造类
QuinticSpiralPath(const std::array<double, 3>& start,
const std::array<double, 3>& end, const double delta_s);
//根据始终点与曲线长度计算系数,通过在参数列表中调用父类的构造函数
//QuinticPolynomialCurve1d(x0, dx0, ddx0, x1, dx1, ddx1, p)把delta_s参数传给param_
QuinticSpiralPath(const double theta0, const double kappa0,
const double dkappa0, const double theta1,
const double kappa1, const double dkappa1,
const double delta_s);
//通过lambda函数计算s处的cos(theta)然后使用IntegrateByGaussLegendre函数计算0-s处的积分(曲线长度)得x坐标
template <size_t N>
double ComputeCartesianDeviationX(const double s) const {
auto cos_theta = [this](const double s) {
const auto a = Evaluate(0, s);
return std::cos(a);
};
return common::math::IntegrateByGaussLegendre<N>(cos_theta, 0.0, s);
}
//同x坐标获取y坐标
template <size_t N>
double ComputeCartesianDeviationY(const double s) const {
auto sin_theta = [this](const double s) {
const auto a = Evaluate(0, s);
return std::sin(a);
};
return common::math::IntegrateByGaussLegendre<N>(sin_theta, 0.0, s);
}
//计算对\Delta s_i的偏导
template <size_t N>
std::pair<double, double> DeriveCartesianDeviation(
const size_t param_index) const {
auto gauss_points = common::math::GetGaussLegendrePoints<N>();
std::array<double, N> x = gauss_points.first;
std::array<double, N> w = gauss_points.second;
std::pair<double, double> cartesian_deviation = {0.0, 0.0};
for (size_t i = 0; i < N; ++i) {
double r = 0.5 * x[i] + 0.5;
auto curr_theta = Evaluate(0, r * param_);
double derived_theta = DeriveTheta(param_index, r);
cartesian_deviation.first +=
w[i] * (-std::sin(curr_theta)) * derived_theta;
cartesian_deviation.second += w[i] * std::cos(curr_theta) * derived_theta;
}
cartesian_deviation.first *= param_ * 0.5;
cartesian_deviation.second *= param_ * 0.5;
if (param_index == DELTA_S) {
for (size_t i = 0; i < N; ++i) {
double r = 0.5 * x[i] + 0.5;
auto theta_angle = Evaluate(0, r * param_);
cartesian_deviation.first += 0.5 * w[i] * std::cos(theta_angle);
cartesian_deviation.second += 0.5 * w[i] * std::sin(theta_angle);
}
}
return cartesian_deviation;
}
//利用DeriveTheta得到的Theta值计算1-3阶导
double DeriveKappaDerivative(const size_t param_index,
const double ratio) const;
double DeriveDKappaDerivative(const size_t param_index,
const double ratio) const;
double DeriveD2KappaDerivative(const size_t param_index,
const double r) const;
//分别对应优化变量里的theta_i,kappa_i,dkappa_i,theta_i_1,kappa_i_1,dkappa_i_1,delta_s_i,方便对其们求偏导
static const size_t THETA0 = 0;
static const size_t KAPPA0 = 1;
static const size_t DKAPPA0 = 2;
static const size_t THETA1 = 3;
static const size_t KAPPA1 = 4;
static const size_t DKAPPA1 = 5;
static const size_t DELTA_S = 6;
private:
//计算在delta_s_ratio处的theta,param_index为上面的static const值,作用未知
double DeriveTheta(const size_t param_index,
const double delta_s_ratio) const;
//6行7列系数矩阵,每行分别为a,b,c,d,e,f算式各项,每列为算式各项,但是第七列[6]作用未知
std::array<std::array<double, 7>, 6> coef_deriv_;
};QuinticSpiralPathWithDerivation, 继承 3 级类, 带导数信息的五次螺旋线
与quintic_spiral_path的区别是用 unordered_map 存储了积分点处的cache_cartesian_deriv_,cache_kappa_deriv_,cache_dkappa_deriv_, 并且仅有头文件.1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83template <size_t N>
class QuinticSpiralPathWithDerivation : public QuinticPolynomialCurve1d {
public:
QuinticSpiralPathWithDerivation() = default;
QuinticSpiralPathWithDerivation(const std::array<double, 3>& start,
const std::array<double, 3>& end,
const double delta_s);
QuinticSpiralPathWithDerivation(const double theta0, const double kappa0,
const double dkappa0, const double theta1,
const double kappa1, const double dkappa1,
const double delta_s);
virtual ~QuinticSpiralPathWithDerivation() = default;
//使用key = order * 100 + n * 10 + i来查询cache_evaluate_的值
double evaluate(const size_t order, const size_t i, const size_t n);
//计算到Delta_s处的X,Y积分
double ComputeCartesianDeviationX() const { return dx_; }
double ComputeCartesianDeviationY() const { return dy_; }
//计算X,Y对s的微分,存储到cache_cartesian_deriv_,未被使用
std::pair<double, double> DeriveCartesianDeviation(const size_t param_index);
//计算第n段的第i小段处的偏导值,切割成5段
double DeriveKappaDerivative(const size_t param_index, const int i,
const int n);
double DeriveDKappaDerivative(const size_t param_index, const int i,
const int n);
//优化变量,求偏导时使用
static const size_t THETA0 = 0;
static const size_t KAPPA0 = 1;
static const size_t DKAPPA0 =
static const size_t THETA1 = 3;
static const size_t KAPPA1 = 4;
static const size_t DKAPPA1 = 5;
static const size_t DELTA_S = 6;
static const size_t INDEX_MAX = 7;
//计算0-s的x积分值
double ComputeCartesianDeviationX(const double s) const {
auto cos_theta = [this](const double s) {
const auto a = Evaluate(0, s);
return std::cos(a);
};
return common::math::IntegrateByGaussLegendre<N>(cos_theta, 0.0, s);
}
//计算0-s的y积分值
double ComputeCartesianDeviationY(const double s) const {
auto sin_theta = [this](const double s) {
const auto a = Evaluate(0, s);
return std::sin(a);
};
return common::math::IntegrateByGaussLegendre<N>(sin_theta, 0.0, s);
}
private:
//计算cos(theta)的值
double DeriveCosTheta(const size_t param_index, const double r) const;
//计算sin(theta)的值
double DeriveSinTheta(const size_t param_index, const double r) const;
//计算theta对r的param_inde阶导
double DeriveThetaDerivative(const size_t param_index, const double r) const;
//用于计算三个散列表
std::array<std::array<double, 7>, 6> coef_deriv_;
//此数据未被赋值
std::unordered_map<size_t, double> cache_evaluate_;
//未被使用
std::unordered_map<size_t, std::pair<double, double>> cache_cartesian_deriv_;
//下面两个散列表分别存储: ,所有点处的kappa,dkappa
//点: n段曲线按长度平均分成i小段
//散列函数为: auto key = param_index * INDEX_MAX + i;
std::unordered_map<size_t, double> cache_kappa_deriv_;
std::unordered_map<size_t, double> cache_dkappa_deriv_;
double dx_ = 0.0;
double dy_ = 0.0;
//根据N得到Gauss–Legendre积分点与权重系数,计算积分
std::array<double, N> gauss_points_;
std::array<double, N> gauss_point_weights_;
};SpiralProblemInterface, 继承Ipopt::TNLP, 使用 IPOPT 的接口1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138class SpiralProblemInterface : public Ipopt::TNLP {
public:
//仅一个构造函数,初始化内容见成员变量说明
explicit SpiralProblemInterface(std::vector<Eigen::Vector2d> points);
virtual ~SpiralProblemInterface() = default
void set_default_max_point_deviation(const double point_max_deviation);
void set_start_point(const double x, const double y, const double theta,
const double kappa, const double dkappa);
void set_end_point(const double x, const double y, const double theta,
const double kappa, const double dkappa);
void set_end_point_position(const double x, const double y);
void set_element_weight_curve_length(const double weight_curve_length);
void set_element_weight_kappa(const double weight_kappa);
void set_element_weight_dkappa(const double weight_dkappa);
//获取优化后的结果,vector指针指向vector
void get_optimization_results(std::vector<double>* ptr_theta,
std::vector<double>* ptr_kappa,
std::vector<double>* ptr_dkappa,
std::vector<double>* ptr_s,
std::vector<double>* ptr_x,
std::vector<double>* ptr_y)
//以下成员函数为继承IPOPT接口
//n=num_of_points_ * 5 + num_of_points_ - 1
//注:此处-1是因为Delta_theta_0=0
//m = (num_of_points_ - 1) * 2+num_of_points_;
//nnz_jac_g = (num_of_points_ - 1) * 2 * 9 + num_of_points_ * 2;
//nnz_h_lag = 0;
/** Method to return some info about the nlp */
bool get_nlp_info(int& n, int& m, int& nnz_jac_g, int& nnz_h_lag,
IndexStyleEnum& index_style) override;
//分别针对has_fixed_start_point_,has_fixed_end_point_,has_fixed_end_point_position_的情况下添加始终点约束以及中间点bound,顺序: theta,kappa,dkappa,x,y,delta_s,
//接下来是约束: positional equality constraints,positional deviation constraints
/** Method to return the bounds for my problem */
bool get_bounds_info(int n, double* x_l, double* x_u, int m, double* g_l,
double* g_u) override;
//初始值: 每个输入点处的theta,kappa,dkappa,x,y,共5*num_of_points_个
//其中kappa为: Delta_theta_i/(D_i/cos(Delta_theta_i/2)),第一个点为fix_start_point或者是第二个点 处的kappa
//注: 直接使用欧式距离作为\Delta s误差太大肯定小于实际值,因此取了稍大一些的近似值,希望减小误差.
/** Method to return the starting point for the algorithm */
bool get_starting_point(int n, bool init_x, double* x, bool init_z,
double* z_L, double* z_U, int m, bool init_lambda,
double* lambda) override;
/** Method to return the objective value*/
//计算目标函数,其中曲线长度通过spiral_curve.ParamLength()函数获取, kappa = spiral_curve.Evaluate(1, s),dkappa = spiral_curve.Evaluate(2, s)获取
bool eval_f(int n, const double* x, bool new_x, double& obj_value) override;
/** Method to return the gradient of the objective */
bool eval_grad_f(int n, const double* x, bool new_x, double* grad_f) override;
/** Method to return the constraint residuals
bool eval_g(int n, const double* x, bool new_x, int m, double* g) override;
/** Method to return:
* 1) The structure of the jacobian (if "values" is nullptr)
* 2) The values of the jacobian (if "values" is not nullptr)
*/
bool eval_jac_g(int n, const double* x, bool new_x, int m, int nele_jac,
int* iRow, int* jCol, double* values) override;
/** Method to return:
* 1) The structure of the hessian of the lagrangian (if "values" is
* nullptr) 2) The values of the hessian of the lagrangian (if "values" is not
* nullptr)
*/
bool eval_h(int n, const double* x, bool new_x, double obj_factor, int m,
const double* lambda, bool new_lambda, int nele_hess, int* iRow,
int* jCol, double* values) override;
/** @name Solution Methods */
/** This method is called when the algorithm is complete so the TNLP can
* store/write the solution */
void finalize_solution(Ipopt::SolverReturn status, int n, const double* x,
const double* z_L, const double* z_U, int m,
const double* g, const double* lambda,
double obj_value, const Ipopt::IpoptData* ip_data,
Ipopt::IpoptCalculatedQuantities* ip_cq) override;
public:
static constexpr size_t N = 10;
private:
//根据输入的x数组与n问题维度update每段螺旋线(vector容器),使用std::move()提高vector =操作符号的效率.
void update_piecewise_spiral_paths(const double* x, const int n);
//输入拟合离散点列,通过移动构造函数初始化
std::vector<Eigen::Vector2d> init_points_;
//以下opt_开头的变量为优化后的各项优化变量结果
std::vector<double> opt_theta_;
std::vector<double> opt_kappa_;
std::vector<double> opt_dkappa_;
std::vector<double> opt_s_;
std::vector<double> opt_x_;
std::vector<double> opt_y_;
//输入点间距,使用Eigen的.norm()函数计算
std::vector<double> ;
//以下为各种个数定义
int num_of_variables_ = 0;
int num_of_constraints_ = 0;
//输入点个数static_cast<int>(init_points_.size()
int num_of_points_ = 0;
//x,y坐标的最大偏移边界约束
double default_max_point_deviation_ = 0.0;
//每段曲线分段个数即m
const int num_of_internal_points_ = 5;
//将输入点间的theta过滤为·[-2PI,2PI]之间,最后一个值与倒数第二值一样,构造函数里初始化
std::vector<double> relative_theta_;
//分段段数,个数=输入点数-1
std::vector<QuinticSpiralPathWithDerivation<N>> piecewise_paths_;
//以下成员变量为始终点的约束
bool has_fixed_start_point_ = false;
double start_x_ = 0.0;
double start_y_ = 0.0;
double start_theta_ = 0.0;
double start_kappa_ = 0.0;
double start_dkappa_ = 0.0;
bool has_fixed_end_point_ = false;
double end_x_ = 0.0;
double end_y_ = 0.0;
double end_theta_ = 0.0;
double end_kappa_ = 0.0;
double end_dkappa_ = 0.0;
bool has_fixed_end_point_position_ = false;
//目标函数的权重系数
double weight_curve_length_ = 1.0;
double weight_kappa_ = 1.0;
double weight_dkappa_ = 1.0;
};PiecewiseQuinticSpiralPath, 继承 1 级类
把多段定义成 path, 并定义操作, 实际未被使用.1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32//FILE:modules/planning/math/curve1d/piecewise_quintic_spiral_path.h
class PiecewiseQuinticSpiralPath : public Curve1d {
publi
//构造函数,初始化起点的theta/kappa/dkappa/accumulated_s_
PiecewiseQuinticSpiralPath(const double theta, const double kappa,
const double dkappa);
virtual ~PiecewiseQuinticSpiralPath() = default;
//尾部添加曲线段
void Append(const double theta, const double kappa, const double dkappa,
const double delta_s);
//计算长度为param处的order阶导数值(先用LocatePiece定位段数),并且使用了子类中重载的Evaluate函数
double Evaluate(const std::uint32_t order, const double param) const override;
//计算长度为s处的kappa(先用LocatePiece定位段数)
double DeriveKappaS(const double s) const;
//曲线总长度
double ParamLength() const override;
std::string ToString() const override { return "PiecewiseQuinticSpiralPath"; }
private:
//定位长度为param时为第几段曲线
size_t LocatePiece(const double param) const;
//曲线段容器
std::vector<QuinticSpiralPath> pieces_;
//累积长度
std::vector<double> accumulated_s_;
//最后一个点的值
double last_theta_ = 0.0;
double last_kappa_ = 0.0;
double last_dkappa_ = 0.0;
};
观码感触
- 抽象层次清晰合理:1d曲线->1d多项式曲线->1d五次多项式曲线->五次多项式螺旋.
- 通过继承父类的构造函数构造子类的同时实现多态构造子类构造函数,
QuinticSpiralPathWithDerivation类继承QuinticPolynomialCurve1d类的构造函数. - 类内构造函数对另一个构造函数的调用.
- 对于计算重复使用的项, 用散列表存储, 提升效率, 例如
cache_kappa_deriv_, 散列函数的设计是使用 $n$( 最大类型个数)进制. - 把很多矩阵运算改成了方程式运算, 虽然难懂了一些, 官方称效率能提升.
- 使用 lambda 函数计算 cos 值, 简洁.
- 将多阶的求导/取值/积分利用矩阵存储, 简洁.
- 吐槽: 长度为啥不用 length/l 这种一看就理解的命名, 非要用param_/p/delta_s 乱七八糟的不统一命名.
- 吐槽: theta0, kappa0, dkappa0, theta1, kappa1, dkappa1 的命名刚开始很难懂, 如果命名为 theta_i<->theta_i_1 可能会跟公式对的更整齐一些.
注:
- 《Reactive Nonholonomic Trajectory Generation via Parametric Optimal Control》此论文中求解的问题Two-
Point-Boundary-Value problem (TPBVP) , 优化变量为多项式系数, 求解过程很详细值得参考.
分段五次螺旋线非线性优化拟合(基于Apollo)
https://www.chuxin911.com/piecewise_cubic_spiral_curve_20210923/