基于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); %AKfor i=1:1:Pfei(i) = A_^i;endtheta =size(P,P);  %BKfor i=1:1:Pfor j=1:1:itheta(i,j)=A_^(i-j)*B_;endendE = fei*xk-Rk;           %E H = 2*(theta'*Q*theta+W) %Hf = (2*E'*Q*theta)'      %fUk=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下。

给点积分也不是不行

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

使用webots的MPC的移动机器人轨迹跟踪控制
上一篇文章中使用MPC对机器人的一个方向自由度进行了控制, 基于MPC的移动机器人轨迹跟踪控制qpOASES例程现在使用速度与角速度对机器人进行平面运动控制. 所以机器人的控制量为U=[v ;w], ...
ROS功能包|mav_control_rw（基于MPC的无人机轨迹跟踪控制）---gazebo仿真测试
ROS功能包|mav_control_rw(基于MPC的无人机轨迹跟踪控制)---gazebo仿真测试 gazebo仿真测试 gazebo仿真测试启动gazebo并加载无人机模型 $ roslaun ...
基于运动学模型的轨迹跟踪控制
章四基于运动学模型的轨迹跟踪控制 MPC(4)基于运动学模型的轨迹跟踪控制器设计无人驾驶车辆模型预测控制(龚建伟)第四章基于运动学模型的轨迹跟踪控制(仿真部分) 无人车辆在惯性坐标系中,车辆必须 ...
1、MPC 算法（模型预测控制算法（MPC算法）轨迹跟踪控制）
MPC 跟踪圆形轨迹/直线轨迹 MPC 跟踪双移线轨迹 MPC 进行局部路径规划+轨迹跟踪 MPC跟踪直线轨迹 N MPC 对直线轨迹进行跟踪 MPC 算法跟踪五次多项式曲线以上为目录推荐学习的软 ...
【Carsim Simulink自动驾驶仿真】基于MPC的轨迹跟踪控制
如果对Carsim的基础使用还不了解,可以参考:[Carsim Simulink自动驾驶仿真]基于MPC的速度控制如果对MPC算法原理不清楚,可以参考:如何理解MPC模型预测控制理论项目介绍: 教 ...
基于模糊补偿的液压机械臂轨迹跟踪控制
摘要: 针对具有摩擦和负载等不确定性的液压机械臂轨迹跟踪控制问题,建立机械臂机械系统.液压系统的数学模型,基于李亚普洛夫函数,提出一种模糊补偿策略进行控制,构造模糊系统逼近摩擦力和负载,应用流量补偿器 ...
基于CarSim和Simulink的车辆预瞄轨迹跟踪控制模型+carsim设置文件
基于CarSim和Simulink的车辆预瞄轨迹跟踪控制模型+carsim设置文件可选模型说明文件和操作说明 ID:2180642445175030
无人驾驶车辆路径规划及轨迹跟踪控制学习笔记（2）
目录汇总学习笔记汇总在关键交通场景中,轨迹规划和轨迹跟踪控制是自动驾驶车辆避免碰撞的两个关键.它不仅需要系统功能,而且需要强大的实时性. 我们集成了自动驾驶汽车的轨迹规划器和跟踪控制器,通过轨 ...
无人驾驶车辆规划+轨迹跟踪控制学习笔记（1）
综述自动驾驶汽车有一个很有前途的未来,它可以使运输时间变得轻松,并使驾驶员能够参与其他活动,从而改变世界各地的日常生活.它们有可能大大减少由驾驶员的过失造成的撞车事故,包括驾驶积极.补偿过度.经验不 ...

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

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

最新文章

热门文章