0
0

自动驾驶规划 - 5次多项式拟合

简介

自动驾驶运动规划中会用到各种曲线，主要用于生成车辆的轨迹，常见的轨迹生成算法，如贝塞尔曲线，样条曲线，以及apollo EM Planner的五次多项式曲线，城市场景中使用的是分段多项式曲线，在EM Planner和Lattice Planner中思路是，都是先通过动态规划生成点，再用5次多项式生成曲线连接两个点（虽然后面的版本改动很大，至少lattice planner目前还是这个方法）。

在这里可以看出5次多项式的作用，就是生成轨迹，这里的轨迹不一定是车行驶的轨迹，比如S—T图中的线，是用来做速度规划的。****如下图：

请添加图片描述

在apollo里面用到了，3-5次多项式，

  • cubic_polynomial_curve1d，三次多项式曲线
  • quartic_polynomial_curve1d，四次多项式曲线-
  • quintic_polynomial_curve1d，五次多项式曲线

3次多项式

  1.       f 
  2.      
  3.     
  4.       ( 
  5.      
  6.     
  7.       x 
  8.      
  9.     
  10.       ) 
  11.      
  12.     
  13.       = 
  14.      
  15.      
  16.      
  17.        C 
  18.       
  19.      
  20.        0 
  21.       
  22.      
  23.     
  24.       + 
  25.      
  26.      
  27.      
  28.        C 
  29.       
  30.      
  31.        1 
  32.       
  33.      
  34.     
  35.       x 
  36.      
  37.     
  38.       + 
  39.      
  40.      
  41.      
  42.        C 
  43.       
  44.      
  45.        2 
  46.       
  47.      
  48.      
  49.      
  50.        x 
  51.       
  52.      
  53.        2 
  54.       
  55.      
  56.     
  57.       + 
  58.      
  59.      
  60.      
  61.        C 
  62.       
  63.      
  64.        3 
  65.       
  66.      
  67.      
  68.      
  69.        x 
  70.       
  71.      
  72.        3 
  73.       
  74.      
  75.     
  76.       − 
  77.      
  78.     
  79.       − 
  80.      
  81.     
  82.       − 
  83.      
  84.     
  85.       − 
  86.      
  87.     
  88.       ( 
  89.      
  90.     
  91.       1 
  92.      
  93.     
  94.       ) 
  95.      
  96.     
  97.    
  98.      f(x) = C_0 + C_1x + C_2x^2+ C_3x^3 ----(1) 
  99.     
  100.    
  101.  f(x)=C0​+C1​x+C2​x2+C3​x3−−−−(1)

对x求导====>

  1.        f 
  2.       
  3.      
  4.        ′ 
  5.       
  6.      
  7.     
  8.       ( 
  9.      
  10.     
  11.       x 
  12.      
  13.     
  14.       ) 
  15.      
  16.     
  17.       = 
  18.      
  19.      
  20.      
  21.        C 
  22.       
  23.      
  24.        1 
  25.       
  26.      
  27.     
  28.       + 
  29.      
  30.     
  31.       2 
  32.      
  33.      
  34.      
  35.        C 
  36.       
  37.      
  38.        2 
  39.       
  40.      
  41.     
  42.       x 
  43.      
  44.     
  45.       + 
  46.      
  47.     
  48.       3 
  49.      
  50.      
  51.      
  52.        C 
  53.       
  54.      
  55.        3 
  56.       
  57.      
  58.      
  59.      
  60.        x 
  61.       
  62.      
  63.        2 
  64.       
  65.      
  66.     
  67.    
  68.      f'(x) = C_1 + 2C_2x+ 3C_3x^2 
  69.     
  70.    
  71.  f′(x)=C1​+2C2​x+3C3​x2
  73.   
  74.    
  75.     
  76.     
  77.       f 
  78.      
  79.     
  80.       ( 
  81.      
  82.     
  83.       x 
  84.      
  85.     
  86.       ) 
  87.      
  88.     
  89.       = 
  90.      
  91.     
  92.       2 
  93.      
  94.      
  95.      
  96.        C 
  97.       
  98.      
  99.        2 
  100.       
  101.      
  102.     
  103.       + 
  104.      
  105.     
  106.       6 
  107.      
  108.      
  109.      
  110.        C 
  111.       
  112.      
  113.        3 
  114.       
  115.      
  116.     
  117.       x 
  118.      
  119.     
  120.    
  121.      f(x) = 2C_2+ 6C_3x 
  122.     
  123.    
  124.  f(x)=2C2​+6C3​x

因为是连接两个已知点，所以两个轨迹点的信息是已知的。
如已知，当x=0，代如得：

  1.       f 
  2.      
  3.     
  4.       ( 
  5.      
  6.     
  7.       0 
  8.      
  9.     
  10.       ) 
  11.      
  12.     
  13.       = 
  14.      
  15.      
  16.      
  17.        C 
  18.       
  19.      
  20.        0 
  21.       
  22.      
  23.     
  24.       ； 
  25.      
  26.      
  27.      
  28.        f 
  29.       
  30.      
  31.        ′ 
  32.       
  33.      
  34.     
  35.       ( 
  36.      
  37.     
  38.       0 
  39.      
  40.     
  41.       ) 
  42.      
  43.     
  44.       = 
  45.      
  46.      
  47.      
  48.        C 
  49.       
  50.      
  51.        1 
  52.       
  53.      
  54.     
  55.       ； 
  56.      
  57.      
  58.      
  59.        f 
  60.       
  61.       
  62.       
  63.         ′ 
  64.        
  65.       
  66.         ′ 
  67.        
  68.       
  69.      
  70.     
  71.       ( 
  72.      
  73.     
  74.       0 
  75.      
  76.     
  77.       ) 
  78.      
  79.     
  80.       = 
  81.      
  82.     
  83.       2 
  84.      
  85.      
  86.      
  87.        C 
  88.       
  89.      
  90.        2 
  91.       
  92.      
  93.     
  94.    
  95.      f(0) = C_0 ； f'(0) = C_1；f''(0) =2C_2 
  96.     
  97.    
  98.  f(0)=C0​；f′(0)=C1​；f′′(0)=2C2​

