💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文内容如下：🎁🎁🎁
⛳️赠与读者
👨💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。
或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎
💥第一部分——内容介绍
基于模型预测控制（MPC）与滚动时域估计（MHE）集成的目标点镇定研究
摘要：本文提出一种“模型预测控制（MPC）+ 滚动时域估计（MHE）”一体化框架，旨在解决在传感器和执行器双重噪声环境下，将移动机器人稳定到指定目标点 xs 的问题。与现有研究仅单独考虑状态或控制噪声、且将估计与控制分步求解的做法不同，本文创新性地把传感器噪声和执行器噪声同时纳入联合优化，实现了真正意义上的“估计–控制闭环”。采用多重打靶法将 MPC 问题转化为非线性规划（NLP），并利用 CASADI 工具求解，为处理非线性 MPC 问题提供了有效途径。通过仿真验证，在双重噪声干扰下，所提方法仍能保证机器人最终收敛到目标点。
关键词：模型预测控制；滚动时域估计；目标点镇定；双重噪声；非线性规划
一、引言
在移动机器人控制领域，将机器人稳定到指定目标点（目标点镇定）是一个基本且重要的任务。然而，实际应用中，机器人不可避免地会受到各种噪声的干扰，如传感器噪声和执行器噪声。传感器噪声会使得机器人获取的自身状态信息不准确，而执行器噪声则会导致机器人实际执行的控制指令与期望值存在偏差。这些噪声的存在给机器人的目标点镇定带来了巨大挑战。
传统的控制方法往往单独考虑状态噪声或控制噪声，并且将状态估计与控制设计分步进行。例如，先利用滤波算法（如卡尔曼滤波）对带噪的状态进行估计，然后将估计值作为真实状态用于控制设计。这种分步求解的方式虽然在一定程度上能够处理噪声问题，但由于估计和控制过程相互独立，没有充分考虑二者之间的相互影响，难以在双重噪声环境下实现最优的控制性能。
模型预测控制（MPC）作为一种先进的控制策略，具有对系统模型要求不高、能够显式处理约束以及良好的动态性能等优点。滚动时域估计（MHE）则是一种基于优化方法的估计技术，它利用过去一段时间内的观测数据，通过滚动优化来估计当前系统的状态。将 MPC 与 MHE 集成，能够充分发挥二者的优势，实现估计与控制的联合优化，从而更好地应对传感器和执行器双重噪声带来的挑战。
本文提出一种基于 MPC 与 MHE 集成的目标点镇定方法，将传感器噪声和执行器噪声同时纳入优化框架，通过联合优化实现“估计–控制闭环”，以提高机器人在双重噪声环境下的目标点镇定性能。
二、问题描述
2.1 控制目标
2.2 系统模型
2.3 噪声模型
三、系统流程
3.1 传感
测距 - 测角传感器对机器人的状态进行测量，输出带有高斯噪声的“真值”观测 zk。这些带噪观测数据将作为 MHE 的输入，用于估计机器人的当前状态。
3.2 估计
MHE 利用过去一段观测序列 {zk−Ne+1,zk−Ne+2,⋯,zk}（其中 Ne 为估计时域长度），通过滚动求解一个优化问题，得到当前状态 x^k 的最优估计。MHE 的优化目标是在满足系统模型和观测方程的约束下，最小化估计误差和过程噪声的加权和。
3.3 控制
MPC 以 MHE 估计得到的当前状态 x^k 为初始状态，优化未来 N 步（预测时域长度）的控制序列 {uk,uk+1,⋯,uk+N−1}，使得机器人在该控制序列的作用下趋向目标位姿 xs。MPC 的优化目标通常包括两部分：一是使预测时域内的状态与目标位姿之间的二次误差最小化，二是使控制量相对于零输入（us=0）的二次误差最小化，以实现“到达并稳定在目标点”的控制要求。优化完成后，仅实施控制序列的第一项 uk。
3.4 执行
底层控制器接收 MPC 输出的控制指令 uk，但在执行过程中会再次叠加高斯噪声 wk，得到实际执行的控制输入 uk，从而驱动机器人运动。
3.5 闭环
受扰动后的实际状态 xk+1 又通过带噪传感器进行量测，得到新的观测值 zk+1，然后回到第 2 步的 MHE 环节，形成“估计 - 控制 - 执行 - 观测”的闭环循环。通过不断循环这个过程，机器人能够在传感器与执行器双重噪声的影响下，逐步调整自身的运动，最终收敛到目标点 xs。
四、实现要点
4.1 MPC 问题的转化
4.2 MPC 目标函数
MPC 的目标是让机器人到达目标位姿 xs，并稳定在该点。因此，目标函数设计为在整个预测时域内，最小化状态与目标位姿之间的二次误差；同时，为了使机器人到达目标点后能够稳定下来，将控制量相对于零输入（us=0）的二次误差也加入目标函数。具体目标函数如下：
4.3 约束条件
4.4 CASADI 求解
CASADI 是一种用于非线性优化和数值计算的开源工具，它提供了高效的符号计算和自动微分功能，能够方便地求解非线性规划问题。利用 CASADI 将上述 MPC 问题进行建模，定义优化变量、目标函数和约束条件，然后调用其内置的优化求解器（如 IPOPT）对非线性规划问题进行求解，得到最优的控制序列。
五、仿真验证
为了验证所提基于 MPC 与 MHE 集成的目标点镇定方法的有效性，进行了一系列仿真实验。在仿真中，设置机器人的初始状态为随机值，目标位姿 xs 为固定值。传感器噪声和执行器噪声均设置为高斯白噪声，其协方差矩阵根据实际情况进行设定。
仿真结果表明，在传感器与执行器双重噪声的影响下，采用传统分步估计与控制方法的机器人难以准确到达目标点，且在目标点附近会出现较大的振荡。而采用本文所提的基于 MPC 与 MHE 集成的方法，机器人能够逐渐调整自身的运动，克服噪声的干扰，最终稳定地收敛到目标点 xs，验证了该方法在双重噪声环境下的有效性和鲁棒性。
六、结论
本文提出了一种基于 MPC 与 MHE 集成的目标点镇定方法，将传感器噪声和执行器噪声同时纳入联合优化框架，实现了“估计–控制闭环”。通过采用多重打靶法将 MPC 问题转化为非线性规划，并利用 CASADI 工具进行求解，有效地解决了非线性 MPC 问题的求解难题。仿真实验结果表明，该方法在传感器与执行器双重噪声环境下，能够保证机器人最终收敛到目标点，具有较好的控制性能和鲁棒性。未来的研究可以进一步考虑更复杂的系统模型和噪声特性，以及如何优化算法的计算效率，以满足实时控制的要求。
📚第二部分——运行结果
部分代码：
% plot the ground truth
figure(1)
subplot(311)
plot(t,xx_G(1,1:end-1),'-.b','linewidth',1.5); hold on
ylabel('x (m)')
grid on
subplot(312)
plot(t,xx_G(2,1:end-1),'-.b','linewidth',1.5);hold on
ylabel('y (m)')
grid on
subplot(313)
plot(t,xx_G(3,1:end-1),'-.b','linewidth',1.5); hold on
xlabel('time (seconds)')
ylabel('\theta (rad)')
grid on
% Plot the cartesian coordinates from the measurements used
figure(1)
subplot(311)
plot(t,y_measurements(1:end-1,1).*cos(y_measurements(1:end-1,2)),'r','linewidth',1.5); hold on
grid on
legend('Ground Truth','Measurement')
subplot(312)
plot(t,y_measurements(1:end-1,1).*sin(y_measurements(1:end-1,2)),'r','linewidth',1.5); hold on
grid on
% plot the ground truth mesurements VS the noisy measurements
figure(2)
subplot(211)
plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on
plot(t,y_measurements(1:end-1,1),'r','linewidth',1.5); hold on
ylabel('Range: [ r (m) ]')
grid on
legend('Ground Truth','Measurement')
subplot(212)
plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on
plot(t,y_measurements(1:end-1,2),'r','linewidth',1.5);hold on
ylabel('Bearing: [ \alpha (rad) ]')
grid on
figure(1)
subplot(311)
plot(t,xx(1,1:end-1),'--g','linewidth',1.5); hold on
ylabel('x (m)')
grid on
subplot(312)
plot(t,xx(2,1:end-1),'--g','linewidth',1.5);hold on
ylabel('y (m)')
grid on
subplot(313)
plot(t,xx(3,1:end-1),'--g','linewidth',1.5); hold on
xlabel('time (seconds)')
ylabel('\theta (rad)')
grid on
🎉第三部分——参考文献
文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)
🌈第四部分——Matlab代码、文档下载
资料获取，更多粉丝福利，MATLAB|Simulink|Python资源获取
