基于MPC的移动机器人轨迹跟踪控制qpOASES例程

参考了
一个模型预测控制（MPC）的简单实现

https://www.cnblogs.com/zhjblogs/p/13880682.html

与
基于MPC的移动机器人轨迹跟踪控制matlab例程

https://blog.csdn.net/a735148617/article/details/113784730

上一篇写了如何使用qpOASES

https://blog.csdn.net/weixin_42454034/article/details/118600394

这里是如何将其应用在MPC中。
为了最大的简化，这里使用了机器人控制速度到达目标点的模型，并且机器人只有一个方向的自由度。

第一步移动机器人建模：
首先构建系统状态方程为：

在这里

第二步离散化：
在k时刻有

其中T为控制间隔时长。
移项后可得：

所以有：

第三步预测
在第一个参考里面有详细的说明。
我们记未来 P个控制周期内预测的系统状态为:

测时域内的控制量Uk:

通过离散化状态方程依次对未来 P个控制周期的系统状态进行预测

整合成矩阵形式：

第四步优化
参考轨迹为：

优化目标函数为：

其中Q状态误差权重，对角阵；W为输出权重，对角阵。
该优化问题可以描述如下：

将优化函数 展开后合并同类项：
在此注意一下，qpOASES中求解格式为：

所以要将其展开为要求的格式。
在参考一中其展开的形式为：

它展开后的第二项与qpOASES中格式不一致，但是刚刚好是qpOASES要求的格式是其第二项的转置。
参考中也说明了UTthitaTQE与ET Q * thita * U相等的原因。
所以在代码中转置一下就行。
根据qpOASES格式。

g = fT的转置。

代码中仿真应用：
在ubutu20.04中使用webots仿真环境，首先建立一个简单的仿真机器人.

只控制机器人一个方向上的速度。
然后需要安装eigen 和qpOASES。之前文档中写了ubuntu20.04下eigen 安装及qpOASES的安装

随后编写控制器，加入MPC控制。
在这贴出代码的主要部分：

    xk(0,0) = pos;
    for(int i=0 ; i<P;i++){
      fei(i,0) = pow(A_,i+1);
    }
    //  cout <<"fei "<<fei <<"\n";

    for(int i=0; i <P;++i){
      for(int j=0; j <=i;++j){
        theta(i,j)= pow(A_,i-j)*B_;
      }
    }
    //cout <<"theta"<<theta <<"\n";

    E = fei*xk-Rk;
    H = 2.0*(theta.transpose()*Q*theta+W);  //求矩阵的转置
    f = (2.0*E.transpose()*Q*theta).transpose(); 
    g = f.transpose();   //g为f的转置  
     // cout <<E <<"\n" << H <<"\n" <<f <<"\n";
     //转为qpOASES格式
    for(int i =0;i<H.rows();i++)
    {
      for(int j=0;j<H.cols();j++)
      {
        H_matrix[(i)*H.cols()+j] = H(i,j);
      }
      g_matrix[i] = g(0,i);
    }
    
    int_t nWSR = 10;
    v_pro.init(H_matrix, g_matrix, lb_matrix, ub_matrix, nWSR);
    real_t xOpt[2];
	  v_pro.getPrimalSolution( xOpt );
	  printf( "\nxOpt = [ %e, %e ];  objVal = %e\n\n", xOpt[0],xOpt[1],v_pro.getObjVal() );

    setV(xOpt[0],timeStep);
    // setV(0.5,timeStep);
    //pos = pos + xOpt[0]*T ;
    pos = robot->getSelf()->getPosition()[0];  //机器人位置
    printf("pos:%f\n",pos);

设定目标为运行到2m位置，
运行后效果如下：

最后与matlab中对比一下，matlab中代码如下

clc; clear;
T_long  = 2; %总时长2s
T=0.05;    %控制间隔 
P = 5;   %预测长度
Q  = 10*eye(P);   %状态误差权重
W  = 0.1*eye(P); %控制输出权重
Rk  = zeros(P,1);  %参考值序列
Rk(1:end) = 2;%参考位置由函数参数指定
vmax = 1;
lb = zeros(P,1);   %最小输出
lb(1:end) = -vmax;
ub = zeros(P,1);   %最大输出
ub(1:end) = vmax;

pos(1) = 0 ;     %当前状态

for t=1:50  %步数
    %构建中间变量
    xk = pos(t);    %当前状态量
    A_ = 1;  
    B_ = T;
    fei = zeros(P,1); %AK

    for i=1:1:P
        fei(i) = A_^i;
    end

    theta =size(P,P);  %BK
    for i=1:1:P
        for j=1:1:i
            theta(i,j)=A_^(i-j)*B_;
        end
    end
 
    E = fei*xk-Rk;           %E 
    H = 2*(theta'*Q*theta+W) %H
    f = (2*E'*Q*theta)'      %f

    Uk=quadprog(H,f,[],[],[],[],lb,ub);
  
    Uk(1)
     pos(t)
    pos(t+1) = pos(t) + Uk(1)*T;% + Uk(1)*T*unifrnd(-0.2,0.2);  %位置自己加
    
end
plot(pos)

运行结果如图，通过修改Q 或者W能够改变位置曲线。

项目githup地址
使用webots2020b,在ubuntu20.04下。

给点积分也不是不行