所以

  1.        C 
  2.       
  3.      
  4.        0 
  5.       
  6.      
  7.     
  8.       = 
  9.      
  10.     
  11.       f 
  12.      
  13.     
  14.       ( 
  15.      
  16.     
  17.       0 
  18.      
  19.     
  20.       ) 
  21.      
  22.     
  23.       ； 
  24.      
  25.      
  26.      
  27.        C 
  28.       
  29.      
  30.        1 
  31.       
  32.      
  33.     
  34.       = 
  35.      
  36.      
  37.      
  38.        f 
  39.       
  40.      
  41.        ′ 
  42.       
  43.      
  44.     
  45.       ( 
  46.      
  47.     
  48.       0 
  49.      
  50.     
  51.       ) 
  52.      
  53.     
  54.       ； 
  55.      
  56.      
  57.      
  58.        C 
  59.       
  60.      
  61.        2 
  62.       
  63.      
  64.     
  65.       = 
  66.      
  67.      
  68.      
  69.        f 
  70.       
  71.       
  72.       
  73.         ′ 
  74.        
  75.       
  76.         ′ 
  77.        
  78.       
  79.      
  80.     
  81.       ( 
  82.      
  83.     
  84.       0 
  85.      
  86.     
  87.       ) 
  88.      
  89.     
  90.       / 
  91.      
  92.     
  93.       2 
  94.      
  95.     
  96.    
  97.      C_0 = f(0)；C_1 = f'(0)； C_2 = f''(0)/2 
  98.     
  99.    
  100.  C0​=f(0)；C1​=f′(0)；C2​=f′′(0)/2

将终点 (X_end，f(X_end))带入得到C_3。

  1.        C 
  2.       
  3.      
  4.        3 
  5.       
  6.      
  7.     
  8.       = 
  9.      
  10.     
  11.       ( 
  12.      
  13.     
  14.       f 
  15.      
  16.     
  17.       ( 
  18.      
  19.      
  20.      
  21.        x 
  22.       
  23.      
  24.        p 
  25.       
  26.      
  27.     
  28.       ) 
  29.      
  30.     
  31.       − 
  32.      
  33.      
  34.      
  35.        C 
  36.       
  37.      
  38.        0 
  39.       
  40.      
  41.     
  42.       − 
  43.      
  44.      
  45.      
  46.        C 
  47.       
  48.      
  49.        1 
  50.       
  51.      
  52.      
  53.      
  54.        x 
  55.       
  56.      
  57.        p 
  58.       
  59.      
  60.     
  61.       − 
  62.      
  63.      
  64.      
  65.        C 
  66.       
  67.      
  68.        2 
  69.       
  70.      
  71.      
  72.      
  73.        x 
  74.       
  75.      
  76.        p 
  77.       
  78.      
  79.        2 
  80.       
  81.      
  82.     
  83.       ) 
  84.      
  85.     
  86.       / 
  87.      
  88.      
  89.      
  90.        x 
  91.       
  92.      
  93.        p 
  94.       
  95.      
  96.        3 
  97.       
  98.      
  99.     
  100.    
  101.      C_3 = (f(x_p)-C_0-C_1x_p-C_2x_p^2)/x_p^3 
  102.     
  103.    
  104.  C3​=(f(xp​)−C0​−C1​xp​−C2​xp2​)/xp3​

（1）式的所有未知参数都求出，代入x可以得到两点间的轨迹。

apollo中3次多项式的计算

  1. // cubic_polynomial_curve1d.cc
  2. CubicPolynomialCurve1d::CubicPolynomialCurve1d(constdouble x0,constdouble dx0,constdouble ddx0,constdouble x1,constdouble param){ComputeCoefficients(x0, dx0, ddx0, x1, param);
  3.   param_ = param;
  4.   start_condition_[0]= x0;
  5.   start_condition_[1]= dx0;
  6.   start_condition_[2]= ddx0;
  7.   end_condition_ = x1;}void CubicPolynomialCurve1d::ComputeCoefficients(constdouble x0,constdouble dx0,constdouble ddx0,constdouble x1,constdouble param){DCHECK(param >0.0);constdouble p2 = param * param;constdouble p3 = param * p2;
  8.   coef_[0]= x0;
  9.   coef_[1]= dx0;
  10.   coef_[2]=0.5* ddx0;
  11.   coef_[3]=(x1 - x0 - dx0 * param - coef_[2]* p2)/ p3;}

5次多项式

  1.       f 
  2.      
  3.     
  4.       ( 
  5.      
  6.     
  7.       x 
  8.      
  9.     
  10.       ) 
  11.      
  12.     
  13.       = 
  14.      
  15.      
  16.      
  17.        C 
  18.       
  19.      
  20.        0 
  21.       
  22.      
  23.     
  24.       + 
  25.      
  26.      
  27.      
  28.        C 
  29.       
  30.      
  31.        1 
  32.       
  33.      
  34.     
  35.       x 
  36.      
  37.     
  38.       + 
  39.      
  40.      
  41.      
  42.        C 
  43.       
  44.      
  45.        2 
  46.       
  47.      
  48.      
  49.      
  50.        x 
  51.       
  52.      
  53.        2 
  54.       
  55.      
  56.     
  57.       + 
  58.      
  59.      
  60.      
  61.        C 
  62.       
  63.      
  64.        3 
  65.       
  66.      
  67.      
  68.      
  69.        x 
  70.       
  71.      
  72.        3 
  73.       
  74.      
  75.     
  76.       + 
  77.      
  78.      
  79.      
  80.        C 
  81.       
  82.      
  83.        4 
  84.       
  85.      
  86.      
  87.      
  88.        x 
  89.       
  90.      
  91.        4 
  92.       
  93.      
  94.     
  95.       + 
  96.      
  97.      
  98.      
  99.        C 
  100.       
  101.      
  102.        5 
  103.       
  104.      
  105.      
  106.      
  107.        x 
  108.       
  109.      
  110.        5 
  111.       
  112.      
  113.     
  114.       − 
  115.      
  116.     
  117.       − 
  118.      
  119.     
  120.       − 
  121.      
  122.     
  123.       − 
  124.      
  125.     
  126.       ( 
  127.      
  128.     
  129.       3 
  130.      
  131.     
  132.       − 
  133.      
  134.     
  135.       1 
  136.      
  137.     
  138.       ) 
  139.      
  140.     
  141.    
  142.      f(x) = C_0 + C_1x + C_2x^2+ C_3x^3+C_4x^4+C_5x^5 ----(3-1) 
  143.     
  144.    
  145.  f(x)=C0​+C1​x+C2​x2+C3​x3+C4​x4+C5​x5−−−−(3−1)

