├── .gitignore ├── CMakeLists.txt ├── README.md ├── pic ├── Runge_Phenomenon.png ├── a_single_track_vehicle_model.png ├── coodination.png ├── coordination_setting.png ├── input_curve_points.jpg ├── input_curve_points_based_on_axis_interval.jpg ├── position_constraint.jpg ├── result_y_arc_length_1.jpg ├── result_y_arc_length_2.jpg ├── result_y_arc_length_3.jpg ├── result_y_axis_interval_1.jpg ├── result_y_axis_interval_2.jpg ├── result_y_axis_interval_3.jpg └── smoothness_constraint.jpg └── src ├── QPSolution.csv ├── input_data.csv ├── input_data_generation_and_result_plot.m └── main.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.1) 2 | 3 | set (CMAKE_CXX_STANDARD 11) 4 | 5 | project(Spline_generation) 6 | 7 | find_package(OsqpEigen) 8 | find_package(Eigen3) 9 | 10 | include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR}) 11 | 12 | #MPCExample 13 | add_executable(main src/main.cpp) 14 | target_link_libraries(main OsqpEigen::OsqpEigen) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # path planning-5次Spline样条曲线光滑算法Demo 2 | 3 | ## 0.基础知识 4 | 5 | ### 0.1.[非参数化曲线缺点](https://blog.csdn.net/Jurbo/article/details/75046766) 6 | 1. 与坐标轴有关. 7 | 2. 用显式函数表示会存在多值性. 8 | 3. 用隐式函数表示不直观,作图不方便. 9 | 4. 会出现斜率为无穷大的情形.F 10 | 11 | ### 0.2.五次样条曲线(quintic spline)的应用: 12 | 1. 五次的原因:车辆输入为加速度a(油门),a的加速度jerk对应于a的平滑性,正对应位置的五次微分. 13 | 2. 样条曲线的优点:相对而言较为简单,待补充. 14 | 3. 样条曲线的缺点:容易出现[龙格现象](https://zh.wikipedia.org/wiki/%E9%BE%99%E6%A0%BC%E7%8E%B0%E8%B1%A1),如下图![红色曲线是龙格函数,蓝色曲线是 5 阶多项式,绿色曲线是 9 阶多项式.随着阶次的增加,误差逐渐变大](./pic/Runge_Phenomenon.png) 15 | 4. 分段求解的原因:对于比较极端的曲线(例:直角弯,U型弯)一条多项式曲线已经不足以精确描述曲线的形状. 16 | 5. 输入:为多项式曲线的采样点(基于自车坐标系,见下图的右手系)$x=c_0+c_1y+c_2y^2+c_3y_3^3$,即,$x=\vec{C}T(y)$,$T(s)$为对$s$的多项式矩阵(mobileye的车道线输入为三次多项式).对于离散点输入,Apollo在相同位置有类似的算法,后面有机会可以介绍. 17 | 6. 输入曲线采样: 18 | 参照百度Apollo里的代码可知,输入的ReferenceLine先是分段为knot,后又细分成anchor_point(实际上是先有的anchor point再有的knot,在这个算法里顺序是反的).默认的配置文件表明前者的采样间距为25m,后者的为5m.至于为什么按照这个方法采样,先按下不表,后面详细解释. 19 | Apollo代码演示采样分段,[来源](https://github.com/xinchu911/Apollo-Note/tree/master/docs/planning). 20 | 先对输入ReferenceLine进行AnchorPoints采样,同时初始化bounding box的约束. 21 | ```c++ 22 | /// file in apollo/modules/planning/reference_line/reference_line_provider.cc 23 | void ReferenceLineProvider::GetAnchorPoints(const ReferenceLine &reference_line, 24 | std::vector *anchor_points) const { 25 | // interval为采样间隔，默认max_constraint_interval=5.0，即路径累积距离每5m采样一个点。 26 | const double interval = smoother_config_.max_constraint_interval(); 27 | // 路径采样点数量计算 28 | int num_of_anchors = std::max(2, static_cast(reference_line.Length() / interval + 0.5)); 29 | std::vector anchor_s; 30 | // uniform_slice函数就是对[0.0, reference_line.Length()]区间等间隔采样，每两个点之间距离为(length_-0.0)/(num_of_anchors - 1) 31 | common::util::uniform_slice(0.0, reference_line.Length(), num_of_anchors - 1, &anchor_s); 32 | // 根据每个采样点的累积距离s，以及Path的lane_segments_to_next_point_进行平滑插值，得到累积距离为s的采样点的坐标(x,y)，并进行轨迹点矫正 33 | for (const double s : anchor_s) { 34 | anchor_points->emplace_back(GetAnchorPoint(reference_line, s)); 35 | } 36 | anchor_points->front().longitudinal_bound = 1e-6; 37 | anchor_points->front().lateral_bound = 1e-6; 38 | anchor_points->front().enforced = true; 39 | anchor_points->back().longitudinal_bound = 1e-6; 40 | anchor_points->back().lateral_bound = 1e-6; 41 | anchor_points->back().enforced = true; 42 | } 43 | ``` 44 | 再进行分段t_knots_. 45 | ```c++ 46 | uint32_t num_spline = std::max(1u, static_cast(length / config_.qp_spline().max_spline_length() + 0.5)); 47 | for (std::uint32_t i = 0; i <= num_spline; ++i) { 48 | t_knots_.push_back(i * 1.0); 49 | } 50 | ``` 51 | 52 | 刚开始不解Apollo的这种分段做法,先从简单的设定开始.我决定在自车坐标系坐标轴等距采样,即沿着自车行进方(+y轴)采样,得点列($x_{ref_i},y_{ref_i}$) 53 | 此方法的问题在于,一个是不准,二个是在U-turn等场景下可能会出现采样多值的问题.可以考虑通过Dubin曲线筛选路径较短的那个值来解决. 54 | ![自车坐标系](./pic/coordination_setting.png) 55 | ## 1.数学模型建立 56 | 57 | ### 1.1.QP标准形式 58 | $$ 59 | \begin{aligned} 60 | minimize & \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\\\ 61 | s.t. \qquad & LB \leq x \leq UB \\\\ 62 | & A_{eq}x = b_{eq} \\\\ 63 | & Ax \geq b 64 | \end{aligned} 65 | $$ 66 | (注:其中等式约束可以化成两个不等式约束用osqp求解) 67 | ### 1.2.输入定义 68 | 69 | * **输入采样后的离散点列${P_i}$** 70 | $$(x_{refi},y_{refi}) \qquad i=1 \cdots n$$ 71 | 72 | * **路径方程** 73 | $P_i$总共$n$个点,$n-1$个路径段.可得$i$段的$x$与$y$的关于$t$的参数方程(后面只列$x(t)$的求解过程,$y(t)$类似).对每段曲线自变量归一化$t \in [0,1]$,后面用$t_{io}$与$t_{i1}$分别表示每段曲线的起止点. 74 | $$x_i(t)=a_{i0}+a_{i1} \times t+a_{i2} \times t^2+a_{i3} \times t^3+a_{i4} \times t^4+a_{i5} \times t^5\\\\ 75 | y_i(t)=b_{i0}+b_{i1} \times t+b_{i2} \times t^2+b_{i3} \times t^3+b_{i4} \times b^4+b_{i5} \times t^5$$ 76 | 77 | * **优化变量** 78 | 使用$\vec{a_i}$表示第$i$段$x_i(t)$多项式系数向量: 79 | $$\vec{a_i}=\begin{bmatrix} 80 | a_{i0} & a_{i1} & a_{i2} & a_{i3} & a_{i4} & a_{i5} 81 | \end{bmatrix}$$ 82 | 使用$\vec{b_i}$表示第$i$段$y_i(t)$多项式系数向量: 83 | 84 | $$\vec{b_i}=\begin{bmatrix} 85 | b_{i0} & b_{i1} & b_{i2} & b_{i3} & b_{i4} & b_{i5} 86 | \end{bmatrix}$$ 87 | 合并得: 88 | 89 | $$ X_i = {[\vec{a_i} \quad \vec{b_i}]}^T $$ 90 | 91 | * **目标方程** 92 | 使用$x_i(t)$的三阶导的平方和近似为路径总长度(车辆操作不管油门还是方向盘都是加速度输入,即为三阶导,平方和为操作量的大小.对应的物理意义为尽量减少操作量,Least Energy.) 93 | 94 | $$cost = \sum_{i=1}^n \bigg( \int_{t_{i0}}^{t_{i1}} ({x_i}^{\prime\prime\prime})^2(t)dt + \int_{t_{i0}}^{t_{i1}} ({y_i}^{\prime\prime\prime})^2(t)dt\bigg)$$ 95 | 96 | 对 $x_i(t)$ 求各阶导: 97 | $${x_i}^{\prime}(t)=\vec{a_i}\begin{vmatrix} 98 | 0\\\\1\\\\{2t}\\\\{3t^2}\\\\{4t^3}\\\\{5t^4} 99 | \end{vmatrix} = \vec{a_i} \cdot \vec{T^\prime(t)} $$ 100 | 101 | $${x_i}^{\prime\prime}(t)=\vec{a_i}\begin{vmatrix} 102 | 0\\\\0\\\\2\\\\{6t}\\\\{12t^2}\\\\{20t^3} 103 | \end{vmatrix} = \vec{a_i} \cdot \vec{T^{\prime\prime}(t)} $$ 104 | 105 | $${x_i}^{\prime\prime\prime}(t)=\vec{a_i}\begin{vmatrix} 106 | 0\\\\0\\\\0\\\\6\\\\{24t}\\\\{60t^2} 107 | \end{vmatrix} = \vec{a_i} \cdot \vec{T^{\prime\prime\prime}(t)} $$ 108 | 109 | 可得: 110 | 111 | $$ \int\limits_{t_{i0}}^{t_{i1}} 112 | ({x_i}^{\prime\prime\prime})^2(t) dt = \vec{a_i} \int\limits_{t_{i0}}^{t_{i1}}(T^{'''}(t) \cdot T^{'''}(t)^T \cdot \vec{a_i}^T)dt \\\\ = \vec{a_i} \int \limits_{t_{i0}}^{t_{i1}} \Bigg(\begin{bmatrix} 113 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 114 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 115 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 116 | 0 & 0 & 0 & {36} & {144t} & {360t^2}\\\\ 117 | 0 & 0 & 0 & {144t} & {576t^2} & {1440t^3}\\\\ 118 | 0 & 0 & 0 & {360t^2} & {1440t^3} & {3600t^4}\\\\ 119 | \end{bmatrix} 120 | \cdot \vec{a_i}^T \Bigg)dt \\\\=\vec{a_i}\begin{bmatrix} 121 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 122 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 123 | 0 & 0 & 0 & 0 & 0 & 0\\\\ 124 | 0 & 0 & 0 & 36 & 72 & 120\\\\ 125 | 0 & 0 & 0 & 72 & 192 & 360\\\\ 126 | 0 & 0 & 0 & 120 & 360 & 720\\\\ 127 | \end{bmatrix} 128 | \cdot \vec{a_i}^T $$ 129 | 130 | 令: 131 | $$ M = \begin{bmatrix} 132 | 36 & 72 & 120\\\\ 133 | 72 & 192 & 360\\\\ 134 | 120 & 360 & 720\\\\ 135 | \end{bmatrix} $$ 136 | 137 | 因此在不考虑松弛变量下的$H$如下: 138 | 139 | $$ cost= \sum_{i=1}^{n}(\vec{a_i}H_{ix}\vec{a_i}^T + \vec{b_i}H_{iy}\vec{b_i}^T) \\\\ = \sum_{i=1}^{n} (\vec{X_i}^T H_i \vec{X_i} )\\\\ = \vec{X}H\vec{X}^T $$ 140 | 141 | 其中, 142 | 143 | $$ H_i = 2\begin{bmatrix} 144 | 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\ 145 | 0 & &&&&\cdots &&&&&& 0\\\\ 146 | 0 & &&&&\cdots &&&&&& 0\\\\ 147 | 0 & 0 &0 &36 & 72 & 120 &&&\cdots &&&0\\\\ 148 | 0 & 0 &0 &72 & 192 & 360&&&\cdots &&&0\\\\ 149 | 0 & 0 &0 &120 & 360 & 720 &&&\cdots &&&0\\\\0 & &&&&\cdots &&&&&& 0\\\\ 150 | 0 & &&&&\cdots &&&&&& 0\\\\ 151 | 0 & &&&&\cdots &&&&&& 0\\\\ 152 | 0 & &&&&\cdots &&&& 36 & 72 & 120\\\\ 153 | 0 & &&&&\cdots &&&& 72 & 192 & 360\\\\ 154 | 0 & &&&&\cdots &&&& 120 & 360 & 720 \\\\ 155 | \end{bmatrix} $$ 156 | 157 | 然后,$H$为$12n \times 12n$的Hessain稀疏矩阵,仅在第$4i$行$4i$列～$6i$行$6i$列block处为$M$. 158 | 159 | ## 2.约束设定 160 | ### 2.1.平滑性约束 161 | 为保证各段曲线连接处平滑 162 | 各个连接点处的0阶,1阶,2阶,3阶导数连续.注意多段曲线连接处不一定在离散点处,见下图绿框处.(图为盗图([原址](https://zhuanlan.zhihu.com/p/345175084)),按本文设定终点应该为$p_{n-1}$) 163 | ![smoothness_constraint](./pic/smoothness_constraint.jpg) 164 | $$ x_i(t_{i1}) = x_{i+1}(t_{(i+1)0}) \qquad | \qquad y_i(t_{i1}) = y_{i+1}(t_{(i+1)0})$$ 165 | 166 | $$ x_i ^\prime(t_{i1}) = x_{i+1} ^\prime(t_{(i+1)0}) \qquad | \qquad y_i ^\prime(t_{i1}) = y_{i+1} ^\prime(t_{(i+1)0})$$ 167 | 168 | $$ x_i ^{\prime\prime}(t_{i1}) = x_{i+1} ^{\prime\prime}(t_{(i+1)0}) \qquad | \qquad y_i ^{\prime\prime}(t_{i1}) = y_{i+1} ^{\prime\prime}(t_{(i+1)0})$$ 169 | 170 | $$ x_i ^{\prime\prime\prime}(t_{i1}) = x_{i+1} ^{\prime\prime\prime}(t_{(i+1)0}) \qquad | \qquad y_i ^{\prime\prime\prime}(t_{i1}) = y_{i+1} ^{\prime\prime\prime}(t_{(i+1)0})$$ 171 | 172 | 平滑性约束的等式可以转化为如下矩阵形式: 173 | 174 | $$ \vec{T(t_{i1})} \cdot \vec{a_i}^T = \vec{T(t_{(i+1)0})} \cdot \vec{a_{i+1}}^T \\\\ 175 | \vec{T'(t_{i1})} \cdot \vec{a_i}^T = \vec{T'(t_{(i+1)0})} \cdot \vec{a_{i+1}}^T \\\\ 176 | \vec{T''(t_{i1}) }\cdot \vec{a_i}^T = \vec{T''(t_{(i+1)0})} \cdot \vec{a_{i+1}}^T \\\\ 177 | \vec{T'''(t_{i1})} \cdot \vec{a_i}^T =\vec{ T'''(t_{(i+1)0})} \cdot \vec{a_{i+1}}^T \\\\ $$ 178 | 179 | 即, 180 | $$ {[\vec{T(t_{i1})} -\vec{T(t_{(i+1)0})}] \cdot [\vec{a_i}^T \quad \vec{a_{i+1}}^T] = 0} \\\\ 181 | {[\vec{T'(t_{i1})} -\vec{T'(t_{(i+1)0})}] \cdot [\vec{a_i}^T \quad \vec{a_{i+1}}^T] = 0} \\\\ 182 | {[\vec{T''(t_{i1})} -\vec{T''(t_{(i+1)0})}] \cdot [\vec{a_i}^T \quad \vec{a_{i+1}}^T] = 0} \\\\ 183 | {[\vec{T'''(t_{i1})} -\vec{T'''(t_{(i+1)0})}] \cdot [\vec{a_i}^T \quad \vec{a_{i+1}}^T] = 0} \\\\ $$ 184 | 185 | 186 | 举例: 187 | 188 | $$ \begin{vmatrix} 189 | 1 & t_{i1} & t_{i1}^2 & t_{i1}^3 & t_{i1}^4&t_{i1}^5 & -1 & -t_{(i+1)0} & -t_{(i+1)0}^2 & -t_{(i+1)0}^3 & -t_{(i+1)0}^4&-t_{(i+1)0}^5\\\\ 190 | \end{vmatrix} 191 | \cdot 192 | \begin{vmatrix} 193 | a_{i0} \\\\ a_{i1} \\\\ a_{i2} \\\\ a_{i3} \\\\ a_{i4} \\\\ a_{i5} \\\\ a_{i+1,0} \\\\ a_{i+1,1} \\\\ a_{i+1,2} \\\\ a_{i+1,3} \\\\ a_{i+1,4} \\\\ a_{i+1,5} 194 | \end{vmatrix} 195 | = 0 $$ 196 | 197 | 化为标准等式约束形式,共有$N_1=4 \times (n-1) \times 2$个约束. 198 | 199 | $$A_{eq1i} \cdot X = b_{eq1} = 0 \\\\ 200 | X = [\vec{a_1} \cdots \vec{a_i} \quad \vec{a_{i+1}} \cdots \vec{a_n} \quad \vec{b_1} \cdots \vec{b_n}]^T$$ 201 | 202 | $A_{eq1i}$为$N_1 \times 12n$的矩阵,其中第$i$行$6(i-1)$列 ~ $6i$列为约束向量,$y_i(t_i)$的约束为第$4(n-1)+i$行$6(n+i-1)$列 ~ $6(n+i)$列为约束向量的稀疏矩阵. 203 | 204 | ### 2.2.起止点约束 205 | 假设起止点0-3阶输入值如下所示: 206 | 207 | $(x_{ref_1},y_{ref_1}),(x_{ref_1}',y_{ref_1}'),(x_{ref_1}'',y_{ref_1}''),(x_{ref_1}''',y_{ref_1}''') \\\\ 208 | (x_{ref_n},y_{ref_n}),(x_{ref_n}',y_{ref_n}'),(x_{ref_n}'',y_{ref_n}''),(x_{ref_n}''',y_{ref_n}''')$ 209 | 210 | 可得等式约束如下(以$x(t)$为例): 211 | $$x_1(t_{10}) = 212 | \begin{vmatrix} 1 & t_{10} & t_{10}^2 & t_{10}^3 & t_{10}^4 & t_{10}^5 \end{vmatrix} 213 | \cdot 214 | \begin{vmatrix} a_{10} \\\\ a_{11} \\\\ a_{12} \\\\ a_{13} \\\\ a_{14} \\\\ a_{15}\end{vmatrix} = x_{ref_1}$$ 215 | 216 | 且 217 | 218 | $$x_1'(t_{10}) = 219 | \begin{vmatrix} 0 & 1 & 2t_{10} & 3t_{10}^2 & 4t_{10}^3 & 5 t_{10}^4 \end{vmatrix} 220 | \cdot 221 | \begin{vmatrix} a_{10} \\\\ a_{11} \\\\ a_{12} \\\\ a_{13} \\\\ a_{14} \\\\ a_{15} \end{vmatrix} = x_{ref_1}'$$ 222 | 223 | 且 224 | 225 | $$x_1''(t_{10}) = 226 | \begin{vmatrix} 0&0& 2 & 6t_{10} & 12t_{10}^2 & 20t_{10}^3 \end{vmatrix} 227 | \cdot 228 | \begin{vmatrix} a_{10} \\\\ a_{11} \\\\ a_{12} \\\\ a_{13} \\\\ a_{14} \\\\ a_{15} \end{vmatrix} = x_{ref_1}''$$ 229 | 230 | 且 231 | 232 | $$x_1'''(t_{10}) = 233 | \begin{vmatrix} 0&0& 0 & 6 & 24t_{10} & 60t_{10}^2 \end{vmatrix} 234 | \cdot 235 | \begin{vmatrix} a_{10} \\\\ a_{11} \\\\ a_{12} \\\\ a_{13} \\\\ a_{14} \\\\ a_{15} \end{vmatrix} = x_{ref_1}'''$$ 236 | 237 | 238 | 但apollo里未对起止点进行如此严格的约束,只进行了方向约束即$arctan(\frac{y_i'(t_{10})}{x_i'(t_{10})})$的约束(见下节位置约束图中的黄色箭头). 239 | 240 | 由于$arctan$函数为单调递增函数可得如下(以起点为例): 241 | 242 | $$ \frac{y'(0)}{x'(0)} = x'_{ref_1} $$ 243 | 244 | 即: 245 | 246 | $$T'(0)\vec{b_1}+[-x'_{ref_1}T'(0)]\vec{a_1} =0$$ 247 | 248 | 化为标准约束方程: 249 | 250 | $$A_{eq2_1} \cdot X = b_{eq2_1} \\\\ 251 | => A_{eq2_1} = [0 \quad -x_{ref_1}' \quad 0 \cdots 0 \quad 1 \quad 0 \quad 0 \quad 0 \quad 0 \cdots 0 ] $$ 252 | $$A_{eq2_2} \cdot X = b_{eq2_2} \\\\ 253 | => A_{eq2_2}=[0 \cdots 0 \quad -x_{ref_n}'\quad 0 \quad 0 \quad 0 \quad 0 \quad 0 \cdots 0 \quad 1 \quad 0 \quad 0 \quad 0 \quad 0] $$ 254 | 其中第$2$,$6n+2$; $6(n-1)+2$,$6(2n-1)+2$列处分别不为0. 255 | 256 | 257 | ### 2.3.位置约束 258 | 平滑之后的曲线和原始曲线的差别不能差别太大.如下图所示进行约束. 259 | ![position_constraint](./pic/position_constraint.jpg) 260 | 约束的方法为对平滑后的曲线进行采样得到m个点的点列$Q_i$.即: 261 | $$Q_i=\bigg ( x_{q_i}(t_i), y_{q_i}(t_i) \bigg ), \quad i=1 \cdots m $$ 262 | boudary可以将障碍物,路牙,不可通行区域等信息融合进去. 263 | 264 | 265 | 例如在Apollo的`planning/reference_line/ReferenceLineProvider`类中考虑到了宽道以及路牙的偏移(当道路比较宽时,车辆不能一味的在中间行驶,需要考虑到其他车辆超车情况.在这种情况下,车辆需要靠右行驶(当然不同区域的模式不一样,部分地区是靠左形式),所以道路过宽时,需要将轨迹点向右或者向左矫正一段距离).代码如下: 266 | 267 | ```c++ 268 | //如果车道宽度大于车辆宽度的wide_lane_threshold_factor倍，默认为2，则需要靠边行驶，因为存在其他车辆超车的可能性 269 | // shift to left (or right) on wide lanes 270 | if (!(waypoint.lane->lane().left_boundary().virtual_() || 271 | waypoint.lane->lane().right_boundary().virtual_()) && 272 | total_width > adc_width * smoother_config_.wide_lane_threshold_factor()) { 273 | // 靠右行驶模式 274 | if (smoother_config_.driving_side() == ReferenceLineSmootherConfig::RIGHT) { 275 | shifted_left_width = adc_half_width + adc_width * smoother_config_.wide_lane_shift_remain_factor(); 276 | } else { 277 | // 靠左形式模式 278 | shifted_left_width = std::fmax( 279 | adc_half_width, 280 | total_width - (adc_half_width + adc_width * smoother_config_.wide_lane_shift_remain_factor())); 281 | } 282 | } 283 | ``` 284 | 285 | 本实例不考虑融合这些约束. 286 | 采样方法为在每段曲线$t_i=0.5$处采样,算出 287 | $$ m = n \\\\ 288 | x_{qi} = \vec{T(0.5)} \cdot \vec{a_i} \\\\ 289 | y_{qi} = \vec{T(0.5)} \cdot \vec{b_i} \\\\ 290 | y_{ref_i} = y_i \\\\ 291 | x_{ref_i} = c_0+c_1y_{ref_i}+c_2y_{ref_i}^2+c_3y_{ref_i}^3$$ 292 | 293 | 设boundry为如下矩阵,由于使用Y轴作为采样比对轴,其实对于y坐标的约束可以忽略(不考虑其他约束条件的前提下),所有元素均为正值. 294 | $$ \begin{bmatrix} 295 | \vec{x_l} \\\\ 296 | \vec{x_u} \\\\ 297 | \vec{y_l} \\\\ 298 | \vec{y_l} 299 | \end{bmatrix}$$ 300 | 301 | 举例: 302 | $$x_{q_i}(t_i) - x_{ref_i} >= \vec{x_l}[i-1]$$ 303 | 总结可得如下非等式约束组,共$2 \times 2 \times m$个: 304 | 305 | $$ AX >= -b_l \\\\ 306 | AX <= b_u $$ 307 | 308 | 举例$x_1$处X坐标的下限约束方程: 309 | 310 | $$ \vec{a_i} \cdot T(0.5) - \vec{C}T({[\vec{b_i} \cdot T(0.5)]}) >= \vec{x_l}[i-1]$$ 311 | 312 | $$ X = [\vec{a_1} \cdots \vec{a_i} \quad \vec{a_{i+1}} \cdots \vec{a_n} \quad \vec{b_1} \cdots \vec{b_n}]^T$$ 313 | 314 | 315 | ### 2.4.正则化 316 | 为了防止过拟合加入正则化项 317 | $$ cost_{regular} = \frac{1}{2}X^TRX$$ 318 | $R$为对角线元素均为1.0e-5的对角矩阵. 319 | 最终的目标函数为 320 | $$ minimize \frac{1}{2} \cdot X^T \cdot (H+R) \cdot X $$ 321 | 322 | 323 | ## 3.构造验证 324 | 假设如下图所示曲线(纯几何形状,不考虑实际道路) 325 | ![输入三项式曲线与相关点](./pic/input_curve_points_based_on_axis_interval.jpg) 326 | $$ c_0=0\\\\c_1=-0.4\\\\c_2=0.02\\\\c_3=-0.004$$ 327 | 自车初始位置(0,0),$yaw=90^o$即+y方向. 328 | 采用OSQP二次规划求解器求解.关于OSQP求解器及其C++的接口应用可见本博客其他[博文](http://139.224.210.242/2021/07/15/p2_osqp-introduction/index.html). 329 | 求解的C++代码参考[repo](https://github.com/xinchu911/path_smootning_algorithm_demo)(代码为面向过程式编程后续整理为面向对象).文件结构为 330 | 331 | ```bash 332 | . 333 | ├── build 334 | ├── CMakeLists.txt 335 | ├── pic 336 | ├── README.md 337 | └── src 338 | ├── input_data.csv 339 | ├── input_data_generation_and_result_plot.m 340 | ├── main.cpp 341 | ├── plot_result.m 342 | └── QPSolution.csv 343 | ``` 344 | 345 | 346 | .其中`input_data_generation_and_result_plot.m`的matlab脚本用于求解采样点以及画出拟合前后的形状. 347 | 348 | 第一次对位置约束的采样点使用的是y轴等距采样,结果如下图所示: 349 | ![result_y_axis_interval_1](./pic/result_y_axis_interval_1.jpg) 350 | 整体看起来拟合的挺贴合,但问题出现在起止点,起点的误差为0.22m,终点的误差为0.20m,对于车辆而言算是很大的误差了. 351 | ![result_y_axis_interval_3](./pic/result_y_axis_interval_3.jpg) 352 | ![result_y_axis_interval_2](./pic/result_y_axis_interval_2.jpg) 353 | 354 | 做到这里我意识到一个问题,如果对原始曲线与拟合后的曲线进行对比约束的话,如何找到两个曲线上对应的点进行对比. 355 | 上面的做法是用一个坐标轴为基准找对应的点,明显不精确,甚至对于大曲率曲线,这种对应方法会存在多值. 356 | 较为精确的做法可能为先随机在原曲线采样,计算出其一阶二阶导然后在拟合曲线上找到一阶二阶相同的点为其对应点. 357 | 化成数学问题如下, 358 | 已知一个点$(x_0,y_0)$及其一阶二阶导$y'(x),y''(x)$,求一段参数曲线$\begin{cases} 359 | x(t)=\vec{a}T\\\\ 360 | y(t)=\vec{b}T 361 | \end{cases}$上的点(即$t$)一阶二阶导与之相等,$t \in [0,1]$. 362 | 即便是二阶也有三次项,明显为非线性约束.可以考虑用非线性优化的算法求解. 363 | 但是使用线性方法近似是否也能达到期待精度? 364 | 365 | 366 | 换一种对应方法,即采样点对应每段曲线的比例相同处.严格的求解也为非线性.例如假定采样点为每段曲线的中点处,对拟合曲线求其曲线长度方程为: 367 | 368 | 369 | $$ \int_{y_0}^{y_i} \left ( \sqrt{1+{\left(x^{\prime}(y)\right)}^2} \right ) dy = \int_{y_0}^{y_i} \left ( \sqrt{1+ {\left(c_1 + 2c_2y+3c_3y^2\right)}^2} \right ) dy = s_{interval} * i $$ 370 | 371 | 372 | 如果直接用$t$值直接代替比例呢? 373 | 这就是Apollo的做法.这里就是用knot分段后用achor_point约束的原因(其他原因也可能为增大求解可能性与求解效率).每个achor_point在长度上每段的1/5处,通过较多约束点数的方式提高用$t$直接代替比例的精度.achor_point之间的距离为5m可能是借鉴普通家用车的最小转弯半径一般在5m左右. 374 | 375 | 下面是采用一个achor_point,$t=0.5$,也就是每段中点做约束的结果. 376 | ![result_y_arc_length_1](./pic/result_y_arc_length_1.jpg) 377 | 可以看到拟合的效果比上面好很多,起点与终点的约束都降到了0.1m左右.如果增加anchor_point进行约束估计会进一步降低误差. 378 | ![result_y_arc_length_3](./pic/result_y_arc_length_3.jpg) 379 | ![result_y_arc_length_2](./pic/result_y_arc_length_2.jpg) 380 | 381 | ## 4.总结 382 | 本文尝试复现了Apollo的算法,即对三次多项式曲线进行分段采样后,进行5次多项式的样条曲线进行拟合的形式光滑原曲线,实现了各点0-3阶导数连续,并且需要操作量最小的优化.优化算法基于二次规划,并对位置约束的非线性约束采取了近似的线性近似的方法. 383 | 384 | 注: 385 | 1. 本文参考的文章都嵌在了文中的链接内. 386 | -------------------------------------------------------------------------------- /pic/Runge_Phenomenon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/Runge_Phenomenon.png -------------------------------------------------------------------------------- /pic/a_single_track_vehicle_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/a_single_track_vehicle_model.png -------------------------------------------------------------------------------- /pic/coodination.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/coodination.png -------------------------------------------------------------------------------- /pic/coordination_setting.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/coordination_setting.png -------------------------------------------------------------------------------- /pic/input_curve_points.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/input_curve_points.jpg -------------------------------------------------------------------------------- /pic/input_curve_points_based_on_axis_interval.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/input_curve_points_based_on_axis_interval.jpg -------------------------------------------------------------------------------- /pic/position_constraint.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/position_constraint.jpg -------------------------------------------------------------------------------- /pic/result_y_arc_length_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_arc_length_1.jpg -------------------------------------------------------------------------------- /pic/result_y_arc_length_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_arc_length_2.jpg -------------------------------------------------------------------------------- /pic/result_y_arc_length_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_arc_length_3.jpg -------------------------------------------------------------------------------- /pic/result_y_axis_interval_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_axis_interval_1.jpg -------------------------------------------------------------------------------- /pic/result_y_axis_interval_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_axis_interval_2.jpg -------------------------------------------------------------------------------- /pic/result_y_axis_interval_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/result_y_axis_interval_3.jpg -------------------------------------------------------------------------------- /pic/smoothness_constraint.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xinchu911/path_smootning_algorithm_demo/aca2cfe283bc189353f0518d9b7c82af1c419583/pic/smoothness_constraint.jpg -------------------------------------------------------------------------------- /src/QPSolution.csv: -------------------------------------------------------------------------------- 1 | QPSolution 2 | 0.0653518 3 | -1.92868 4 | 0.110907 5 | 0.0151848 6 | -0.147287 7 | 0.0261417 8 | -1.85841 9 | -2.11975 10 | -0.465825 11 | -0.312546 12 | 0.119617 13 | 0.00427154 14 | -4.63263 15 | -3.48917 16 | -0.643046 17 | 0.208637 18 | -0.0049795 19 | -0.0143786 20 | -8.57555 21 | -4.24116 22 | -0.190809 23 | 0.0449325 24 | -0.0228662 25 | 0.00455694 26 | -12.9809 27 | -4.55668 28 | -0.147641 29 | -0.000962762 30 | -0.00271573 31 | 0.000961616 32 | -17.6879 33 | -4.8609 34 | -0.157209 35 | -0.00220952 36 | 0.00025335 37 | 0.000132902 38 | -0.05021 39 | 4.82165 40 | -0.0723062 41 | 0.00505857 42 | -0.0559925 43 | 0.00836846 44 | 4.65656 45 | 4.51008 46 | -0.309393 47 | -0.135227 48 | 0.0195 49 | 0.00584189 50 | 8.74736 51 | 3.59285 52 | -0.539653 53 | 0.00119219 54 | 0.037248 55 | -0.00620507 56 | 11.8328 57 | 2.63509 58 | -0.374647 59 | 0.0881335 60 | -0.0220479 61 | -0.00199417 62 | 14.1573 63 | 2.05203 64 | -0.262483 65 | -0.0199998 66 | -0.00723365 67 | 0.00319059 68 | 15.9228 69 | 1.45408 70 | -0.333982 71 | -0.0170285 72 | 0.00353096 73 | 0.000322744 -------------------------------------------------------------------------------- /src/input_data.csv: -------------------------------------------------------------------------------- 1 | No,input_points_x,input_points_y,anchor_points_x,anchor_points_y 2 | 1,0,0,-0.877987143447578,2.34066519505972 3 | 2,-1.83034258887661,4.6512968691933,-3.06718606367727,6.82084008030984 4 | 3,-4.65026188159537,8.75188292150733,-6.51467125218903,10.4143485273059 5 | 4,-8.56830366794098,11.8380263770544,-10.7427893369789,13.0702913983234 6 | 5,-12.9956171318931,14.153340662669,-15.3011350365343,15.1195367829887 7 | 6,-17.6435289263943,15.9927683378651,0,0 8 | -------------------------------------------------------------------------------- /src/input_data_generation_and_result_plot.m: -------------------------------------------------------------------------------- 1 | clear; 2 | clc; 3 | %标志位:采样使用坐标轴等距->1,其他值使用曲线长等距 4 | flag_axis_interval=0; 5 | %标志位:不画出结果图->0,否则为其他值 6 | flag_plot_result=1; 7 | 8 | pc=[0, -0.4, 0.02, -0.004]; %输入三次多项式曲线系数 9 | %初始的x,y均为0 10 | init_x=0; 11 | init_y=0; 12 | knot_interval=5; 13 | knot_num=6; 14 | anchor_num=knot_num-1; 15 | input_points=zeros(knot_num-1,2); 16 | anchor_points=zeros(knot_num,2); 17 | if flag_axis_interval ~= 1 18 | %采用符号函数求解积分,得等距原曲线采样点 19 | syms x x0; 20 | 21 | S_length=knot_interval; 22 | y=pc(1)+pc(2)*x+pc(3)*x^2+pc(4)*x^3; 23 | 24 | f=sqrt(1+diff(y)*diff(y)); 25 | %曲线积分表达式 26 | for i=1:anchor_num 27 | S=int(f,x,[init_x,x0]); 28 | input_points(i,2)=vpasolve(S==(S_length+knot_interval*(i-1)),x0); 29 | input_points(i,1)=pc(1)+pc(2)*input_points(i,2)+pc(3)*input_points(i,2)^2+pc(4)*input_points(i,2)^3; 30 | end 31 | input_points=[[0,0];input_points]; 32 | plot(input_points(1:knot_num,1),input_points(1:knot_num,2),'.','color','r','MarkerSize',10) 33 | hold on 34 | %在采样点的中点处算anchor_point 35 | S_t=int(f,x,[0,x0]); 36 | anchor_start_y=vpasolve(S==(S_length/2),x0); 37 | anchor_start_x=pc(1)+pc(2)*anchor_start_y+pc(3)*anchor_start_y^2+pc(4)*anchor_start_y^3; 38 | for i=1:anchor_num-1 39 | S_an=int(f,x,[anchor_start_y,x0]); 40 | anchor_points(i,2)=vpasolve(S_an==(S_length+knot_interval*(i-1)),x0); 41 | anchor_points(i,1)=pc(1)+pc(2)*anchor_points(i,2)+pc(3)*anchor_points(i,2)^2+pc(4)*anchor_points(i,2)^3; 42 | end 43 | anchor_points=[[anchor_start_x,anchor_start_y];anchor_points]; 44 | plot(anchor_points(1:anchor_num,1),anchor_points(1:anchor_num,2),'.','color','b','MarkerSize',10) 45 | hold on 46 | else 47 | %按y轴等距采样与anchor_point 48 | for i=1:knot_num 49 | input_points(i,2)=(i-1)*knot_interval; 50 | input_points(i,1)=pc(1)+pc(2)*input_points(i,2)+pc(3)*input_points(i,2)^2+pc(4)*input_points(i,2)^3; 51 | if i~=1 52 | anchor_points(i-1,2)=(i-1.5)*knot_interval; 53 | anchor_points(i-1,1)=pc(1)+pc(2)*anchor_points(i-1,2)+pc(3)*anchor_points(i-1,2)^2+pc(4)*anchor_points(i-1,2)^3; 54 | end 55 | end 56 | plot(input_points(1:knot_num,1),input_points(1:knot_num,2),'.','color','r','MarkerSize',10) 57 | hold on 58 | plot(anchor_points(2:anchor_num,1),anchor_points(2:anchor_num,2),'.','color','b','MarkerSize',10) 59 | hold on 60 | end 61 | %输出采样点anchor_point至csv文件供C++程序读取 62 | No=[1:knot_num].'; 63 | title={'No','input_points_x','input_points_y','anchor_points_x','anchor_points_y'}; 64 | result_table=table(No,input_points(1:knot_num,1),input_points(1:knot_num,2),double(anchor_points(1:knot_num,1)),double(anchor_points(1:knot_num,2)),'VariableNames',title); 65 | writetable(result_table,'input_data.csv'); 66 | 67 | y=input_points(1,2):0.01:input_points(knot_num,2); 68 | plot(polyval([-0.004,0.02,-0.4,0],y),y,'b') 69 | hold on 70 | 71 | xlabel('x'); 72 | ylabel('y'); 73 | % legend('分段点','anchor点','输入三项式曲线') 74 | 75 | %read结果多项式系数向量,plot结果 76 | if flag_plot_result~=0 77 | QPSolution=csvread('QPSolution.csv',1,0); 78 | p=zeros(12,6); 79 | step=0.01; 80 | t=0:step:1; 81 | bounding_box=0.05; 82 | %画出二次规划后的拟合各段曲线 83 | for i=1:6 84 | p(1:6,i)=flip(QPSolution(1+6*(i-1):6*i,:)); 85 | p(7:12,i)=flip(QPSolution(36+1+6*(i-1):36+6*i,:)); 86 | plot(polyval(p(1:6,i),t),polyval(p(7:12,i),t),'g') 87 | %画出各anchor_point的bounding box 88 | if i<=5 89 | y_bouding_low=(anchor_points(i,2)-bounding_box).*ones(2*bounding_box/step+1,1); 90 | y_bouding_high=(anchor_points(i,2)+bounding_box).*ones(2*bounding_box/step+1,1); 91 | x_bouding_low=anchor_points(i,1)-bounding_box.*ones(2*bounding_box/step+1,1); 92 | x_bouding_high=(anchor_points(i,1)+bounding_box).*ones(2*bounding_box/step+1,1); 93 | plot(anchor_points(i,1)-bounding_box:step:anchor_points(i,1)+bounding_box,y_bouding_low,':','LineWidth',2,'Color','c') 94 | plot(anchor_points(i,1)-bounding_box:step:anchor_points(i,1)+bounding_box,y_bouding_high,':','LineWidth',2,'Color','c') 95 | plot(x_bouding_low,anchor_points(i,2)-bounding_box:step:anchor_points(i,2)+bounding_box,':','LineWidth',2,'Color','c') 96 | plot(x_bouding_high,anchor_points(i,2)-bounding_box:step:anchor_points(i,2)+bounding_box,':','LineWidth',2,'Color','c') 97 | end 98 | end 99 | end 100 | 101 | -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "OsqpEigen/OsqpEigen.h" 7 | #include 8 | 9 | //(x,y) 10 | using Point2D = std::pair; 11 | using namespace std; 12 | 13 | struct spline_curve 14 | { 15 | double a0; 16 | double a1; 17 | double a2; 18 | double a3; 19 | double a4; 20 | double a5; 21 | double b0; 22 | double b1; 23 | double b2; 24 | double b3; 25 | double b4; 26 | double b5; 27 | double x_start; 28 | double y_start; 29 | double x_end; 30 | double y_end; 31 | }; 32 | 33 | bool readCSVInput(int N, vector &input_points_x, vector &input_points_y, vector &anchor_points_x, vector &anchor_points_y) 34 | { 35 | ifstream readCSV("../src/input_data.csv", ios::in); 36 | int lineNum = 0; 37 | string lineString; 38 | vector> stringArray; 39 | while (getline(readCSV, lineString)) 40 | { 41 | cout << lineString << endl; 42 | if (lineNum > 0) 43 | { 44 | stringstream ss(lineString); 45 | string s; 46 | vector oneLineStrings; 47 | while (getline(ss, s, ',')) 48 | oneLineStrings.push_back(s); 49 | if (oneLineStrings.size() != 5) 50 | { 51 | cout << "ERROR:oneLineStrings.size() != 5" << endl; 52 | return false; 53 | } 54 | else 55 | { 56 | input_points_x.push_back(std::stod(oneLineStrings[1])); 57 | input_points_y.push_back(std::stod(oneLineStrings[2])); 58 | anchor_points_x.push_back(std::stod(oneLineStrings[3])); 59 | anchor_points_y.push_back(std::stod(oneLineStrings[4])); 60 | } 61 | } 62 | lineNum++; 63 | } 64 | if (N == input_points_x.size()) 65 | { 66 | return true; 67 | } 68 | else 69 | { 70 | input_points_x.clear(); 71 | input_points_y.clear(); 72 | anchor_points_x.clear(); 73 | anchor_points_y.clear(); 74 | cout << "ERROR:N == input_points_x.size()" << endl; 75 | return false; 76 | } 77 | } 78 | 79 | bool writeCSVOutput(Eigen::VectorXd const &QPSolution) 80 | { 81 | ofstream writeCSV; 82 | writeCSV.open("../src/QPSolution.csv", ios::out); 83 | writeCSV << "QPSolution" << endl; 84 | for (int i = 1; i < QPSolution.size(); i++) 85 | { 86 | writeCSV << QPSolution[i - 1] << endl; 87 | } 88 | writeCSV << QPSolution[QPSolution.size() - 1]; 89 | writeCSV.close(); 90 | cout << "INFO: csv output." << endl; 91 | return true; 92 | } 93 | 94 | int main() 95 | { 96 | Eigen::VectorXd c(4); 97 | c << 0, -0.4, 0.02, -0.004; 98 | int N = 6; //input point number 99 | int sampling_num = N - 1; 100 | double interval = 5; 101 | Point2D start_point(0, 0); 102 | vector input_points_x; 103 | vector input_points_y; 104 | vector anchor_points_x; 105 | vector anchor_points_y; 106 | if (!readCSVInput(N, input_points_x, input_points_y, anchor_points_x, anchor_points_y)) 107 | { 108 | cout << "ERROR:readCSVInput return" << endl; 109 | return 1; 110 | } 111 | vector input_points; 112 | vector anchor_points; 113 | double x_ref = 0.0; 114 | double y_ref = 0.0; 115 | double ref_tan_start = 0 * c[0] + 1 * c[1] + 2 * c[2] * start_point.second + 3 * c[3] * start_point.second * start_point.second; 116 | // cout << "ref_tan_start: " << ref_tan_start << endl; 117 | double ref_tan_end = 0 * c[0] + 1 * c[1] + 2 * c[2] * (start_point.second + (N - 1) * interval) + 3 * c[3] * pow(start_point.second + (N - 1) * interval, 2); 118 | // cout << "ref_tan_end: " << ref_tan_end << endl; 119 | 120 | double bounding_box_constraint = 1e-7; 121 | Point2D temp_point; 122 | for (int i = 0; i <= N; i++) 123 | { 124 | temp_point.first = input_points_x[i]; 125 | temp_point.second = input_points_y[i]; 126 | input_points.push_back(temp_point); 127 | } 128 | for (int i = 0; i < sampling_num; i++) 129 | { 130 | temp_point.first = anchor_points_x[i]; 131 | temp_point.second = anchor_points_y[i]; 132 | anchor_points.push_back(temp_point); 133 | } 134 | //至此得到输入离散点列与位置约束的原始线上参考点(anchor points) 135 | 136 | Eigen::SparseMatrix hessian(72, 72); 137 | for (int i = 0; i < hessian.rows() / N; i++) 138 | { 139 | hessian.insert(6 * i + 3, 6 * i + 3) = 36; 140 | hessian.insert(6 * i + 3, 6 * i + 4) = 72; 141 | hessian.insert(6 * i + 3, 6 * i + 5) = 120; 142 | hessian.insert(6 * i + 4, 6 * i + 3) = 72; 143 | hessian.insert(6 * i + 4, 6 * i + 4) = 192; 144 | hessian.insert(6 * i + 4, 6 * i + 5) = 360; 145 | hessian.insert(6 * i + 5, 6 * i + 3) = 120; 146 | hessian.insert(6 * i + 5, 6 * i + 4) = 360; 147 | hessian.insert(6 * i + 5, 6 * i + 5) = 720; 148 | } 149 | Eigen::SparseMatrix R(72, 72); 150 | for (int i = 0; i < R.rows(); i++) 151 | { 152 | R.insert(i, i) = 1.0e-5; 153 | } 154 | Eigen::SparseMatrix H(72, 72); 155 | H = hessian + R; 156 | int smooth_constraint_num = 4 * (N - 1) * 2; 157 | int position_constraint_num = (N - 1) * 2; 158 | int start_end_constraint_num = 2; 159 | int total_constraint_num = smooth_constraint_num + position_constraint_num + start_end_constraint_num; 160 | Eigen::VectorXd lowerBound(total_constraint_num); 161 | Eigen::VectorXd upperBound(total_constraint_num); 162 | Eigen::SparseMatrix A(total_constraint_num, 12 * N); //所有约束的矩阵 163 | Eigen::VectorXd gradient = Eigen::VectorXd::Zero(2 * 6 * N); 164 | //光滑性约束共4*(N-1)*2个,由等式化为2个不等式约束 165 | Eigen::VectorXd T0(12); //0阶导数向量 166 | Eigen::VectorXd T0_05(12); //0阶导数向量参数0.5时 167 | Eigen::VectorXd T1(12); //1阶导数向量 168 | Eigen::VectorXd T2(12); //2阶导数向量 169 | Eigen::VectorXd T3(12); //3阶导数向量 170 | T0 << 1, 1, 1, 1, 1, 1, -1, 0, 0, 0, 0, 0; 171 | T1 << 0, 1, 2, 3, 4, 5, 0, -1, 0, 0, 0, 0; 172 | T2 << 0, 0, 2, 6, 12, 20, 0, 0, -2, 0, 0, 0; 173 | T3 << 0, 0, 0, 6, 24, 60, 0, 0, 0, -6, 0, 0; 174 | T0_05 << 1, 0.5, pow(0.5, 2), pow(0.5, 3), pow(0.5, 4), pow(0.5, 5), 0, 0, 0, 0, 0, 0; 175 | //T0光滑约束-等式约束 176 | for (int i = 0; i < N - 1; ++i) 177 | { 178 | //向量ai 179 | A.insert(i, 6 * i + 0) = T0[0]; 180 | A.insert(i, 6 * i + 1) = T0[1]; 181 | A.insert(i, 6 * i + 2) = T0[2]; 182 | A.insert(i, 6 * i + 3) = T0[3]; 183 | A.insert(i, 6 * i + 4) = T0[4]; 184 | A.insert(i, 6 * i + 5) = T0[5]; 185 | A.insert(i, 6 * i + 6) = T0[6]; 186 | //向量bi 187 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 0) = T0[0]; 188 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 1) = T0[1]; 189 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 2) = T0[2]; 190 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 3) = T0[3]; 191 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 4) = T0[4]; 192 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 5) = T0[5]; 193 | A.insert(i + total_constraint_num / 2, 6 * i + 6 * N + 6) = T0[6]; 194 | lowerBound(i) = 0; 195 | upperBound(i) = 0; 196 | lowerBound(i + total_constraint_num / 2) = 0; 197 | upperBound(i + total_constraint_num / 2) = 0; 198 | } 199 | 200 | //T1光滑约束-等式约束 201 | for (int i = 0; i < N - 1; ++i) 202 | { 203 | //向量ai 204 | A.insert(i + N - 1, 6 * i + 0) = T1[0]; 205 | A.insert(i + N - 1, 6 * i + 1) = T1[1]; 206 | A.insert(i + N - 1, 6 * i + 2) = T1[2]; 207 | A.insert(i + N - 1, 6 * i + 3) = T1[3]; 208 | A.insert(i + N - 1, 6 * i + 4) = T1[4]; 209 | A.insert(i + N - 1, 6 * i + 5) = T1[5]; 210 | A.insert(i + N - 1, 6 * i + 7) = T1[7]; 211 | //向量bi 212 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 0) = T1[0]; 213 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 1) = T1[1]; 214 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 2) = T1[2]; 215 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 3) = T1[3]; 216 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 4) = T1[4]; 217 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 5) = T1[5]; 218 | A.insert(i + total_constraint_num / 2 + N - 1, 6 * i + 6 * N + 7) = T1[7]; 219 | lowerBound(i + N - 1) = 0; 220 | upperBound(i + N - 1) = 0; 221 | lowerBound(i + total_constraint_num / 2 + N - 1) = 0; 222 | upperBound(i + total_constraint_num / 2 + N - 1) = 0; 223 | } 224 | 225 | //T2光滑约束-等式约束 226 | for (int i = 0; i < N - 1; ++i) 227 | { 228 | //向量ai 229 | A.insert(i + (N - 1) * 2, 6 * i + 0) = T2[0]; 230 | A.insert(i + (N - 1) * 2, 6 * i + 1) = T2[1]; 231 | A.insert(i + (N - 1) * 2, 6 * i + 2) = T2[2]; 232 | A.insert(i + (N - 1) * 2, 6 * i + 3) = T2[3]; 233 | A.insert(i + (N - 1) * 2, 6 * i + 4) = T2[4]; 234 | A.insert(i + (N - 1) * 2, 6 * i + 5) = T2[5]; 235 | A.insert(i + (N - 1) * 2, 6 * i + 8) = T2[8]; 236 | //向量bi 237 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 0) = T2[0]; 238 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 1) = T2[1]; 239 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 2) = T2[2]; 240 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 3) = T2[3]; 241 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 4) = T2[4]; 242 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 5) = T2[5]; 243 | A.insert(i + total_constraint_num / 2 + (N - 1) * 2, 6 * i + 6 * N + 8) = T2[8]; 244 | 245 | lowerBound(i + (N - 1) * 2) = 0; 246 | upperBound(i + (N - 1) * 2) = 0; 247 | lowerBound(i + total_constraint_num / 2 + (N - 1) * 2) = 0; 248 | upperBound(i + total_constraint_num / 2 + (N - 1) * 2) = 0; 249 | } 250 | //T3光滑约束-等式约束 251 | for (int i = 0; i < N - 1; ++i) 252 | { 253 | //向量ai 254 | A.insert(i + (N - 1) * 3, 6 * i + 0) = T3[0]; 255 | A.insert(i + (N - 1) * 3, 6 * i + 1) = T3[1]; 256 | A.insert(i + (N - 1) * 3, 6 * i + 2) = T3[2]; 257 | A.insert(i + (N - 1) * 3, 6 * i + 3) = T3[3]; 258 | A.insert(i + (N - 1) * 3, 6 * i + 4) = T3[4]; 259 | A.insert(i + (N - 1) * 3, 6 * i + 5) = T3[5]; 260 | A.insert(i + (N - 1) * 3, 6 * i + 9) = T3[9]; 261 | //向量bi 262 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 0) = T3[0]; 263 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 1) = T3[1]; 264 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 2) = T3[2]; 265 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 3) = T3[3]; 266 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 4) = T3[4]; 267 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 5) = T3[5]; 268 | A.insert(i + total_constraint_num / 2 + (N - 1) * 3, 6 * i + 6 * N + 9) = T3[9]; 269 | 270 | lowerBound(i + (N - 1) * 3) = 0; 271 | upperBound(i + (N - 1) * 3) = 0; 272 | lowerBound(i + total_constraint_num / 2 + (N - 1) * 3) = 0; 273 | upperBound(i + total_constraint_num / 2 + (N - 1) * 3) = 0; 274 | } 275 | 276 | //起止点约束-等式约束 277 | A.insert(smooth_constraint_num / 2, 1) = 1; 278 | A.insert(smooth_constraint_num / 2, 6 * N + 1) = -ref_tan_start; 279 | 280 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (N - 1) + 1) = 1; 281 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (N - 1) + 2) = 2; 282 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (N - 1) + 3) = 3; 283 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (N - 1) + 4) = 4; 284 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (N - 1) + 5) = 5; 285 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (2 * N - 1) + 1) = -ref_tan_end * 1; 286 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (2 * N - 1) + 2) = -ref_tan_end * 2; 287 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (2 * N - 1) + 3) = -ref_tan_end * 3; 288 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (2 * N - 1) + 4) = -ref_tan_end * 4; 289 | A.insert((smooth_constraint_num + total_constraint_num) / 2, 6 * (2 * N - 1) + 5) = -ref_tan_end * 5; 290 | 291 | lowerBound(smooth_constraint_num / 2) = 0; 292 | upperBound(smooth_constraint_num / 2) = 0; 293 | lowerBound((smooth_constraint_num + total_constraint_num) / 2) = 0; 294 | upperBound((smooth_constraint_num + total_constraint_num) / 2) = 0; 295 | 296 | //位置bounding box约束-不等式约束 297 | for (int i = 1; i <= sampling_num; i++) 298 | { 299 | // x坐标约束 300 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 0) = T0_05[0]; 301 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 1) = T0_05[1]; 302 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 2) = T0_05[2]; 303 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 3) = T0_05[3]; 304 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 4) = T0_05[4]; 305 | A.insert(smooth_constraint_num / 2 + i, N * (i - 1) + 5) = T0_05[5]; 306 | // y坐标约束 307 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 0 + 36) = T0_05[0]; 308 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 1 + 36) = T0_05[1]; 309 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 2 + 36) = T0_05[2]; 310 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 3 + 36) = T0_05[3]; 311 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 4 + 36) = T0_05[4]; 312 | A.insert((smooth_constraint_num + total_constraint_num) / 2 + i, N * (i - 1) + 5 + 36) = T0_05[5]; 313 | 314 | lowerBound(smooth_constraint_num / 2 + i) = anchor_points[i - 1].first - bounding_box_constraint; 315 | upperBound(smooth_constraint_num / 2 + i) = anchor_points[i - 1].first + bounding_box_constraint; 316 | lowerBound((smooth_constraint_num + total_constraint_num) / 2 + i) = anchor_points[i - 1].second - bounding_box_constraint; 317 | upperBound((smooth_constraint_num + total_constraint_num) / 2 + i) = anchor_points[i - 1].second + bounding_box_constraint; 318 | cout << i << "th x lowerBound: " << lowerBound(smooth_constraint_num / 2 + i) << "::" << anchor_points[i - 1].first - bounding_box_constraint << endl; 319 | cout << i << "th x upperBound: " << upperBound(smooth_constraint_num / 2 + i) << "::" << anchor_points[i - 1].first + bounding_box_constraint << endl; 320 | cout << i << "th y lowerBound: " << lowerBound((smooth_constraint_num + total_constraint_num) / 2 + i) << "::" << anchor_points[i - 1].second - bounding_box_constraint << endl; 321 | cout << i << "th y upperBound: " << upperBound((smooth_constraint_num + total_constraint_num) / 2 + i) << "::" << anchor_points[i - 1].second + bounding_box_constraint << endl; 322 | } 323 | // cout << A << endl; 324 | // cout << lowerBound << endl; 325 | // cout << "---------------------" << endl; 326 | // cout << upperBound << endl; 327 | 328 | OsqpEigen::Solver solver; 329 | 330 | solver.settings()->setVerbosity(false); 331 | solver.settings()->setWarmStart(true); 332 | 333 | solver.data()->setNumberOfVariables(2 * 6 * N); 334 | solver.data()->setNumberOfConstraints(total_constraint_num); 335 | 336 | if (!solver.data()->setHessianMatrix(H)) 337 | return 1; 338 | if (!solver.data()->setGradient(gradient)) 339 | return 1; 340 | if (!solver.data()->setLinearConstraintsMatrix(A)) 341 | return 1; 342 | if (!solver.data()->setLowerBound(lowerBound)) 343 | return 1; 344 | if (!solver.data()->setUpperBound(upperBound)) 345 | return 1; 346 | 347 | if (!solver.initSolver()) 348 | return 1; 349 | 350 | Eigen::VectorXd QPSolution; 351 | 352 | if (!solver.solve()) 353 | { 354 | return 1; 355 | } 356 | QPSolution = solver.getSolution(); 357 | cout << "QPSolution" << endl 358 | << QPSolution << endl; 359 | if (!writeCSVOutput(QPSolution)) 360 | { 361 | cout << "ERROR" << endl; 362 | return 1; 363 | } 364 | spline_curve temp_curve; 365 | vector curve_seg; 366 | cout << "QPSolution.rows():" << QPSolution.rows() << endl; 367 | Eigen::VectorXd px; 368 | Eigen::VectorXd py; 369 | for (int i = 0; i < QPSolution.rows() / N / 2; i++) 370 | { 371 | temp_curve.a0 = QPSolution(6 * i + 0); 372 | temp_curve.a1 = QPSolution(6 * i + 1); 373 | temp_curve.a2 = QPSolution(6 * i + 2); 374 | temp_curve.a3 = QPSolution(6 * i + 3); 375 | temp_curve.a4 = QPSolution(6 * i + 4); 376 | temp_curve.a5 = QPSolution(6 * i + 5); 377 | 378 | temp_curve.b0 = QPSolution(6 * (i + N) + 0); 379 | temp_curve.b1 = QPSolution(6 * (i + N) + 1); 380 | temp_curve.b2 = QPSolution(6 * (i + N) + 2); 381 | temp_curve.b3 = QPSolution(6 * (i + N) + 3); 382 | temp_curve.b4 = QPSolution(6 * (i + N) + 4); 383 | temp_curve.b5 = QPSolution(6 * (i + N) + 5); 384 | 385 | temp_curve.x_start = temp_curve.a0; 386 | temp_curve.y_start = temp_curve.b0; 387 | temp_curve.x_end = temp_curve.a0 + temp_curve.a1 + temp_curve.a2 + temp_curve.a3 + temp_curve.a4 + temp_curve.a5; 388 | temp_curve.y_end = temp_curve.b0 + temp_curve.b1 + temp_curve.b2 + temp_curve.b3 + temp_curve.b4 + temp_curve.b5; 389 | 390 | curve_seg.push_back(temp_curve); 391 | } 392 | 393 | return 0; 394 | } --------------------------------------------------------------------------------