【速度决策规划】S型速度规划

概要

小车控制需要缓慢加速，即速度平缓上升，对于小车走定距离，速度规划很有帮助

来做点数学吧

速度规划的最根本的点还是想要让小车的加速度连续或者再强一点是加加速度（加速度的导数），不跃变，本文讨论的是最常用的五段速度规划

先列出我们需要的位移公式

x (t_{0}) = a_{0} + a_{1} t_{0} + a_{2} t_{0}^{2} + a_{3} t_{0}^{3} + a_{4} t_{0}^{4} + a_{5} t_{0}^{5}

$x(t_0)=a_0+a_1t_0+a_2t_0^2+a_3t_0^3+a_4t_0^4+a_5t_0^5$

x (t_{1}) = a_{1} + a_{1} t_{1} + a_{2} t_{1}^{2} + a_{3} t_{1}^{3} + a_{4} t_{1}^{4} + a_{5} t_{1}^{5}

$x(t_1)=a_1+a_1t_1+a_2t_1^2+a_3t_1^3+a_4t_1^4+a_5t_1^5$

其中t₀和t₁分别为初始时刻和终点时刻，设定周期T为t₁-t₀

再对两个位移公式进行求导，获得我们的速度公式

v (t_{0}) = a_{1} + 2 a_{2} t_{0} + 3 a_{3} t_{0}^{2} + 4 a_{4} t_{0}^{3} + 5 a_{5} t_{0}^{4}

$v(t_0)=a_1+2a_2t_0+3a_3t_0^2+4a_4t_0^3+5a_5t_0^4$

v (t_{1}) = a_{1} + 2 a_{2} t_{1} + 3 a_{3} t_{1}^{2} + 4 a_{4} t_{1}^{3} + 5 a_{5} t_{1}^{4}

$v(t_1)=a_1+2a_2t_1+3a_3t_1^2+4a_4t_1^3+5a_5t_1^4$

继续求导，获得加速度公式

a (t_{0}) = 2 a_{2} + 6 a_{3} t_{0} + 12 a_{4} t_{0}^{2} + 20 a_{5} t_{0}^{3}

$a(t_0)=2a_2+6a_3t_0+12a_4t_0^2+20a_5t_0^3$

a (t_{1}) = 2 a_{2} + 6 a_{3} t_{1} + 12 a_{4} t_{1}^{2} + 20 a_{5} t_{1}^{3}

$a(t_1)=2a_2+6a_3t_1+12a_4t_1^2+20a_5t_1^3$

我们可以发现，加速度公式变得复杂起来了，暂时咱还不知道他是什么曲线，但是我们可以肯定的是他不会发生跃变了，是一个平缓变化的过程，正好是我们需要的东西

由我们的想法来看，小车应该是静止出发，而后缓慢停下来，所以可以知道初始的速度和加速度以及终点的速度和加速度都为0

这样的话就好办了！不就是解方程嘛！6个方程，6个解，~~简单的一~~……

斯……斯……（算了）

我们将公式整理整理，交给电脑去算吧

将六个公式写成线性方程组的形式

T * A = [\begin{matrix} t_{0}^{5} & t_{0}^{4} & t_{0}^{3} & t_{0}^{2} & t_{0} & 1 \\ 5 t_{0}^{4} & 4 t_{0}^{3} & 3 t_{0}^{2} & 2 t_{0} & 1 & 0 \\ 20 t_{0}^{3} & 12 t_{0}^{2} & 6 t_{0} & 2 & 0 & 0 \\ t_{1}^{5} & t_{1}^{4} & t_{1}^{3} & t_{1}^{2} & t_{1} & 1 \\ 5 t_{1}^{4} & 4 t_{1}^{3} & 3 t_{1}^{2} & 2 t_{1} & 1 & 0 \\ 20 t_{1}^{3} & 12 t_{1}^{2} & 6 t_{1} & 2 & 0 & 0 \end{matrix}] [\begin{matrix} a_{5} \\ a_{4} \\ a_{3} \\ a_{2} \\ a_{1} \\ a_{0} \end{matrix}] = [\begin{matrix} x (t_{0}) \\ v (t_{0}) \\ a (t_{0}) \\ x (t_{1}) \\ v (t_{1}) \\ a (t_{1}) \end{matrix}] = X

$T*A=\left[ \begin{matrix} t_0^5 & t_0^4 & t_0^3 & t_0^2 & t_0 & 1 \\ 5t_0^4 & 4t_0^3 & 3t_0^2 & 2t_0 & 1 & 0 \\ 20t_0^3 & 12t_0^2 & 6t_0 & 2 & 0 & 0\\ t_1^5 & t_1^4 & t_1^3 & t_1^2 & t_1 & 1 \\ 5t_1^4 & 4t_1^3 & 3t_1^2 & 2t_1 & 1 & 0 \\ 20t_1^3 & 12t_1^2 & 6t_1 & 2 & 0 & 0 \end{matrix} \right] \left[ \begin{matrix} a_5\\ a_4\\ a_3\\ a_2\\ a_1\\ a_0 \end{matrix} \right] = \left[ \begin{matrix} x(t_0)\\ v(t_0)\\ a(t_0)\\ x(t_1)\\ v(t_1)\\ a(t_1) \end{matrix} \right]=X$

所以可知 A=T^-1*X

期望已知，公式已知，交给MATLAB来算