对x求导====>

  1.        f 
  2.       
  3.      
  4.        ′ 
  5.       
  6.      
  7.     
  8.       ( 
  9.      
  10.     
  11.       x 
  12.      
  13.     
  14.       ) 
  15.      
  16.     
  17.       = 
  18.      
  19.      
  20.      
  21.        C 
  22.       
  23.      
  24.        1 
  25.       
  26.      
  27.     
  28.       + 
  29.      
  30.     
  31.       2 
  32.      
  33.      
  34.      
  35.        C 
  36.       
  37.      
  38.        2 
  39.       
  40.      
  41.     
  42.       x 
  43.      
  44.     
  45.       + 
  46.      
  47.     
  48.       3 
  49.      
  50.      
  51.      
  52.        C 
  53.       
  54.      
  55.        3 
  56.       
  57.      
  58.      
  59.      
  60.        x 
  61.       
  62.      
  63.        2 
  64.       
  65.      
  66.     
  67.       + 
  68.      
  69.     
  70.       4 
  71.      
  72.      
  73.      
  74.        C 
  75.       
  76.      
  77.        4 
  78.       
  79.      
  80.      
  81.      
  82.        x 
  83.       
  84.      
  85.        3 
  86.       
  87.      
  88.     
  89.       + 
  90.      
  91.     
  92.       5 
  93.      
  94.      
  95.      
  96.        C 
  97.       
  98.      
  99.        5 
  100.       
  101.      
  102.      
  103.      
  104.        x 
  105.       
  106.      
  107.        4 
  108.       
  109.      
  110.     
  111.       − 
  112.      
  113.     
  114.       − 
  115.      
  116.     
  117.       − 
  118.      
  119.     
  120.       − 
  121.      
  122.     
  123.       ( 
  124.      
  125.     
  126.       3 
  127.      
  128.     
  129.       − 
  130.      
  131.     
  132.       2 
  133.      
  134.     
  135.       ) 
  136.      
  137.     
  138.    
  139.      f'(x) = C_1 + 2C_2x+ 3C_3x^2+4C_4x^3+5C_5x^4----(3-2) 
  140.     
  141.    
  142.  f′(x)=C1​+2C2​x+3C3​x2+4C4​x3+5C5​x4−−−−(3−2)
  145.   
  146.    
  147.     
  148.      
  149.      
  150.        f 
  151.       
  152.       
  153.       
  154.         ′ 
  155.        
  156.       
  157.         ′ 
  158.        
  159.       
  160.      
  161.     
  162.       ( 
  163.      
  164.     
  165.       x 
  166.      
  167.     
  168.       ) 
  169.      
  170.     
  171.       = 
  172.      
  173.     
  174.       2 
  175.      
  176.      
  177.      
  178.        C 
  179.       
  180.      
  181.        2 
  182.       
  183.      
  184.     
  185.       + 
  186.      
  187.     
  188.       6 
  189.      
  190.      
  191.      
  192.        C 
  193.       
  194.      
  195.        3 
  196.       
  197.      
  198.     
  199.       x 
  200.      
  201.     
  202.       + 
  203.      
  204.     
  205.       12 
  206.      
  207.      
  208.      
  209.        C 
  210.       
  211.      
  212.        4 
  213.       
  214.      
  215.      
  216.      
  217.        x 
  218.       
  219.      
  220.        2 
  221.       
  222.      
  223.     
  224.       + 
  225.      
  226.     
  227.       20 
  228.      
  229.      
  230.      
  231.        C 
  232.       
  233.      
  234.        5 
  235.       
  236.      
  237.      
  238.      
  239.        x 
  240.       
  241.      
  242.        3 
  243.       
  244.      
  245.     
  246.       − 
  247.      
  248.     
  249.       − 
  250.      
  251.     
  252.       − 
  253.      
  254.     
  255.       − 
  256.      
  257.     
  258.       ( 
  259.      
  260.     
  261.       3 
  262.      
  263.     
  264.       − 
  265.      
  266.     
  267.       3 
  268.      
  269.     
  270.       ) 
  271.      
  272.     
  273.    
  274.      f''(x) = 2C_2+ 6C_3x+12C_4x^2+20C_5x^3----(3-3) 
  275.     
  276.    
  277.  f′′(x)=2C2​+6C3​x+12C4​x2+20C5​x3−−−−(3−3)

因为是连接两个已知点，所以两个轨迹点的信息是已知的。
如已知，当x=0，代如得：

  1.       f 
  2.      
  3.     
  4.       ( 
  5.      
  6.     
  7.       0 
  8.      
  9.     
  10.       ) 
  11.      
  12.     
  13.       = 
  14.      
  15.      
  16.      
  17.        C 
  18.       
  19.      
  20.        0 
  21.       
  22.      
  23.     
  24.       ； 
  25.      
  26.      
  27.      
  28.        f 
  29.       
  30.      
  31.        ′ 
  32.       
  33.      
  34.     
  35.       ( 
  36.      
  37.     
  38.       0 
  39.      
  40.     
  41.       ) 
  42.      
  43.     
  44.       = 
  45.      
  46.      
  47.      
  48.        C 
  49.       
  50.      
  51.        1 
  52.       
  53.      
  54.     
  55.       ； 
  56.      
  57.      
  58.      
  59.        f 
  60.       
  61.       
  62.       
  63.         ′ 
  64.        
  65.       
  66.         ′ 
  67.        
  68.       
  69.      
  70.     
  71.       ( 
  72.      
  73.     
  74.       0 
  75.      
  76.     
  77.       ) 
  78.      
  79.     
  80.       = 
  81.      
  82.     
  83.       2 
  84.      
  85.      
  86.      
  87.        C 
  88.       
  89.      
  90.        2 
  91.       
  92.      
  93.     
  94.    
  95.      f(0) = C_0 ； f'(0) = C_1；f''(0) =2C_2 
  96.     
  97.    
  98.  f(0)=C0​；f′(0)=C1​；f′′(0)=2C2​

所以

  1.        C 
  2.       
  3.      
  4.        0 
  5.       
  6.      
  7.     
  8.       = 
  9.      
  10.     
  11.       f 
  12.      
  13.     
  14.       ( 
  15.      
  16.     
  17.       0 
  18.      
  19.     
  20.       ) 
  21.      
  22.     
  23.       ； 
  24.      
  25.      
  26.      
  27.        C 
  28.       
  29.      
  30.        1 
  31.       
  32.      
  33.     
  34.       = 
  35.      
  36.      
  37.      
  38.        f 
  39.       
  40.      
  41.        ′ 
  42.       
  43.      
  44.     
  45.       ( 
  46.      
  47.     
  48.       0 
  49.      
  50.     
  51.       ) 
  52.      
  53.     
  54.       ； 
  55.      
  56.      
  57.      
  58.        C 
  59.       
  60.      
  61.        2 
  62.       
  63.      
  64.     
  65.       = 
  66.      
  67.      
  68.      
  69.        f 
  70.       
  71.       
  72.       
  73.         ′ 
  74.        
  75.       
  76.         ′ 
  77.        
  78.       
  79.      
  80.     
  81.       ( 
  82.      
  83.     
  84.       0 
  85.      
  86.     
  87.       ) 
  88.      
  89.     
  90.       / 
  91.      
  92.     
  93.       2 
  94.      
  95.     
  96.    
  97.      C_0 = f(0)；C_1 = f'(0)； C_2 = f''(0)/2 
  98.     
  99.    
  100.  C0​=f(0)；C1​=f′(0)；C2​=f′′(0)/2

