目录
- 0 专栏引见
- 1 多项式插值
- 2 多项式插值轨迹布局
- 3 算法仿真
-
- 3.1 ROS C++仿真
- 3.2 Python仿真
- 3.3 Matlab仿真
0 专栏引见
🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、翻新比赛必备！具体引见全局布局(图搜查、采样法、智能算法等)；部散布局(DWA、APF等)；曲线提升(贝塞尔曲线、B样条曲线等)。
🚀概略：图解智能驾驶中的静止布局(Motion Planning)，附几十种布局算法
1 多项式插值
多项式插值(polynomial interpolation) 基于一元多项式启动曲线插值，可以保障微合成放的延续性，使轨迹平滑、机械冲击小。多项式插值的运行场景十分宽泛，例如
必定指出，当经常使用等距多项式插值时，随着多项式阶数升高，或许形成函数解析区间过小，发生 龙格现象(runge phenomenon) ：插值两端发生猛烈振荡。因此多项式插值普通驳回三次、五次或七次。
2 多项式插值轨迹布局
对门路启动多项式插值时，须要给定机器人在首末位置的位姿以及速度、减速度等微分项作为解放条件。若要求在期间 t 0 t_0 t 0 内经过两个门路点 θ ( 0 ) \boldsymbol{\theta }\left( 0 \right) 0 、 θ ( t 0 ) \boldsymbol{\theta }\left( t_0 \right) t 0 ，再思考首末点的速度解放 θ ˙ ( 0 ) \boldsymbol{\dot{\theta}}\left( 0 \right) 0 、 θ ˙ ( t 0 ) \boldsymbol{\dot{\theta}}\left( t_0 \right) t 0 共四个自在度，由三次多项式可齐全确定
θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 \boldsymbol{\theta }\left( t \right) =\boldsymbol{a}_0+\boldsymbol{a}_1t+\boldsymbol{a}_2t^2+\boldsymbol{a}_3t^3 t a 0 a 1 t a 2 t 2 a 3 t 3
其中多项式系数由位置解放和速度解放确定
[ θ ( 0 ) θ ( t 0 ) θ ˙ ( 0 ) θ ˙ ( t 0 ) ] = [ 1 0 0 0 1 t 0 t 0 2 t 0 3 0 1 0 0 0 1 2 t 0 3 t 0 2 ] [ a 0 a 1 a 2 a 3 ] ⇔ [ a 0 a 1 a 2 a 3 ] = [ θ ( 0 ) θ ˙ ( 0 ) 3 t 0 2 [ θ ( t 0 ) − θ ( 0 ) ] − 2 t 0 θ ˙ ( 0 ) − 1 t 0 θ ˙ ( t 0 ) − 2 t 0 3 [ θ ( t 0 ) − θ ( 0 ) ] + 1 t 0 2 [ θ ˙ ( 0 ) − θ ˙ ( t 0 ) ] ] \left[ \begin{array}{c} \boldsymbol{\theta }\left( 0 \right)\\ \boldsymbol{\theta }\left( t_0 \right)\\ \boldsymbol{\dot{\theta}}\left( 0 \right)\\ \boldsymbol{\dot{\theta}}\left( t_0 \right)\\\end{array} \right] =\left[ \begin{matrix} 1& 0& 0& 0\\ 1& t_0& t_{0}^{2}& t_{0}^{3}\\ 0& 1& 0& 0\\ 0& 1& 2t_0& 3t_{0}^{2}\\\end{matrix} \right] \left[ \begin{array}{c} \boldsymbol{a}_0\\ \boldsymbol{a}_1\\ \boldsymbol{a}_2\\ \boldsymbol{a}_3\\\end{array} \right] \\ \Leftrightarrow \left[ \begin{array}{c} \boldsymbol{a}_0\\ \boldsymbol{a}_1\\ \boldsymbol{a}_2\\ \boldsymbol{a}_3\\\end{array} \right] =\left[ \begin{array}{c} \boldsymbol{\theta }\left( 0 \right)\\ \boldsymbol{\dot{\theta}}\left( 0 \right)\\ \frac{3}{t_{0}^{2}}\left[ \boldsymbol{\theta }\left( t_0 \right) -\boldsymbol{\theta }\left( 0 \right) \right] -\frac{2}{t_0}\boldsymbol{\dot{\theta}}\left( 0 \right) -\frac{1}{t_0}\boldsymbol{\dot{\theta}}\left( t_0 \right)\\ -\frac{2}{t_{0}^{3}}\left[ \boldsymbol{\theta }\left( t_0 \right) -\boldsymbol{\theta }\left( 0 \right) \right] +\frac{1}{t_{0}^{2}}\left[ \boldsymbol{\dot{\theta}}\left( 0 \right) -\boldsymbol{\dot{\theta}}\left( t_0 \right) \right]\\\end{array} \right] 0 t 0 0 t 0 1 1 0 0 0 t 0 1 1 0 t 0 2 0 2 t 0 0 t 0 3 0 3 t 0 2 a 0 a 1 a 2 a 3 a 0 a 1 a 2 a 3 0 0 t 0 2 3 t 0 0 t 0 2 0 t 0 1 t 0 t 0 3 2 t 0 0 t 0 2 1 0 t 0
再思考首末点的减速度解放 θ ¨ ( 0 ) \boldsymbol{\ddot{\theta}}\left( 0 \right) 0 、 θ ¨ ( t 0 ) \boldsymbol{\ddot{\theta}}\left( t_0 \right) t 0 共六个自在度，由五次多项式曲线可齐全确定
θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 \boldsymbol{\theta }\left( t \right) =\boldsymbol{a}_0+\boldsymbol{a}_1t+\boldsymbol{a}_2t^2+\boldsymbol{a}_3t^3+\boldsymbol{a}_4t^4+\boldsymbol{a}_5t^5 t a 0 a 1 t a 2 t 2 a 3 t 3 a 4 t 4 a 5 t 5
其中多项式系数由位置解放、速度解放和减速度解放确定
[ θ ( 0 ) θ ( t 0 ) θ ˙ ( 0 ) θ ˙ ( t 0 ) θ ¨ ( 0 ) θ ¨ ( t 0 ) ] = [ l 1 0 0 0 0 0 1 t 0 t 0 2 t 0 3 t 0 4 t 0 5 0 1 0 0 0 0 0 1 2 t 0 3 t 0 2 4 t 0 3 5 t 0 4 0 0 2 0 0 0 0 0 2 6 t 0 12 t 0 2 20 t 0 3 ] [ a 0 a 1 a 2 a 3 a 4 a 5 ] ⇔ [ a 0 a 1 a 2 a 3 a 4 a 5 ] = [ θ ( 0 ) θ ˙ ( 0 ) 1 2 θ ¨ ( 0 ) 10 t 0 3 [ θ ( t 0 ) − θ ( 0 ) ] − 1 t 0 2 [ 4 θ ˙ ( t 0 ) + 6 θ ˙ ( 0 ) ] + 1 2 t 0 [ θ ¨ ( t 0 ) − 3 θ ¨ ( 0 ) ] − 15 t 0 4 [ θ ( t 0 ) − θ ( 0 ) ] + 1 t 0 3 [ 7 θ ˙ ( t 0 ) + 8 θ ˙ ( 0 ) ] − 1 2 t 0 2 [ 2 θ ¨ ( t 0 ) − 3 θ ¨ ( 0 ) ] 6 t 0 5 [ θ ( t 0 ) − θ ( 0 ) ] − 1 t 0 4 [ 3 θ ˙ ( t 0 ) + 3 θ ˙ ( 0 ) ] + 1 2 t 0 3 [ θ ¨ ( t 0 ) − θ ¨ ( 0 ) ] ] \left[ \begin{array}{c} \boldsymbol{\theta }\left( 0 \right)\\ \boldsymbol{\theta }\left( t_0 \right)\\ \boldsymbol{\dot{\theta}}\left( 0 \right)\\ \boldsymbol{\dot{\theta}}\left( t_0 \right)\\ \boldsymbol{\ddot{\theta}}\left( 0 \right)\\ \boldsymbol{\ddot{\theta}}\left( t_0 \right)\\\end{array} \right] =\left[ \begin{matrix}{l} 1& 0& 0& 0& 0& 0\\ 1& t_0& t_{0}^{2}& t_{0}^{3}& t_{0}^{4}& t_{0}^{5}\\ 0& 1& 0& 0& 0& 0\\ 0& 1& 2t_0& 3t_{0}^{2}& 4t_{0}^{3}& 5t_{0}^{4}\\ 0& 0& 2& 0& 0& 0\\ 0& 0& 2& 6t_0& 12t_{0}^{2}& 20t_{0}^{3}\\\end{matrix} \right] \left[ \begin{array}{c} \boldsymbol{a}_0\\ \boldsymbol{a}_1\\ \boldsymbol{a}_2\\ \boldsymbol{a}_3\\ \boldsymbol{a}_4\\ \boldsymbol{a}_5\\\end{array} \right]\\ \Leftrightarrow \left[ \begin{array}{c} \boldsymbol{a}_0\\ \boldsymbol{a}_1\\ \boldsymbol{a}_2\\ \boldsymbol{a}_3\\ \boldsymbol{a}_4\\ \boldsymbol{a}_5\\\end{array} \right] =\left[ \begin{array}{c} \boldsymbol{\theta }\left( 0 \right)\\ \boldsymbol{\dot{\theta}}\left( 0 \right)\\ \frac{1}{2}\boldsymbol{\ddot{\theta}}\left( 0 \right)\\ \frac{10}{t_{0}^{3}}\left[ \boldsymbol{\theta }\left( t_0 \right) -\boldsymbol{\theta }\left( 0 \right) \right] -\frac{1}{t_{0}^{2}}\left[ 4\boldsymbol{\dot{\theta}}\left( t_0 \right) +6\boldsymbol{\dot{\theta}}\left( 0 \right) \right] +\frac{1}{2t_0}\left[ \boldsymbol{\ddot{\theta}}\left( t_0 \right) -3\boldsymbol{\ddot{\theta}}\left( 0 \right) \right]\\ \frac{-15}{t_{0}^{4}}\left[ \boldsymbol{\theta }\left( t_0 \right) -\boldsymbol{\theta }\left( 0 \right) \right] +\frac{1}{t_{0}^{3}}\left[ 7\boldsymbol{\dot{\theta}}\left( t_0 \right) +8\boldsymbol{\dot{\theta}}\left( 0 \right) \right] -\frac{1}{2t_{0}^{2}}\left[ 2\boldsymbol{\ddot{\theta}}\left( t_0 \right) -3\boldsymbol{\ddot{\theta}}\left( 0 \right) \right]\\ \frac{6}{t_{0}^{5}}\left[ \boldsymbol{\theta }\left( t_0 \right) -\boldsymbol{\theta }\left( 0 \right) \right] -\frac{1}{t_{0}^{4}}\left[ 3\boldsymbol{\dot{\theta}}\left( t_0 \right) +3\boldsymbol{\dot{\theta}}\left( 0 \right) \right] +\frac{1}{2t_{0}^{3}}\left[ \boldsymbol{\ddot{\theta}}\left( t_0 \right) -\boldsymbol{\ddot{\theta}}\left( 0 \right) \right]\\\end{array} \right]
0
t
0
0
t
0
0
t
0
l
1
1
0
0
0
0
0
t
0
1
1
0
0
0
t
0
2
0
2
t
0
2
2
0
t
0
3
0
3
t
0
2
0
6
t
0
0
t
0
4
0
4
t
0
3
0
12
t
0
2
0
t
0
5
0
5
t
0
4
0
20
t
0
3
a
0
a
1
a
2
a
3
a
4
a
5
a
0
a
1
a
2
a
3
a
4
a
5
0
0
2
1
0
t
0
3
10
t
0
0
t
0
2
1
4
t
0
6
0
2
t
0
1
t
0
3
0
t
0
4
15
t
0
0
t
0
3
1
7
t
0
8
0
2
t
0
2
1
2
t
0
3
0
t
0
5
6
t
0
0
t
0
4
1
3
t
0
3
0
2
t
0
3
1
t
0
0
若思考更多期间微合成放，则须要更高次数的多项式曲线，但多项式阶数的升高会影响计算效率，从而降落实时性。
3 算法仿真
3.1 ROS C++仿真
外围代码如下所示
PolyPolystdtupledouble double double state_0 stdtupledouble double double state_1 double tdouble x0 v0 a0double xt vt atstdtiex0 v0 a0 state_0stdtiext vt at state_1EigenMatrix3d AA stdpowt 3 stdpowt 4 stdpowt 5 3 stdpowt 2 4 stdpowt 3 5 stdpowt 46 t 12 stdpowt 2 20 stdpowt 3EigenVector3d bxt x0 v0 t a0 t t 2 vt v0 a0 t at a0EigenVector3d x Alusolveb// Quintic polynomial coefficientp0 x0p1 v0p2 a0 2.0p3 x0p4 x1p5 x2这部分红功的是五次多项式的系数计算
在轨迹布局环节中，只有要遍历门路点，在相邻门路点间启动插值即可
bool Polynomialrunconst Poses2d points Points2d pathif pointssize 4return falseelsepathclear// generate velocity and acceleration constraints heuristicallystdvectordouble vpointssize 1.0v0 0.0stdvectordouble afor size_t i 0 i pointssize 1 iapush_backvi 1 vi 5apush_back0.0for size_t i 0 i pointssize 1 iPolyTrajectory trajPolyState startstdget0pointsi stdget1pointsi stdget2pointsi vi aiPolyState goalstdget0pointsi 1 stdget1pointsi 1 stdget2pointsi 1 vi 1ai 1generationstart goal trajPoints2d path_i trajtoPathpathinsertpathend path_ibegin path_iendreturn pathempty3.2 Python仿真
外围代码如下所示
class Poly'''Polynomial interpolation solver'''def __init__self state0 tuple state1 tuple t float Nonex0 v0 a0 state0xt vt at state1A nparrayt 3 t 4 t 53 t 2 4 t 3 5 t 46 t 12 t 2 20 t 3b nparrayxt x0 v0 t a0 t 2 2vt v0 a0 tat a0X nplinalgsolveA b# Quintic polynomial coefficientselfp0 x0selfp1 v0selfp2 a0 2.0selfp3 X0selfp4 X1selfp5 X2
3.3 Matlab仿真
外围代码如下所示
for T paramt_min paramstep paramt_maxA powerT 3 powerT 4 powerT 53 powerT 2 4 powerT 3 5 powerT 46 T 12 powerT 2 20 powerT 3b gx sx sv_x T sa_x T T 2gv_x sv_x sa_x Tga_x sa_xX A bpx sx sv_x sa_x 2 X1 X2 X3b gy sy sv_y T sa_y T T 2gv_y sv_y sa_y Tga_y sa_yX A bpy sy sv_y sa_y 2 X1 X2 X3for t0 paramdt T paramdttraj_x traj_x xpx ttraj_y traj_y xpy tvx dxpx t vy dxpy ttraj_v traj_v hypotvx vyax ddxpx t ay ddxpy ta hypotax ayif lengthtraj_v 2 traj_vend traj_vend 1a aendtraj_a traj_a ajx dddxpx t jy dddxpy tj hypotjx jyif lengthtraj_a 2 traj_aend traj_aend 1j jendtraj_j traj_j jendif maxabstraj_a parammax_acc maxabstraj_j parammax_jerkbreakelsetraj_x traj_y traj_v traj_a traj_j endend
完整工程代码请咨询下方博主名片失掉
🔥 更多精彩专栏 ：
- 《ROS从入门到知晓》
- 《Pytorch深度学习实战》
- 《机器学习强基方案》
- 《静止布局实战精讲》
还没有评论，来说两句吧...