%速度规划验证
%已知参数
%初始加速度，速度，终点加速度，速度都为0
%定义变量
clc;
clear all;
%修改flag值，算出不同结果
flag = 1;
if(flag==1)
    %假设t0!=0,x0!=0
    syms x0 x1 t0 t1;
else   
    %假设t0=0,x0=0
    syms x1 t1;
    t0=0;x0=0;    
end
v0=0;a0=0;v1=0;a1=0;
%将各方程写成矩阵形式
T=[t0^5,t0^4,t0^3,t0^2,t0,1;
5*t0^4,4*t0^3,3*t0^2,2*t0,1,0;
20*t0^3,12*t0^2,6*t0,2,0,0;
t1^5,t1^4,t1^3,t1^2,t1,1;
5*t1^4,4*t1^3,3*t1^2,2*t1,1,0;
20*t1^3,12*t1^2,6*t1,2,0,0;];
%一一对应后矩阵B为
B=[x0;v0;a0;x1;v1;a1];
%求解，可以知道T*A = B，所以A=T(逆)*B
A = inv(T)*B;
fprintf("%s",char(A));

本文讨论控制中最常用的t₀=0，x₀=0的情况

通过程序计算分析

得到

[\begin{matrix} a_{5} \\ a_{4} \\ a_{3} \\ a_{2} \\ a_{1} \\ a_{0} \end{matrix}] = [\begin{matrix} \frac{6 x_{1}}{t_{1}^{5}} \\ \frac{- 15 x_{1}}{t_{1}^{4}} \\ \frac{10 x_{1}}{t_{1}^{3}} \\ 0 \\ 0 \\ 0 \end{matrix}]

$\left[ \begin{matrix} a_5\\ a_4\\ a_3\\ a_2\\ a_1\\ a_0 \end{matrix} \right] = \left[ \begin{matrix} {6x_1\over t_1^5}\\ {-15x_1\over t_1^4}\\ {10x_1\over t_1^3}\\ 0\\ 0\\ 0 \end{matrix} \right]$

电脑算出来的也不一定对，咱们需要找个值验证一下

%画个函数图像看看
clc;
clear all;
x1=10;
t1=5;
t=0:0.01:t1;
%位移
s = 10*x1/t1^3*t.^3-15*x1/t1^4*t.^4+6*x1/t1^5*t.^5;
%速度
v = 30*x1/t1^3*t.^2-60*x1/t1^4*t.^3+30*x1/t1^5*t.^4;
%加速度
a = 60*x1/t1^3*t-180*x1/t1^4*t.^2+120*x1/t1^5*t.^3;
plot(t,s,t,v,t,a);

得到图像

其中横轴为时间t，纵轴为位移s,蓝色为位移曲线，红色为速度曲线，黄色为加速度曲线

至此，基本的速度规划已经完成

开始应用

/**
 * @brief Y轴速度规划
 * @param T 输入期望运动总时间
 * @param deltaT 定时器周期
 * @param tar_pos 目标位移
 * @return float
 */
float y_vel_planned(double T, float deltaT, float tar_pos)
{

    vel_p_.k[3] = 10.0f * tar_pos / pow(T, 3.0f);
    vel_p_.k[4] = -15.0f * tar_pos / pow(T, 4.0f);
    vel_p_.k[5] = 6.0f * tar_pos / pow(T, 5.0f);
    vel_p_.time_y = (vel_p_.t_y++) * deltaT;
    if (vel_p_.time_y <= T && vel_p_.end_flag_y == false)
    {
        vel_p_.res_y = vel_p_.k[3] * pow(vel_p_.time_y, 3) + vel_p_.k[4] * pow(vel_p_.time_y, 4) + vel_p_.k[5] * pow(vel_p_.time_y, 5);
        vel_p_.res_v_y = 3 * vel_p_.k[3] * pow(vel_p_.time_y, 2) + 4 * vel_p_.k[4] * pow(vel_p_.time_y, 3) + 5 * vel_p_.k[5] * pow(vel_p_.time_y, 4);
    }
    if (vel_p_.time_y > T && vel_p_.end_flag_y == false)
    {
        vel_p_.time_y = T;
    }
    if (fabs(vel_p_.res_y - tar_pos) <= 0.001f)
    {
        vel_p_.res_y = tar_pos;
        vel_p_.t_y = 0;
        vel_p_.end_flag_y = true;
    }
    return vel_p_.res_y;
}

上面的程序可以直接应用于单片机中，其中的一些变量名修改成自己的就好

经过自己的验证，通过调节PID和时间周期T，还是能够让小车跑的非常丝滑的

上述讲述如有误，烦请大佬指正！

tips

在调试的过程中，发现时间周期T对于小车外环（位置环）的距离有较大影响，虽说后面通过踩点，将误差拟合掉了，但其实这暴露了一些底层控制的问题，有可能是最底层速度闭环没有做好，也有
可能是编码器的累计脉冲产生误差，但对于大多数控制精度的来讲，这样够用了，如有大佬知道怎么做，还望评论区留言，还是很想知道的！嘿嘿
正常来讲，速度规划嘛，用到位置外环，但是我们可能会发现位置外环的曲线并不能够很好的拟合到我们要的曲线，这个时候就需要加入速度前馈，即速度内环的期望=位置环PID输出+上面代
码中的vel_p_.res_v_y