将终点 **(x_e，f(x_e))带入 (3-1) (3-2) (3-3)**。

得到

  1.       f 
  2.      
  3.     
  4.       ( 
  5.      
  6.      
  7.      
  8.        x 
  9.       
  10.      
  11.        e 
  12.       
  13.      
  14.     
  15.       ) 
  16.      
  17.     
  18.       = 
  19.      
  20.      
  21.      
  22.        C 
  23.       
  24.      
  25.        0 
  26.       
  27.      
  28.     
  29.       + 
  30.      
  31.      
  32.      
  33.        C 
  34.       
  35.      
  36.        1 
  37.       
  38.      
  39.      
  40.      
  41.        x 
  42.       
  43.      
  44.        e 
  45.       
  46.      
  47.     
  48.       + 
  49.      
  50.      
  51.      
  52.        C 
  53.       
  54.      
  55.        2 
  56.       
  57.      
  58.      
  59.      
  60.        x 
  61.       
  62.      
  63.        e 
  64.       
  65.      
  66.        2 
  67.       
  68.      
  69.     
  70.       + 
  71.      
  72.      
  73.      
  74.        C 
  75.       
  76.      
  77.        3 
  78.       
  79.      
  80.      
  81.      
  82.        x 
  83.       
  84.      
  85.        e 
  86.       
  87.      
  88.        3 
  89.       
  90.      
  91.     
  92.       + 
  93.      
  94.      
  95.      
  96.        C 
  97.       
  98.      
  99.        4 
  100.       
  101.      
  102.      
  103.      
  104.        x 
  105.       
  106.      
  107.        e 
  108.       
  109.      
  110.        4 
  111.       
  112.      
  113.     
  114.       + 
  115.      
  116.      
  117.      
  118.        C 
  119.       
  120.      
  121.        5 
  122.       
  123.      
  124.      
  125.      
  126.        x 
  127.       
  128.      
  129.        e 
  130.       
  131.      
  132.        5 
  133.       
  134.      
  135.     
  136.       − 
  137.      
  138.     
  139.       − 
  140.      
  141.     
  142.       − 
  143.      
  144.     
  145.       − 
  146.      
  147.     
  148.       ( 
  149.      
  150.     
  151.       3 
  152.      
  153.     
  154.       − 
  155.      
  156.     
  157.       4 
  158.      
  159.     
  160.       ) 
  161.      
  162.     
  163.    
  164.      f(x_e) = C_0 + C_1x_e + C_2x_e^2+ C_3x_e^3+C_4x_e^4+C_5x_e^5 ----(3-4) 
  165.     
  166.    
  167.  f(xe​)=C0​+C1​xe​+C2​xe2​+C3​xe3​+C4​xe4​+C5​xe5​−−−−(3−4)

对x求导====>

  1.        f 
  2.       
  3.      
  4.        ′ 
  5.       
  6.      
  7.     
  8.       ( 
  9.      
  10.     
  11.       x 
  12.      
  13.     
  14.       ) 
  15.      
  16.     
  17.       = 
  18.      
  19.      
  20.      
  21.        C 
  22.       
  23.      
  24.        1 
  25.       
  26.      
  27.     
  28.       + 
  29.      
  30.     
  31.       2 
  32.      
  33.      
  34.      
  35.        C 
  36.       
  37.      
  38.        2 
  39.       
  40.      
  41.      
  42.      
  43.        x 
  44.       
  45.      
  46.        e 
  47.       
  48.      
  49.     
  50.       + 
  51.      
  52.     
  53.       3 
  54.      
  55.      
  56.      
  57.        C 
  58.       
  59.      
  60.        3 
  61.       
  62.      
  63.      
  64.      
  65.        x 
  66.       
  67.      
  68.        e 
  69.       
  70.      
  71.        2 
  72.       
  73.      
  74.     
  75.       + 
  76.      
  77.     
  78.       4 
  79.      
  80.      
  81.      
  82.        C 
  83.       
  84.      
  85.        4 
  86.       
  87.      
  88.      
  89.      
  90.        x 
  91.       
  92.      
  93.        e 
  94.       
  95.      
  96.        3 
  97.       
  98.      
  99.     
  100.       + 
  101.      
  102.     
  103.       5 
  104.      
  105.      
  106.      
  107.        C 
  108.       
  109.      
  110.        5 
  111.       
  112.      
  113.      
  114.      
  115.        x 
  116.       
  117.      
  118.        e 
  119.       
  120.      
  121.        4 
  122.       
  123.      
  124.     
  125.       − 
  126.      
  127.     
  128.       − 
  129.      
  130.     
  131.       − 
  132.      
  133.     
  134.       − 
  135.      
  136.     
  137.       ( 
  138.      
  139.     
  140.       3 
  141.      
  142.     
  143.       − 
  144.      
  145.     
  146.       5 
  147.      
  148.     
  149.       ) 
  150.      
  151.     
  152.    
  153.      f'(x) = C_1 + 2C_2x_e+ 3C_3x_e^2+4C_4x_e^3+5C_5x_e^4----(3-5) 
  154.     
  155.    
  156.  f′(x)=C1​+2C2​xe​+3C3​xe2​+4C4​xe3​+5C5​xe4​−−−−(3−5)
  159.   
  160.    
  161.     
  162.      
  163.      
  164.        f 
  165.       
  166.       
  167.       
  168.         ′ 
  169.        
  170.       
  171.         ′ 
  172.        
  173.       
  174.      
  175.     
  176.       ( 
  177.      
  178.     
  179.       x 
  180.      
  181.     
  182.       ) 
  183.      
  184.     
  185.       = 
  186.      
  187.     
  188.       2 
  189.      
  190.      
  191.      
  192.        C 
  193.       
  194.      
  195.        2 
  196.       
  197.      
  198.     
  199.       + 
  200.      
  201.     
  202.       6 
  203.      
  204.      
  205.      
  206.        C 
  207.       
  208.      
  209.        3 
  210.       
  211.      
  212.      
  213.      
  214.        x 
  215.       
  216.      
  217.        e 
  218.       
  219.      
  220.     
  221.       + 
  222.      
  223.     
  224.       12 
  225.      
  226.      
  227.      
  228.        C 
  229.       
  230.      
  231.        4 
  232.       
  233.      
  234.      
  235.      
  236.        x 
  237.       
  238.      
  239.        e 
  240.       
  241.      
  242.        2 
  243.       
  244.      
  245.     
  246.       + 
  247.      
  248.     
  249.       20 
  250.      
  251.      
  252.      
  253.        C 
  254.       
  255.      
  256.        5 
  257.       
  258.      
  259.      
  260.      
  261.        x 
  262.       
  263.      
  264.        e 
  265.       
  266.      
  267.        3 
  268.       
  269.      
  270.     
  271.       − 
  272.      
  273.     
  274.       − 
  275.      
  276.     
  277.       − 
  278.      
  279.     
  280.       − 
  281.      
  282.     
  283.       ( 
  284.      
  285.     
  286.       3 
  287.      
  288.     
  289.       − 
  290.      
  291.     
  292.       6 
  293.      
  294.     
  295.       ) 
  296.      
  297.     
  298.    
  299.      f''(x) = 2C_2+ 6C_3x_e+12C_4x_e^2+20C_5x_e^3----(3-6) 
  300.     
  301.    
  302.  f′′(x)=2C2​+6C3​xe​+12C4​xe2​+20C5​xe3​−−−−(3−6)

