为您推荐:
精华内容
最热下载
问答
  • 5星
    5KB weixin_42696333 2021-09-10 15:53:15
  • 5星
    30KB weixin_42325129 2021-06-14 23:11:58
  • 5星
    93KB weixin_42696333 2019-05-06 09:35:54
  • 261KB makeflyeasy 2017-11-11 21:36:20
  • 5星
    5KB weixin_42696333 2021-09-10 15:53:15
  • 5星
    93KB weixin_42696333 2019-05-06 09:35:54
  • 在介绍LQR之前,首先,先回顾一下现在控制理论中的基本的控制器 —————— 全状态反馈控制,上图 假设有一个线性系统用状态向量表示: {x˙=Ax+Buy=Cx+Du(1) \begin{cases} \dot{x} = Ax + Bu...

    首先,这篇文章是看了几个大神的博客后,自己抄录以及整理的内容,其中有些自己的想法,但是原理部分基本都是学习大神们的,在此先说明一下。

    1 全状态反馈控制系统

    在介绍LQR之前,首先,先回顾一下现在控制理论中的基本的控制器 —————— 全状态反馈控制,上图
    在这里插入图片描述
    假设有一个线性系统用状态向量表示:
    { x ˙ = A x + B u y = C x + D u (1) \begin{cases} \dot{x} = Ax + Bu & \\ y = Cx + Du & \end{cases} \tag{1} {x˙=Ax+Buy=Cx+Du(1)
    其中, x ( t ) ∈ R n x(t) \in R^n x(t)Rn u ( t ) ∈ R m u(t) \in R^m u(t)Rm,初始条件是 x ( 0 ) x(0) x(0)
    在此,我们需要设计一个状态反馈控制器
    u = − K x (2) u = -Kx \tag{2} u=Kx(2)
    使上述控制器能达到期望的稳定性能,将式(2)带入系统状态方程(1)中,有
    x ˙ = ( A − B K ) x = A c x (3) \dot{x} = (A - BK)x = A_{c}x \tag{3} x˙=(ABK)x=Acx(3)
    设定系统中的各个状态量都可知,式(1)所示的开环系统,传递函数的极点就是系统矩阵A的特征值。现在变换成了式(2)的闭环形式,通过配置反馈矩阵 K K K,可以使得闭环系统达到所期望的系统状态。(注意,这种控制器设计与矩阵C、D没什么关系)
    SO,来了一个新的问题,极点在什么样的位置会使得系统的性能较好呢?并且,当系统变量很多的时候,就算设计极点已经达到了最优,矩阵K的计算如何计算呢?
    因此,LQR提供了如下思路。

    2 LQR

    LQR的目标就是找到一组控制量 u 0 , u 1 , . . . u_0,u_1,... u0,u1,...,使得同时有 x 0 , x 1 , . . . x_0,x_1,... x0,x1,...足够小(系统达到稳定状态), u 0 , u 1 , . . . u_0,u_1,... u0,u1,...足够小(控制量尽量小的变化),选取代价函数为
    J = 1 2 ∫ 0 ∞ x T Q x + u T R u   d t (4) J = {1 \over 2}\int_0^\infty {x^TQx + u^TRu} \space dt \tag{4} J=210xTQx+uTRu dt(4)
    其中,Q、R就是需要设计的半正定矩阵和正定矩阵。
    看上式(4)是不是线代中的二次型(线代真的是一门重要的学科,工程中大量都是线代的运算),而且还是那种只有平方项的二次型,这样就成了最小二乘法的问题。代价函数 J J J需要达到最小值,那么在 t t t趋近于无穷时,状态向量 x ( t ) x(t) x(t)肯定趋近于0,即是达到了系统稳态;同理, t t t趋近于无穷时,控制向量 u ( t ) u(t) u(t)也会趋近于0,意味着,随着时间的推移,需要对系统施加的控制量会越来越小,意味着使用最小的控制量使得系统达到了最终控制目标。

    下面来聊聊Q、R值的选取,一般来说,选取Q、R矩阵的时候,为了方便观察各个系统状态量而选取对角阵,增加Q的一个值,意味着这个值作用的系统状态量,将以更快的速度衰减到0,这时候,举个栗子还是很必要的,比如, Q 11 Q_{11} Q11选取较大的值,会让 x 1 x_1 x1很快的衰减到0;另外一方面,加大 R R R的值,会使得对应的控制量减小,控制器执行更少的动作,意味着系统的状态衰减将变慢。所以, Q 、 R Q、R QR的选取,要综合看具体的实际应用场景来调节,俗话说,鱼和熊掌不可兼得,这个矛盾就像做的轨迹跟踪MPC中的设置不同的权重一样,期望车辆后轴心cte值越小,那么到了转弯处必须要很大幅度的刹车,以及打方向盘,速度以及yaw都会大幅度变化,而如果期望执行器动作越小,那么就无法保证cte的准确度一样。

    好了,上面介绍了一些参数的意义问题,下面重点就是公式的推导了,在不同大神那看到了不同的推导方式,下面就把自己比较欣赏的一种写出来吧,虽说过程也很重要,但是工程嘛,只要结果对,效果好,选个自己看起来舒服的方式就好了,开始推导过程

    1. u = − K x u = -Kx u=Kx代入代价函数后,有
      J = 1 2 ∫ 0 ∞ x T ( Q + K T R K ) x   d t (5) J = {1 \over 2}\int_0^\infty x^T(Q + K^TRK)x \space dt \tag{5} J=210xT(Q+KTRK)x dt(5)
    2. 假设纯在一个常量矩阵 P P P使得,
      d d t ( x T P x ) = − x T ( Q + K T R K ) x (6) {d \over dt}(x^TPx) = - x^T(Q + K^TRK)x \tag{6} dtd(xTPx)=xT(Q+KTRK)x(6)
    3. 把式(6)代入(5)后,有
      J = − 1 2 ∫ 0 ∞ d d t x T ( P ) x   = 1 2 x T ( 0 ) P x ( 0 ) (7) J = -{1 \over 2}\int_0^\infty \frac{d}{dt}x^T(P)x \space = {1 \over 2}x^T(0)Px(0) \tag{7} J=210dtdxT(P)x =21xT(0)Px(0)(7)
      式(7)的意思就是,t趋近于无穷时,系统状态向量x(t)趋近于0,这样就直接结算出了积分方程。
    4. 把式(6)左边微分展开,
      x ˙ T P x + x T P x ˙ + x T Q x + x T K T R K x = 0 \dot{x}^{T}Px + x^{T}P\dot{x}+x^TQx+x^TK^TRKx = 0 x˙TPx+xTPx˙+xTQx+xTKTRKx=0
      状态变量x的微分用式(3)表示,
      x T A c T P x + x T P A c x + x T Q x + x T K T R K x = 0 x^{T}A^{T}_cPx+x^{T}PA_{c}x+x^TQx+x^TK^TRKx = 0 xTAcTPx+xTPAcx+xTQx+xTKTRKx=0
      整理后,有
      x T ( A c T P + P A c + Q + K T R K ) x = 0 (8) x^{T}(A^{T}_{c}P+PA_{c}+Q+K^{T}RK)x=0 \tag{8} xT(AcTP+PAc+Q+KTRK)x=0(8)
      这样,就又回到了二次型的问题,如果式(8)要有解,那么括号里面的部分必须等于0.
      A c T P + P A c + Q + K T R K = 0 (9) A^{T}_{c}P+PA_{c}+Q+K^{T}RK = 0 \tag{9} AcTP+PAc+Q+KTRK=0(9)
      A c = A − B K A_c = A-BK Ac=ABK代入式(9)
      ( A − B K ) T P + P ( A − B K ) + Q + K T R K = 0 (10) (A-BK)^{T}P+P(A-BK)+Q+K^{T}RK = 0 \tag{10} (ABK)TP+P(ABK)+Q+KTRK=0(10)
      A T P + P A + Q + K T R K − K T B T P − P B K = 0 (11) A^{T}P+PA+Q+K^{T}RK-K^{T}B^{T}P-PBK=0 \tag{11} ATP+PA+Q+KTRKKTBTPPBK=0(11)
    5. 式(11)还是一个关于 K K K的二次型,这样会导致计算量太复杂,so只要这个等式成立就好,那么这里令 K = R − 1 B T P K = R^{-1}B^TP K=R1BTP,然后式(11),可以化为
      A T P + P A + Q + K T R ( R − 1 B T P ) − K T B T P − P B ( R − 1 B T P ) = 0 A^{T}P+PA+Q+K^{T}R(R^{-1}B^TP)-K^{T}B^{T}P-PB(R^{-1}B^TP)=0 ATP+PA+Q+KTR(R1BTP)KTBTPPB(R1BTP)=0
      A T P + P A + Q − P B R − 1 B T P = 0 (12) A^{T}P+PA+Q-PBR^{-1}B^TP=0 \tag{12} ATP+PA+QPBR1BTP=0(12)
      式(12)中, A , B , Q , R A,B,Q,R A,B,Q,R都是已知量,那么通过式(12)可以求解出 P P P,式(12)就是著名的Riccati方程。

    总结

    上面,从理论以及公式推导两个方面,介绍了LQR,现在从头看一下LQR的思路:

    • 选择参数矩阵Q,R
    • 求解Riccati方程得到矩阵P
    • 根据P计算 K = R − 1 B T P K=R^{-1}B^{T}P K=R1BTP
    • 计算控制量 u = − K x u=-Kx u=Kx

    MPC与LQR比较

    MPC和LQR两种控制方式有很多的相似之处,但是也有很多不相同的地方,

    • 首先,LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统,而MPC的研究对象可以是线性系统,也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化,然后进行相关计算,具体要根据自己的工程情况来确定哪种方式比较好,比如之前做MPC的时候,线控车底层速度控制接口就是加速度,那就没必要根据IMU再套嵌个一层PID。
    • 其次,既然是优化问题,那就离不开目标函数的设计,LQR的目标函数在上面已经有描述,MPC的目标函数,多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同,不过都是对达到控制目标的代价累计。
    • 最后,工作时域上的不同,LQR的计算针对同一工作时域,在一个控制周期内,LQR只计算一次,并将此次计算出的最优解下发给控制器即可;而MPC是滚动优化的,计算未来一段时间内,每个采样周期都会经过计算,得出一组控制序列,但是只将第一个控制值下发给控制器。

    Reference:

    1. F.L. Lewis .<< Linear Quadratic Regulator (LQR) State Feedback Design >>
    2. https://blog.csdn.net/u013914471/article/details/84324754?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task
    3. https://zhuanlan.zhihu.com/p/72605138
    4. https://blog.csdn.net/heyijia0327/article/details/39270597
    展开全文
    qq_24649627 2020-03-09 14:32:30
  • 百度Apollo 2.0 车辆控制算法之LQR控制算法解读 Apollo 中横向控制的LQR控制算法在Latcontroller..cc 中实现 根据车辆的二自由度动力学模型 (1) 根据魔术公式在小角度偏角的情况下有,轮胎的侧向力与轮胎的偏离角...

    百度Apollo 2.0 车辆控制算法之LQR控制算法解读

    Apollo 中横向控制的LQR控制算法在Latcontroller..cc 中实现

    根据车辆的二自由度动力学模型

    (1)

    根据魔术公式在小角度偏角的情况下有,轮胎的侧向力与轮胎的偏离角成正比. ,分别为前、后轮的侧偏刚度,

    (2)

    (3)在小角度的情况下有

    所以有

    (4)

    因此上述车辆的动力学模型可以简化写成

    (5)

     (6)期望横摆角角速度

    (7) 横摆角角度偏差

     (7)横向偏差变化率求导数

     (8)横向偏差变化率

     

    车辆模型的连续状态空间方程

    (9)

    状态变量X的选择分别为横向偏差、横向偏差变化率,横摆角角度偏差,横摆角角度偏差变化率。控制量u为前轮偏角。

    选择合适的状态变量后得到A,B,B1,B2矩阵分别如下

       

    由于只对横摆角角度偏差变化率的导数产生影响,在横向控制中主要是控制横向偏差、横向偏差变化率,横摆角角度偏差,横摆角角度偏差变化率,因而忽略了公式中项。车辆系统的状态空间方程表示为

     (10)

    Init()函数中将A,B, 与Vx无关的系数先行计算,与Vx相关的系数参数计算根据Vx不断更新。

    上述连续的状态空间方程用于计算机控制需要对连续的状态空间方程进行离散化,其中At采用双线性变换得到

    , At来源见右边公式

    , ,T为控制周期,本程序中为0.01s。

    上述参数计算和离散化的过程在UpdateMatrix()函数和UpdateMatrixCompound()函数中

    的离散化过程在Init()函数中实现

      matrix_b_ = Matrix::Zero(basic_state_size_, 1);

      matrix_bd_ = Matrix::Zero(basic_state_size_, 1);

      matrix_bdc_ = Matrix::Zero(matrix_size, 1);

      matrix_b_(1, 0) = cf_ / mass_;

      matrix_b_(3, 0) = lf_ * cf_ / iz_;

      matrix_bd_ = matrix_b_ * ts_;

    的离散化过程在UpdateMatrix()函数中实现

    void LatController::UpdateMatrix() {

      const double v =

          std::max(VehicleStateProvider::instance()->linear_velocity(), 0.2);

      matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;

      matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;

      matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;

      matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;

      Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());

      matrix_ad_ = (matrix_i + ts_ * 0.5 * matrix_a_) *

                   (matrix_i - ts_ * 0.5 * matrix_a_).inverse(); //双线性变换离散化A

    }

    通过上述介绍我们得到了车辆离散状态空间方程(11)中的At,Bt,则系统的最优前轮转角 (12)

    定义如下目标函数

     (13)

    其中Q为状态权重系数,R为控制量权重系数

    当上述目标函数最小时就得到最优的状态反馈矩阵K

    上述公式的,(14)

    同时矩阵P满足黎卡提方程:

    (15)

    求解状态反馈矩阵K的在

        common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,

                                      matrix_r_, lqr_eps_, lqr_max_iteration_,

                                      &matrix_k_); 函数中实现

    函数输入为At,Bt,Q,R,最大迭代次数lqr_max_iteration_,最小精度lqr_eps_,函数输出为状态反馈矩阵matrix_k_。

    当前最新的状态量是通过UpdateStateAnalyticalMatching()函数获得,由此我们可以通过 (12)

    计算出当前的最优反馈控制量即最优前轮偏角。然而这还没玩完。

    将公式(12)的控制量带入公式(10)得到系统状态反馈后的状态空间方程如下

      (13)

    车辆沿固定曲率的轨迹运行时不为零。因此通过LQR调节的特征值使系统趋于稳定,但是系统的稳态偏差并不为0。

         为此在原有最优控制量的基础上增加一个前馈环节,使系统趋于稳定的同时系统的横向稳态偏差为0。

        (14)

    公式中为前馈环节提供的前轮转角。

    将公式(14)带入公式(10)得到

     (15)

    设初始条件为0,对公式(15)进行拉普拉斯变换得到

      (16)

    假设汽车以固定纵向速度Vx沿某一固定曲率的弯道行驶,则通过纵向车速Vx和道路的半径R可以计算出期望汽车横摆角速度:

       (6)

    公式(6)可知横摆角速度的拉普拉斯变换结果为

     (17)

    假设为固定值,则其拉普拉斯变换结果为

    (18)

    根据终值定理,系统的稳态误差为

     (19)

    将A,B,B1,K带入公式(19)得到

     (20)

    观察公式(20)中的第一项和第三项。可知道对横摆角角度偏差无影响,选择合适的可将横向偏差稳态值趋向与0。

     (21)

    要想横向偏差的稳态值趋于零,则

     (22)

    因此

       (23)

    ,不足转向梯度系数  (24)

    公式(23)可以简化为

      (25)

    Apollo程序中计算控制量的函数为ComputeControlCommand()函数

     

    前馈环节计算的前轮转角对应如下程序

    double LatController::ComputeFeedForward(double ref_curvature) const {

      const double kv =

          lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_; //对应公式(24)

     

      // then change it from rad to %

      const double v = VehicleStateProvider::instance()->linear_velocity();

      const double steer_angle_feedforwardterm =

          (wheelbase_ * ref_curvature + kv * v * v * ref_curvature -

           matrix_k_(0, 2) *

               (lr_ * ref_curvature -

                lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_)) *

          180 / M_PI * steer_transmission_ratio_ /

          steer_single_direction_max_degree_ * 100; //对应公式(25),并将角度由弧度转变为角度,最后转变为百分比

      return steer_angle_feedforwardterm;

    }

     

    最终的前轮转角的控制量为最优状态反馈控制量与前馈控制前轮转角之和。对应程序如下

      const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /

                                          M_PI * steer_transmission_ratio_ /

                                          steer_single_direction_max_degree_ * 100;

     //计算状态反馈对应的控制量,将弧度转变为角度,最后转变为百分比。

    // steer_transmission_ratio_表示方向盘转动角度与车轮转动角度的比值,在车辆信息中定义该参数,

    // steer_single_direction_max_degree表示最大的方向盘转动的角度,单位为度

      const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());

    //计算前馈控制对应的控制量

     

      // Clamp the steer angle to -100.0 to 100.0

      double steer_angle = common::math::Clamp(

          steer_angle_feedback + steer_angle_feedforward, -100.0, 100.0); //将状态反馈的控制量与前馈控制控制量进行叠加,并进行限幅处理。

    计算的出前轮转角经过上下限的限幅后进行输出

      if (FLAGS_set_steer_limit) {

        const double steer_limit =

            std::atan(max_lat_acc_ * wheelbase_ /

                      (VehicleStateProvider::instance()->linear_velocity() *

                       VehicleStateProvider::instance()->linear_velocity())) *

            steer_transmission_ratio_ * 180 / M_PI /

            steer_single_direction_max_degree_ * 100; //计算前轮转角的上下限限幅值。

     

        // Clamp the steer angle

        double steer_angle_limited =

            common::math::Clamp(steer_angle, -steer_limit, steer_limit); //对前轮转角进行上下限的限幅处理

        steer_angle_limited = digital_filter_.Filter(steer_angle_limited); //对前轮转角进行低通滤波处理

        cmd->set_steering_target(steer_angle_limited);

        debug->set_steer_angle_limited(steer_angle_limited);

      } else {

        steer_angle = digital_filter_.Filter(steer_angle);//对前轮转角进行低通滤波处理

        cmd->set_steering_target(steer_angle);

      }

     

     

    部分成员函数介绍

    1、LoadControlConf 成员函数用于获取控制参数包括车身参数、LQR控制的精度、最大迭代次数等。

    其中车辆的参数来自modules\common\data\ mkz_config.pb.txt文件

    LQR的控制参数来自modules\control\conf\ lincoln.pb.txt文件

    2、InitializeFilters成员函数用于设置巴斯沃特低通低通滤波参数, 以及对 lateral_error、heading_error进行均值滤波。滤波器的参数来自modules\control\conf\ lincoln.pb.txt文件

     

    3、Init成员函数用于初始化 状态空间方程的A,B,K, Q,R 以及控制系统的相关参数。

    LoadLatGainScheduler函数的作用?

    LoadLatGainScheduler函数用于获取 lateral_error、heading_error 增益的策略。

     

    4、ComputeControlCommand 函数最重要,计算控制量

     

    通过LQR求解黎卡提方程得到控制量

    SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,

                                      matrix_r_, lqr_eps_, lqr_max_iteration_,

                                      &matrix_k_);

    //在高速运行时,控制量权重矩阵matrix_q_的系数根据速度变化进行调整

      // Add gain sheduler for higher speed steering

      if (FLAGS_enable_gain_scheduler) {

        matrix_q_updated_(0, 0) =

            matrix_q_(0, 0) *

            lat_err_interpolation_->Interpolate(

                VehicleStateProvider::instance()->linear_velocity());

        matrix_q_updated_(2, 2) =

            matrix_q_(2, 2) *

            heading_err_interpolation_->Interpolate(

                VehicleStateProvider::instance()->linear_velocity());

        common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_updated_,

                                      matrix_r_, lqr_eps_, lqr_max_iteration_,

                                      &matrix_k_);

      } else {

        common::math::SolveLQRProblem(matrix_adc_, matrix_bdc_, matrix_q_,

                                      matrix_r_, lqr_eps_, lqr_max_iteration_,

                                      &matrix_k_);

      }

     

    5、UpdateMatrix()函数

    用于更新matrix_a_(离散之前的A矩阵) 和matrix_ad_(离散之后的A矩阵)

    matrix_a_ 由中系数分为两类,一类与速度无关,另外一类与速度相关放在matrix_a_coeff_

    从何得到线性时变状态空间方程。

    matrix_adc_ 由matrix_ad_ 和 过去的变量对应的矩阵组合而成

     matrix_bdc_. 由matrix_bd_ 和过去变量对应的矩组合而成。

     

    6、GetLateralError()函数

    获取横向偏差,具体计算过程为根据车辆当前的位置查找参考轨迹最近点,形成直线L。

    计算出车辆位置与最近点的距离std::sqrt(dx * dx + dy * dy),同时计算出该条线与x轴正方向之间的角度point_angle,将该角度减去参考轨迹的方向角得到直线L与参考轨迹速度方向之间的夹角point2path_angle。

    横向偏差为std::sin(point2path_angle) * std::sqrt(dx * dx + dy * dy);

    7、UpdateStateAnalyticalMatching()函数获取状态偏差,ComputeLateralErrors()函数主要通过根据当前车辆的位置计算出在参考轨迹上上离车辆当前位置最近点作为参考点,通过参考点与实际车辆位置就可以获得各种状态偏差(横向偏差、横向偏差变化率、航向角偏差、航向角偏差变化率)。

    展开全文
    weijimin1 2019-01-04 19:26:13
  • 之前有写过MPC的控制算法,主要介绍的也是理论部分,在实际使用过程中发现C++没有高效的优化方法,类似python中的cvxpy的库,所以想绕过去,研究一下LQR控制算法。 1. PID控制系统 这里并不是要介绍PID系统,而是...

            之前有写过MPC的控制算法,主要介绍的也是理论部分,在实际使用过程中发现C++没有高效的优化方法,类似python中的cvxpy的库,所以想绕过去,研究一下LQR控制算法。

    1. 控制系统

            这里我们先介绍常用的控制系统逻辑:

                                          在这里插入图片描述

            假设我们现在状态是x0,我们有状态方程 :                 (u为控制矩阵)

            特别的,这里我们是对偏差建立方程,x是偏差的状态,优化的目的是x=0,针对我们通过一些假设可以得到详细的方程,这里我直接先给出其中,  

                         

            详细的推导有兴趣可以看最后。

            再假设有一个反馈控制器:                 这里很重要,可以认为是当前的控制量是通过当前的状态量计算出来的

            通过这套方法,我们就能得到一个稳定的系统 :  

            当然这是基本的理论,再进一步,我们就会思考,通过这些控制量作为自变量,再设计一个代价函数,来优化这些控制量?

    2. LQR控制算法

            讲到这里就很自然引出LQR了,首先的问题代价函数是什么?一方面我们希望系统达到稳定状态,及偏差最小; 另一方面我们希望控制量较小,即付出较小的代价达到我们的目的。这里我直接给出:

                                                                

             其中x为状态量,u为控制量,Q为状态权重矩阵,R为控制权重矩阵。

              特别的x和u中取值有正有负,所以需要平方和最小,在矩阵中没有平方,这里我们采用转置乘以本身的做法模拟矩阵的平方,如x^T*x 。这里状态量x和控制量u都是多维向量,上式计算的结果是一个标量。

              其实我们可以把看作是的多维扩展表达式,这里我们需要Q为半正定,就是希望Q能起到a≥0的效果,R为正定矩阵就是希望矩阵R能够起到a>0的效果。

              一般的我们认为状态量x为:横向偏差、横向变化率、角度偏差、角度变化率。Q为我们提前标定的对角矩阵,标定值对应以上不同维度的权重,也可以为非对角矩阵,考虑不同维度之间的相互关系。

             控制向量u为:前轮转角、加速度。同样的R也为提前标定的对角矩阵。也可以考虑相互关系。

             我的理解是Q11选取较大的值,会让x1很快到0;另外一方面,加大R的值,会使得对应的控制量减小,控制器执行更少的动作,意味着系统的状态衰减将变慢。所以要综合看具体的实际应用场景来调节,鱼和熊掌不可兼得。建议在不同场景下采用不同的参数。

    3. 公式求解

             有了上面的优化目标后,就是如何求解最优的控制量了。下面我们先进行一些转换:

              1. 将u=−Kx代入代价函数后:  ,u=−Kx即我们认为当前的控制量可以通过当前的状态量计算出来。如果我们能计算出-K就很简单了,可以直接套出控制量。

              2. 假设纯在一个常量矩阵P使得,  ,这里就直接这么假设,接着往下看,有需要深入的同学可以看后面详细推导,总之,在等式成立的情况下,J取最小值,达到我们的优化目标。

              3. 把上式代入得到:               

              4. 把2中的方程微分展开:

              5. 状态变量x的微分用式表示: 

                   整理得到:

                   通过矩阵得知上式要想有解只有中间部分为零,即:

               6. 把 代入上式:

                    整理得到: 

                7. 这里我们再令      这里需要思考一下,为什么可以这样假设,凑出结果?为了不增加复杂度,把这里的推导我放在最后,感兴趣的朋友可以继续研究。总之 在上面等式成立的情况下,可以得到

                     代入:

                      整理得到: ,得到这一步就很简单了,式中A、B、Q、R都是已知量,很容易就能计算出P的结果。

                 8. 计算出P再通过就可以计算出K,也就是达到了我们一开始的目标,轻松得到当前的控制量。

    4. MPC与LQR比较

              首先,LQR的研究对象是现代控制理论中的状态空间方程给出的线性系统,而MPC的研究对象可以是线性系统,也可以是非线性系统。不过现在很多的做法都是将非线性系统线性化,然后进行相关计算,具体要根据自己的工程情况来确定哪种方式比较好,比如之前做MPC的时候,线控车底层速度控制接口就是加速度,那就没必要根据IMU再套嵌个一层PID。

              其次,既然是优化问题,那就离不开目标函数的设计,LQR的目标函数在上面已经有描述,MPC的目标函数,多数都是多个优化目标乘以不同权重然后求和的方式。虽然方式不同,不过都是对达到控制目标的代价累计。

              最后,工作时域上的不同,LQR的计算针对同一工作时域,在一个控制周期内,LQR只计算一次,并将此次计算出的最优解下发给控制器即可;而MPC是滚动优化的,计算未来一段时间内,每个采样周期都会经过计算,得出一组控制序列,但是只将第一个控制值下发给控制器。

            对于LQR所有的理论部分都介绍完毕,下面是之前的一些公司推导部分,有兴趣的可以阅读:

    1. 运动学模型的推导

            根据车辆的二自由度动力学模型 : 

            在小角度偏角的情况下有,轮胎的侧向力与轮胎的偏离角成正比. ,分别为前、后轮的侧偏刚度:

             (3)在小角度的情况下有 所以有(4)

            因此上述车辆的动力学模型可以简化写成(5)

               (6)期望横摆角角速度    ;

                横摆角角度偏差; 

                横向偏差变化率求导数;

                 (8)横向偏差变化率

                  车辆模型的连续状态空间方程  

                状态变量X的选择分别为横向偏差、横向偏差变化率,横摆角角度偏差,横摆角角度偏差变化率。控制量u为前轮偏角。

               选择合适的状态变量后得到A,B,B1,B2矩阵分别如下

                       

                  由于只对横摆角角度偏差变化率的导数产生影响,在横向控制中主要是控制横向偏差、横向偏差变化率,横摆角角度偏差,横摆角角度偏差变化率,因而忽略了公式中项。车辆系统的状态空间方程表示为 (10)        

    2. 最优化目标的等价条件

             首先控制理论是一门学科,我建议其他专业同学,直接使用以上的结论,如果你非要想完全理解,你需要具备以下知识:泛函极值、哈密尔顿方程、正则方程、无约束条件泛函、变分法、拉格朗日方程、黎卡提微分方程、控制域、欧拉方程、等周长积分方程......等等知识点。如果说你都具备,欢迎你往下看,不然你还是多看几遍上面把。

             1. 对于的最小化问题,

    3.对于 的推导

             在前面的公式中,我们假设,到底这部分假设有没有数学推导呢?

             回到之前的公式:           ,

             同时我们知道R是正定对称矩阵,则一定存在一个矩阵M使得:,因为R的每一项大于零,M也为对角矩阵,数字为其平方根。

             代入R得到:

    展开全文
    zhouyy858 2020-07-27 17:15:09
  • (一)车辆建模 两自由度车辆模型为: 考虑恒定不变,则上述模型可以看作一个输入为、状态变量为的控制系统,可以表示为:...(二) LQR控制算法仿真 根据上一篇博文的分析,LQR设计步骤如下: 1)根据工程经验和

    (一)车辆建模

    两自由度车辆模型为:

    考虑恒定不变,则上述模型可以看作一个输入为、状态变量为的控制系统,可以表示为:

    对于参考轨迹,可以表示为:

    将(2)式在处泰勒展开,并忽略高次项,有:

    ,式(4)-(3)可得:

    对上式进行离散化处理,可得:

    所以,

    记:

    则(7)式可以写为:

    由于系统矩阵A和输入矩阵B的元素随时间变化,所以上述系统是一个线性时变系统

    (二) LQR控制算法仿真

    根据上一篇博文的分析,LQR设计步骤如下:

    1)根据工程经验和性能分析,确定矩阵Qf、Q和R;

    2)倒序求解P;

    3)计算反馈矩阵K;

    4)正序求解u(k)。

    基于第一部分的车辆运动学模型,运用python对LQR控制算法进行仿真,跟踪一个圆形轨迹的仿真结果如下图所示。

    具体仿真总结如下:

    1)R矩阵的选取

    如果不对控制量(前轮转角)进行限制,R需要选择较大的值,比如这里选择86能达到较好的跟踪效果(见上图);如果R选择较小的值,比如取1,跟踪效果就会很差,如下图所示。

    此时,如果对控制量(前轮转角)进行限幅处理,可以提升跟踪效果,如下图所示。

    所以,选取较大的R可以减小控制量

    2)Q矩阵的选取

    有资料指出:较大的Q可以得到较快的调节速度

    目前仿真下来还没有摸清楚Q选取的规律,只能说Q不能太大、也不能太小。

    3)Qf矩阵的选取

    将Qf取为0,也能达到较好的跟踪效果,如下图所示。如果对终端误差不作限制,Qf可以取为0。

    展开全文
    Ronnie_Hu 2021-04-29 20:21:10
  • studyvcmfc 2020-11-02 14:52:17
  • weijimin1 2019-02-28 19:37:24
  • 93KB leavemyleave 2021-10-14 23:35:43
  • 5星
    30KB weixin_42325129 2021-06-14 23:11:58
  • 650KB weixin_62047240 2021-10-04 12:09:12
  • 1.48MB wenyusuran 2021-03-12 21:33:44
  • 367B czb951872154 2019-04-17 14:01:45
  • 465B weixin_41874554 2018-06-28 15:19:53
  • 500KB sinat_41747072 2020-03-29 17:17:57
  • 294KB frank330 2018-07-02 16:19:23
  • 90KB qq_31064283 2018-01-18 09:36:00
  • 3星
    130KB whhithit 2012-06-29 18:34:04
  • 92KB weixin_42595982 2018-08-13 04:58:55
  • weixin_39867142 2021-04-26 16:04:38
  • weixin_39916379 2021-05-24 02:33:46
  • 4星
    92KB u014517312 2014-04-02 20:33:32
  • 5星
    341KB weixin_44020886 2021-02-17 08:45:07
  • zhulianhai0927 2020-03-19 17:29:04
  • 369KB u014053127 2014-11-04 12:34:40
  • TIQCmatlab 2021-07-15 23:36:23
  • 4星
    1KB weixin_42696271 2021-09-10 21:08:13
  • 4星
    321KB subin0403 2017-10-11 21:44:04

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 715
精华内容 286
关键字:

lqr控制算法

友情链接: lunfai_v73.zip