联立可得

  1.        C 
  2.       
  3.      
  4.        3 
  5.       
  6.      
  7.     
  8.       = 
  9.      
  10.      
  11.       
  12.       
  13.         10 
  14.        
  15.       
  16.         ( 
  17.        
  18.       
  19.         f 
  20.        
  21.       
  22.         ( 
  23.        
  24.        
  25.        
  26.          x 
  27.         
  28.        
  29.          p 
  30.         
  31.        
  32.       
  33.         ) 
  34.        
  35.       
  36.         − 
  37.        
  38.       
  39.         1 
  40.        
  41.       
  42.         / 
  43.        
  44.       
  45.         2 
  46.        
  47.       
  48.         ∗ 
  49.        
  50.        
  51.        
  52.          f 
  53.         
  54.         
  55.         
  56.           ′ 
  57.          
  58.         
  59.           ′ 
  60.          
  61.         
  62.        
  63.       
  64.         ( 
  65.        
  66.       
  67.         0 
  68.        
  69.       
  70.         ) 
  71.        
  72.        
  73.        
  74.          x 
  75.         
  76.        
  77.          p 
  78.         
  79.        
  80.          2 
  81.         
  82.        
  83.       
  84.         − 
  85.        
  86.        
  87.        
  88.          f 
  89.         
  90.        
  91.          ′ 
  92.         
  93.        
  94.       
  95.         ( 
  96.        
  97.       
  98.         0 
  99.        
  100.       
  101.         ) 
  102.        
  103.        
  104.        
  105.          x 
  106.         
  107.        
  108.          p 
  109.         
  110.        
  111.       
  112.         − 
  113.        
  114.       
  115.         f 
  116.        
  117.       
  118.         ( 
  119.        
  120.       
  121.         0 
  122.        
  123.       
  124.         ) 
  125.        
  126.       
  127.         ) 
  128.        
  129.       
  130.         − 
  131.        
  132.       
  133.         4 
  134.        
  135.       
  136.         ( 
  137.        
  138.        
  139.        
  140.          f 
  141.         
  142.        
  143.          ′ 
  144.         
  145.        
  146.       
  147.         ( 
  148.        
  149.        
  150.        
  151.          x 
  152.         
  153.        
  154.          p 
  155.         
  156.        
  157.       
  158.         ) 
  159.        
  160.       
  161.         − 
  162.        
  163.        
  164.        
  165.          f 
  166.         
  167.         
  168.         
  169.           ′ 
  170.          
  171.         
  172.           ′ 
  173.          
  174.         
  175.        
  176.       
  177.         ( 
  178.        
  179.       
  180.         0 
  181.        
  182.       
  183.         ) 
  184.        
  185.        
  186.        
  187.          x 
  188.         
  189.        
  190.          p 
  191.         
  192.        
  193.       
  194.         − 
  195.        
  196.        
  197.        
  198.          f 
  199.         
  200.        
  201.          ′ 
  202.         
  203.        
  204.       
  205.         ( 
  206.        
  207.       
  208.         0 
  209.        
  210.       
  211.         ) 
  212.        
  213.       
  214.         ) 
  215.        
  216.        
  217.        
  218.          x 
  219.         
  220.        
  221.          p 
  222.         
  223.        
  224.       
  225.         + 
  226.        
  227.       
  228.         1 
  229.        
  230.       
  231.         / 
  232.        
  233.       
  234.         2 
  235.        
  236.       
  237.         ∗ 
  238.        
  239.       
  240.         ( 
  241.        
  242.        
  243.        
  244.          f 
  245.         
  246.         
  247.         
  248.           ′ 
  249.          
  250.         
  251.           ′ 
  252.          
  253.         
  254.        
  255.       
  256.         ( 
  257.        
  258.        
  259.        
  260.          x 
  261.         
  262.        
  263.          p 
  264.         
  265.        
  266.       
  267.         ) 
  268.        
  269.       
  270.         − 
  271.        
  272.        
  273.        
  274.          f 
  275.         
  276.         
  277.         
  278.           ′ 
  279.          
  280.         
  281.           ′ 
  282.          
  283.         
  284.        
  285.       
  286.         ( 
  287.        
  288.       
  289.         0 
  290.        
  291.       
  292.         ) 
  293.        
  294.       
  295.         ) 
  296.        
  297.        
  298.        
  299.          x 
  300.         
  301.        
  302.          p 
  303.         
  304.        
  305.          2 
  306.         
  307.        
  308.       
  309.       
  310.       
  311.         x 
  312.        
  313.       
  314.         p 
  315.        
  316.       
  317.         3 
  318.        
  319.       
  320.      
  321.     
  322.    
  323.      C_3 = \frac{10(f(x_p)-1/2*f''(0)x_p^2-f'(0)x_p-f(0))-4(f'(x_p)-f''(0)x_p-f'(0))x_p+1/2*(f''(x_p)-f''(0))x_p^2}{x_p^3} 
  324.     
  325.    
  326.  C3​=xp3​10(f(xp​)−1/2∗f′′(0)xp2​−f′(0)xp​−f(0))−4(f′(xp​)−f′′(0)xp​−f′(0))xp​+1/2∗(f′′(xp​)−f′′(0))xp2​​
  329.   
  330.    
  331.     
  332.      
  333.      
  334.        C 
  335.       
  336.      
  337.        4 
  338.       
  339.      
  340.     
  341.       = 
  342.      
  343.      
  344.       
  345.       
  346.         − 
  347.        
  348.       
  349.         15 
  350.        
  351.       
  352.         ( 
  353.        
  354.       
  355.         f 
  356.        
  357.       
  358.         ( 
  359.        
  360.        
  361.        
  362.          x 
  363.         
  364.        
  365.          p 
  366.         
  367.        
  368.       
  369.         ) 
  370.        
  371.       
  372.         − 
  373.        
  374.       
  375.         1 
  376.        
  377.       
  378.         / 
  379.        
  380.       
  381.         2 
  382.        
  383.       
  384.         ∗ 
  385.        
  386.        
  387.        
  388.          f 
  389.         
  390.         
  391.         
  392.           ′ 
  393.          
  394.         
  395.           ′ 
  396.          
  397.         
  398.        
  399.       
  400.         ( 
  401.        
  402.       
  403.         0 
  404.        
  405.       
  406.         ) 
  407.        
  408.        
  409.        
  410.          x 
  411.         
  412.        
  413.          p 
  414.         
  415.        
  416.          2 
  417.         
  418.        
  419.       
  420.         − 
  421.        
  422.        
  423.        
  424.          f 
  425.         
  426.        
  427.          ′ 
  428.         
  429.        
  430.       
  431.         ( 
  432.        
  433.       
  434.         0 
  435.        
  436.       
  437.         ) 
  438.        
  439.        
  440.        
  441.          x 
  442.         
  443.        
  444.          p 
  445.         
  446.        
  447.       
  448.         − 
  449.        
  450.       
  451.         f 
  452.        
  453.       
  454.         ( 
  455.        
  456.       
  457.         0 
  458.        
  459.       
  460.         ) 
  461.        
  462.       
  463.         ) 
  464.        
  465.       
  466.         + 
  467.        
  468.       
  469.         7 
  470.        
  471.       
  472.         ( 
  473.        
  474.        
  475.        
  476.          f 
  477.         
  478.        
  479.          ′ 
  480.         
  481.        
  482.       
  483.         ( 
  484.        
  485.        
  486.        
  487.          x 
  488.         
  489.        
  490.          p 
  491.         
  492.        
  493.       
  494.         ) 
  495.        
  496.       
  497.         − 
  498.        
  499.        
  500.        
  501.          f 
  502.         
  503.         
  504.         
  505.           ′ 
  506.          
  507.         
  508.           ′ 
  509.          
  510.         
  511.        
  512.       
  513.         ( 
  514.        
  515.       
  516.         0 
  517.        
  518.       
  519.         ) 
  520.        
  521.        
  522.        
  523.          x 
  524.         
  525.        
  526.          p 
  527.         
  528.        
  529.       
  530.         − 
  531.        
  532.        
  533.        
  534.          f 
  535.         
  536.        
  537.          ′ 
  538.         
  539.        
  540.       
  541.         ( 
  542.        
  543.       
  544.         0 
  545.        
  546.       
  547.         ) 
  548.        
  549.       
  550.         ) 
  551.        
  552.        
  553.        
  554.          x 
  555.         
  556.        
  557.          p 
  558.         
  559.        
  560.       
  561.         − 
  562.        
  563.       
  564.         1 
  565.        
  566.       
  567.         / 
  568.        
  569.       
  570.         2 
  571.        
  572.       
  573.         ∗ 
  574.        
  575.       
  576.         ( 
  577.        
  578.        
  579.        
  580.          f 
  581.         
  582.         
  583.         
  584.           ′ 
  585.          
  586.         
  587.           ′ 
  588.          
  589.         
  590.        
  591.       
  592.         ( 
  593.        
  594.        
  595.        
  596.          x 
  597.         
  598.        
  599.          p 
  600.         
  601.        
  602.       
  603.         ) 
  604.        
  605.       
  606.         − 
  607.        
  608.        
  609.        
  610.          f 
  611.         
  612.         
  613.         
  614.           ′ 
  615.          
  616.         
  617.           ′ 
  618.          
  619.         
  620.        
  621.       
  622.         ( 
  623.        
  624.       
  625.         0 
  626.        
  627.       
  628.         ) 
  629.        
  630.       
  631.         ) 
  632.        
  633.        
  634.        
  635.          x 
  636.         
  637.        
  638.          p 
  639.         
  640.        
  641.          2 
  642.         
  643.        
  644.       
  645.       
  646.       
  647.         x 
  648.        
  649.       
  650.         p 
  651.        
  652.       
  653.         4 
  654.        
  655.       
  656.      
  657.     
  658.    
  659.      C_4 = \frac{-15(f(x_p)-1/2*f''(0)x_p^2-f'(0)x_p-f(0))+7(f'(x_p)-f''(0)x_p-f'(0))x_p-1/2*(f''(x_p)-f''(0))x_p^2}{x_p^4} 
  660.     
  661.    
  662.  C4​=xp4​−15(f(xp​)−1/2∗f′′(0)xp2​−f′(0)xp​−f(0))+7(f′(xp​)−f′′(0)xp​−f′(0))xp​−1/2∗(f′′(xp​)−f′′(0))xp2​​
  665.   
  666.    
  667.     
  668.      
  669.      
  670.        C 
  671.       
  672.      
  673.        5 
  674.       
  675.      
  676.     
  677.       = 
  678.      
  679.      
  680.       
  681.       
  682.         6 
  683.        
  684.       
  685.         ( 
  686.        
  687.       
  688.         f 
  689.        
  690.       
  691.         ( 
  692.        
  693.        
  694.        
  695.          x 
  696.         
  697.        
  698.          p 
  699.         
  700.        
  701.       
  702.         ) 
  703.        
  704.       
  705.         − 
  706.        
  707.       
  708.         1 
  709.        
  710.       
  711.         / 
  712.        
  713.       
  714.         2 
  715.        
  716.       
  717.         ∗ 
  718.        
  719.        
  720.        
  721.          f 
  722.         
  723.         
  724.         
  725.           ′ 
  726.          
  727.         
  728.           ′ 
  729.          
  730.         
  731.        
  732.       
  733.         ( 
  734.        
  735.       
  736.         0 
  737.        
  738.       
  739.         ) 
  740.        
  741.        
  742.        
  743.          x 
  744.         
  745.        
  746.          p 
  747.         
  748.        
  749.          2 
  750.         
  751.        
  752.       
  753.         − 
  754.        
  755.        
  756.        
  757.          f 
  758.         
  759.        
  760.          ′ 
  761.         
  762.        
  763.       
  764.         ( 
  765.        
  766.       
  767.         0 
  768.        
  769.       
  770.         ) 
  771.        
  772.        
  773.        
  774.          x 
  775.         
  776.        
  777.          p 
  778.         
  779.        
  780.       
  781.         − 
  782.        
  783.       
  784.         f 
  785.        
  786.       
  787.         ( 
  788.        
  789.       
  790.         0 
  791.        
  792.       
  793.         ) 
  794.        
  795.       
  796.         ) 
  797.        
  798.       
  799.         − 
  800.        
  801.       
  802.         3 
  803.        
  804.       
  805.         ( 
  806.        
  807.        
  808.        
  809.          f 
  810.         
  811.        
  812.          ′ 
  813.         
  814.        
  815.       
  816.         ( 
  817.        
  818.        
  819.        
  820.          x 
  821.         
  822.        
  823.          p 
  824.         
  825.        
  826.       
  827.         ) 
  828.        
  829.       
  830.         − 
  831.        
  832.        
  833.        
  834.          f 
  835.         
  836.         
  837.         
  838.           ′ 
  839.          
  840.         
  841.           ′ 
  842.          
  843.         
  844.        
  845.       
  846.         ( 
  847.        
  848.       
  849.         0 
  850.        
  851.       
  852.         ) 
  853.        
  854.        
  855.        
  856.          x 
  857.         
  858.        
  859.          p 
  860.         
  861.        
  862.       
  863.         − 
  864.        
  865.        
  866.        
  867.          f 
  868.         
  869.        
  870.          ′ 
  871.         
  872.        
  873.       
  874.         ( 
  875.        
  876.       
  877.         0 
  878.        
  879.       
  880.         ) 
  881.        
  882.       
  883.         ) 
  884.        
  885.        
  886.        
  887.          x 
  888.         
  889.        
  890.          p 
  891.         
  892.        
  893.       
  894.         + 
  895.        
  896.       
  897.         1 
  898.        
  899.       
  900.         / 
  901.        
  902.       
  903.         2 
  904.        
  905.       
  906.         ∗ 
  907.        
  908.       
  909.         ( 
  910.        
  911.        
  912.        
  913.          f 
  914.         
  915.         
  916.         
  917.           ′ 
  918.          
  919.         
  920.           ′ 
  921.          
  922.         
  923.        
  924.       
  925.         ( 
  926.        
  927.        
  928.        
  929.          x 
  930.         
  931.        
  932.          p 
  933.         
  934.        
  935.       
  936.         ) 
  937.        
  938.       
  939.         − 
  940.        
  941.        
  942.        
  943.          f 
  944.         
  945.         
  946.         
  947.           ′ 
  948.          
  949.         
  950.           ′ 
  951.          
  952.         
  953.        
  954.       
  955.         ( 
  956.        
  957.       
  958.         0 
  959.        
  960.       
  961.         ) 
  962.        
  963.       
  964.         ) 
  965.        
  966.        
  967.        
  968.          x 
  969.         
  970.        
  971.          p 
  972.         
  973.        
  974.          2 
  975.         
  976.        
  977.       
  978.       
  979.       
  980.         x 
  981.        
  982.       
  983.         p 
  984.        
  985.       
  986.         5 
  987.        
  988.       
  989.      
  990.     
  991.    
  992.      C_5 = \frac{6(f(x_p)-1/2*f''(0)x_p^2-f'(0)x_p-f(0))-3(f'(x_p)-f''(0)x_p-f'(0))x_p+1/2*(f''(x_p)-f''(0))x_p^2}{x_p^5} 
  993.     
  994.    
  995.  C5​=xp5​6(f(xp​)−1/2∗f′′(0)xp2​−f′(0)xp​−f(0))−3(f′(xp​)−f′′(0)xp​−f′(0))xp​+1/2∗(f′′(xp​)−f′′(0))xp2​​

apollo中5次多项式的计算

  1. void QuinticPolynomialCurve1d::ComputeCoefficients(constdouble x0,constdouble dx0,constdouble ddx0,constdouble x1,constdouble dx1,constdouble ddx1,constdouble p){CHECK_GT(p,0.0);
  3.   coef_[0]= x0;
  4.   coef_[1]= dx0;
  5.   coef_[2]= ddx0 /2.0;constdouble p2 = p * p;constdouble p3 = p * p2;// the direct analytical method is at least 6 times faster than using matrix// inversion.constdouble c0 =(x1 -0.5* p2 * ddx0 - dx0 * p - x0)/ p3;constdouble c1 =(dx1 - ddx0 * p - dx0)/ p2;constdouble c2 =(ddx1 - ddx0)/ p;
  7.   coef_[3]=0.5*(20.0* c0 -8.0* c1 + c2);
  8.   coef_[4]=(-15.0* c0 +7.0* c1 - c2)/ p;
  9.   coef_[5]=(6.0* c0 -3.0* c1 +0.5* c2)/ p2;}

5次多项式拟合

  1. import math
  3. import matplotlib.pyplot as plt
  4. import numpy as np
  6. # parameter
  7. MAX_T =100.0# maximum time to the goal [s]
  8. MIN_T =5.0# minimum time to the goal[s]
  10. show_animation =TrueclassQuinticPolynomial:def__init__(self, xs, vxs, axs, xe, vxe, axe, time):# calc coefficient of quintic polynomial# See jupyter notebook document for derivation of this equation.# 根据初始状态c0 c1 c2
  11.         self.a0 = xs # x(t)
  12.         self.a1 = vxs # v(t)
  13.         self.a2 = axs /2.0# a(t)
  15.         A = np.array([[time **3, time **4, time **5],[3* time **2,4* time **3,5* time **4],[6* time,12* time **2,20* time **3]])
  16.         b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time **2,
  17.                       vxe - self.a1 -2* self.a2 * time,
  18.                       axe -2* self.a2])# Ax=b 求解x
  19.         x = np.linalg.solve(A, b)# 计算c3 c4 c5
  20.         self.a3 = x[0]
  21.         self.a4 = x[1]
  22.         self.a5 = x[2]# 根据时间计算点x(t)defcalc_point(self, t):
  23.         xt = self.a0 + self.a1 * t + self.a2 * t **2+ \
  24.              self.a3 * t **3+ self.a4 * t **4+ self.a5 * t **5return xt
  25.     # 计算v(t)defcalc_first_derivative(self, t):
  26.         xt = self.a1 +2* self.a2 * t + \
  27.              3* self.a3 * t **2+4* self.a4 * t **3+5* self.a5 * t **4return xt
  28.     # 计算a(t)  defcalc_second_derivative(self, t):
  29.         xt =2* self.a2 +6* self.a3 * t +12* self.a4 * t **2+20* self.a5 * t **3return xt
  30.     # 计算jerk(t)  defcalc_third_derivative(self, t):
  31.         xt =6* self.a3 +24* self.a4 * t +60* self.a5 * t **2return xt
  33. defquintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):"""
  34.     quintic polynomial planner
  36.     input
  37.         s_x: start x position [m]
  38.         s_y: start y position [m]
  39.         s_yaw: start yaw angle [rad]
  40.         sa: start accel [m/ss]
  41.         gx: goal x position [m]
  42.         gy: goal y position [m]
  43.         gyaw: goal yaw angle [rad]
  44.         ga: goal accel [m/ss]
  45.         max_accel: maximum accel [m/ss]
  46.         max_jerk: maximum jerk [m/sss]
  47.         dt: time tick [s]
  49.     return
  50.         time: time result
  51.         rx: x position result list
  52.         ry: y position result list
  53.         ryaw: yaw angle result list
  54.         rv: velocity result list
  55.         ra: accel result list
  57.     """
  59.     vxs = sv * math.cos(syaw)# 起点速度在x方向分量
  60.     vys = sv * math.sin(syaw)# 起点速度在y方向分量
  61.     vxg = gv * math.cos(gyaw)# 终点速度在x方向分量
  62.     vyg = gv * math.sin(gyaw)# 终点速度在y方向分量
  64.     axs = sa * math.cos(syaw)# 起点加速度在x方向分量
  65.     ays = sa * math.sin(syaw)# 终点加速度在y方向分量
  66.     axg = ga * math.cos(gyaw)# 起点加速度在x方向分量
  67.     ayg = ga * math.sin(gyaw)# 终点加速度在y方向分量
  69.     time, rx, ry, ryaw, rv, ra, rj =[],[],[],[],[],[],[]for T in np.arange(MIN_T, MAX_T, MIN_T):
  70.         xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
  71.         yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)
  73.         time, rx, ry, ryaw, rv, ra, rj =[],[],[],[],[],[],[]for t in np.arange(0.0, T + dt, dt):
  74.             time.append(t)
  75.             rx.append(xqp.calc_point(t))
  76.             ry.append(yqp.calc_point(t))
  78.             vx = xqp.calc_first_derivative(t)
  79.             vy = yqp.calc_first_derivative(t)
  80.             v = np.hypot(vx, vy)
  81.             yaw = math.atan2(vy, vx)
  82.             rv.append(v)
  83.             ryaw.append(yaw)
  85.             ax = xqp.calc_second_derivative(t)
  86.             ay = yqp.calc_second_derivative(t)
  87.             a = np.hypot(ax, ay)iflen(rv)>=2and rv[-1]- rv[-2]<0.0:
  88.                 a *=-1
  89.             ra.append(a)
  91.             jx = xqp.calc_third_derivative(t)
  92.             jy = yqp.calc_third_derivative(t)
  93.             j = np.hypot(jx, jy)iflen(ra)>=2and ra[-1]- ra[-2]<0.0:
  94.                 j *=-1
  95.             rj.append(j)ifmax([abs(i)for i in ra])<= max_accel andmax([abs(i)for i in rj])<= max_jerk:print("find path!!")breakif show_animation:# pragma: no coverfor i, _ inenumerate(time):
  96.             plt.cla()# for stopping simulation with the esc key.
  97.             plt.gcf().canvas.mpl_connect('key_release_event',lambda event:[exit(0)if event.key =='escape'elseNone])
  98.             plt.grid(True)
  99.             plt.axis("equal")
  100.             plot_arrow(sx, sy, syaw)
  101.             plot_arrow(gx, gy, gyaw)
  102.             plot_arrow(rx[i], ry[i], ryaw[i])
  103.             plt.title("Time[s]:"+str(time[i])[0:4]+" v[m/s]:"+str(rv[i])[0:4]+" a[m/ss]:"+str(ra[i])[0:4]+" jerk[m/sss]:"+str(rj[i])[0:4],)
  104.             plt.pause(0.001)return time, rx, ry, ryaw, rv, ra, rj
  106. defplot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):# pragma: no cover"""
  107.     Plot arrow
  108.     """ifnotisinstance(x,float):for(ix, iy, iyaw)inzip(x, y, yaw):
  109.             plot_arrow(ix, iy, iyaw)else:
  110.         plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
  111.                   fc=fc, ec=ec, head_width=width, head_length=width)
  112.         plt.plot(x, y)defmain():
  114.     sx =10.0# start x position [m]
  115.     sy =10.0# start y position [m]
  116.     syaw = np.deg2rad(10.0)# start yaw angle [rad]
  117.     sv =1.0# start speed [m/s]
  118.     sa =0.1# start accel [m/ss]
  119.     gx =30.0# goal x position [m]
  120.     gy =-10.0# goal y position [m]
  121.     gyaw = np.deg2rad(20.0)# goal yaw angle [rad]
  122.     gv =1.0# goal speed [m/s]
  123.     ga =0.1# goal accel [m/ss]
  124.     max_accel =1.0# max accel [m/ss]
  125.     max_jerk =0.5# max jerk [m/sss]
  126.     dt =0.1# time tick [s]
  128.     time, x, y, yaw, v, a, j = quintic_polynomials_planner(
  129.         sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)if show_animation:# pragma: no cover
  130.         plt.plot(x, y,"-r")
  132.         plt.subplots()
  133.         plt.plot(time,[np.rad2deg(i)for i in yaw],"-r")
  134.         plt.xlabel("Time[s]")
  135.         plt.ylabel("Yaw[deg]")
  136.         plt.grid(True)
  138.         plt.subplots()
  139.         plt.plot(time, v,"-r")
  140.         plt.xlabel("Time[s]")
  141.         plt.ylabel("Speed[m/s]")
  142.         plt.grid(True)
  144.         plt.subplots()
  145.         plt.plot(time, a,"-r")
  146.         plt.xlabel("Time[s]")
  147.         plt.ylabel("accel[m/ss]")
  148.         plt.grid(True)
  150.         plt.subplots()
  151.         plt.plot(time, j,"-r")
  152.         plt.xlabel("Time[s]")
  153.         plt.ylabel("jerk[m/sss]")
  154.         plt.grid(True)
  156.         plt.show()if __name__ =='__main__':
  157.     main()

在这里插入图片描述
在这里插入图片描述

上图模拟了车辆曲线的拟合过程的轨迹，箭头代表方向。

过程的速度在这里插入图片描述

感觉5次多项式和贝塞尔曲线还是挺像的，有时间写一篇五次多项式、贝塞尔曲线、样条曲线的对比。

还有一个问题就是为啥要用5次多项式，而不是4次，6次更高阶或者低阶呢？下回复习一下老王的讲解。
参考：

标签： 自动驾驶 算法 人工智能
本文转载自: https://blog.csdn.net/weixin_43673156/article/details/129016974
版权归原作者 BineHello 所有， 如有侵权，请联系我们删除。
登录后发布评论

“自动驾驶规划 - 5次多项式拟合”的评论:

还没有评论
关于作者
...
overfit同步小助手
文章同步
相关阅读

网络安全法-网络运行安全

使用selenium/drissionpage时如何阻止chrome自动跳转http到https

docker desktop 里部署的Open WebUI 管理员密码忘记了的处理方法

在ubuntu20.04中搭建onsite比赛运行环境

利用开源的低代码表单设计器FcDesigner高效管理和渲染复杂表单结构

Kafka学习笔记

【前端】浏览器输入url到页面呈现发生了什么？

文章导航
简介