欢迎来到本博客❤️❤️博主优势博客内容尽量做到思维缜密逻辑清晰为了方便读者。完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击本文完整资源下载⛳️座右铭行百里者半于九十。⛳️赠与读者做科研涉及到一个深在的思想系统需要科研者逻辑缜密踏实认真但是不能只是努力很多时候借力比努力更重要然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览免得骤然跌入幽暗的迷宫找不到来时的路它不足为你揭示全部问题的答案但若能解答你胸中升起的一朵朵疑云也未尝不会酿成晚霞斑斓的别一番景致万一它给你带来了一场精神世界的苦雨那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。或许雨过云收神驰的天地更清朗.......第一部分——内容介绍基于李雅普诺夫模型预测控制的自主水下航行器轨迹跟踪控制摘要自主水下航行器AUV是海洋勘探、水下管线巡检、海洋环境监测等任务的核心装备轨迹跟踪能力直接决定水下作业的完成质量。水下复杂流场扰动、航行器本体强耦合非线性动力学、推进器输出幅值限制等多重约束给高精度轨迹跟踪控制器设计带来巨大挑战。传统线性控制难以适配曲线参考轨迹常规反步控制固定增益调节方式动态响应偏弱标准模型预测控制缺少完备稳定证明三类主流方案均存在明显短板。本文面向水平面运动的观测级 AUV提出一套融合李雅普诺夫理论与模型预测控制的 LMPC 一体化跟踪控制框架。该方案将推进器推力分配优化嵌入滚动时域优化求解过程无需单独设计推力分配模块可同步处理推进器饱和、运动状态约束等工程硬限制。依托非线性反步控制构造李雅普诺夫收缩约束从理论层面严格保证优化问题的递推可行性与闭环系统全局渐近稳定完整推导系统吸引域的显式表达形式规避传统模型预测控制依赖局部线性化的固有缺陷。滚动时域迭代的反馈结构赋予控制器天然抗扰动能力有效抑制模型参数失配、海洋洋流带来的跟踪误差。本文从理论层面给出三组核心创新一是构建适配 AUV 非线性动力学的 LMPC 跟踪控制算法同步优化跟踪精度与系统鲁棒性二是摆脱标准 MPC 局部线性化假设完整推导稳定性充分条件并量化系统稳定吸引范围三是算法兼容优化次优解可根据水下嵌入式硬件算力灵活平衡计算负荷与控制性能。基于 Saab SeaEye Falcon 水下航行器实测动力学模型完成多工况仿真测试分别采用正弦连续轨迹、高曲率八字形轨迹开展对比试验同时叠加 30% 模型参数偏差与恒定海流扰动完成鲁棒性验证。仿真数据表明相较于传统反步控制本文所提 LMPC 方案位置、艏向跟踪均方误差最高可降低 99.5%收敛速度大幅提升在强扰动环境下仍能维持高精度轨迹跟踪效果。关键词自主水下航行器轨迹跟踪模型预测控制李雅普诺夫稳定理论反步控制推力分配鲁棒控制一、引言1.1 研究背景与应用价值海洋资源开发、水下基础设施维护、海洋生态调查等水下作业场景对无人水下装备的自主化、高精度运动控制提出了更高要求。自主水下航行器无需人工线缆拖拽、无需操作人员实时远程操控能够长时间潜入水下完成大范围持续性作业可显著降低载人潜水器的作业风险与综合运营成本近年来得到海洋工程、控制工程领域学者与海洋装备企业的广泛关注。自主航行的核心支撑是高性能运动控制系统其中轨迹跟踪是完成水下定点观测、管线跟随、区域巡航等任务的基础功能。航行器在水平面包含纵向、横向、艏向三组耦合运动流体附加质量、非线性阻尼、科氏向心力会带来强非线性耦合动力学特性同时水下洋流、海水密度分层、水底地形扰动会产生时变未知外部干扰实际工程中推进器输出推力存在固定幅值上限多重复杂条件叠加大幅提升轨迹跟踪控制器设计难度。1.2 现有控制方法研究现状当前国内外针对 AUV 轨迹跟踪的控制方案主要分为线性控制、非线性连续控制、模型预测控制三大类各类方法存在各自适用场景与局限性。第一类为线性控制方法包含比例积分微分控制、线性二次调节器等。该类控制器结构简单、参数调试便捷仅适用于点位式分段直线跟踪任务此时航行器运动偏差较小系统可近似满足局部线性化假设。若目标轨迹为连续曲线、大曲率转弯轨迹航行器大范围运动将充分激发系统非线性耦合特性线性控制器会出现跟踪滞后、稳态误差激增甚至失稳问题无法满足复杂水下巡航任务需求。第二类为非线性连续控制主流分为反步控制与滑模控制。基于李雅普诺夫的反步控制是目前水下航行器跟踪控制应用最广泛的方案能够主动利用系统动力学内部有利非线性项相比反馈线性化控制拥有更强的基础鲁棒性现有研究已基于完整六自由度运动学、动力学模型设计全局镇定反步律同时拓展出输出反馈反步控制结构。但传统反步控制器采用固定控制增益增益选取存在天然矛盾增大增益可加快误差收敛速度但会缩小系统稳定吸引域减小增益能够扩大稳定范围却会造成动态响应缓慢、扰动抑制能力不足。滑模控制依靠滑动面对模型不确定性的不敏感特性实现抗扰控制然而原生滑模存在高频抖振现象会加剧推进器机械损耗自适应滑模、高阶滑模虽可削弱抖振但控制器参数整定流程复杂多约束场景下无法直接处理推进器饱和限制。第三类为模型预测控制该方法以有限时域滚动优化为核心能够在控制器设计阶段直接引入执行器幅值、运动状态等各类约束是处理带约束非线性系统的理想控制框架。现有 AUV 模型预测控制研究分为线性 MPC 与非线性 MPC线性 MPC 搭配智能优化算法求解二次规划仅适用于低速小范围运动非线性模型预测控制可同步完成路径规划与轨迹跟踪针对在线计算量大的痛点现有研究提出数值加速算法、分布式分解求解等优化手段。但非线性模型预测控制存在难以弥补的理论缺陷有限预测时域下优化目标最优无法等价保证闭环系统稳定标准稳定证明手段依赖系统局部线性化而曲线轨迹跟踪场景线性化假设完全失效缺少完备的稳定性理论支撑极大限制了 NMPC 在高精度水下航行器中的实际落地。1.3 现有研究存在的关键问题综合现有文献成果当前 AUV 轨迹跟踪控制仍存在四点亟待解决的核心痛点传统非线性连续控制器无法显式处理推进器推力饱和约束极限工况下易出现控制输出超限、系统振荡标准非线性模型预测控制缺少不依赖局部线性化的稳定证明体系无法量化系统稳定吸引范围推力分配与轨迹跟踪控制分层设计两层控制器独立优化易出现推力输出冲突跟踪性能存在损失嵌入式水下控制器算力有限现有优化控制算法对求解精度要求严苛迭代次数受限后极易出现系统失稳。1.4 本文研究内容与创新点针对上述现存问题本文以水平面观测级 AUV 为研究对象提出基于李雅普诺夫理论的模型预测控制框架完整解决带约束非线性航行器轨迹跟踪的稳定与性能优化问题本文三大核心创新如下一体化 LMPC 控制框架设计将推力分配子问题整合进滚动时域优化目标实现轨迹跟踪、推力分配同步求解依靠在线优化充分利用推进器输出上限大幅提升动态跟踪性能与抗扰动能力无局部线性化稳定理论推导借助反步控制构造李雅普诺夫收缩约束规避标准 MPC 的线性化依赖严格推导优化问题递推可行的充分条件解析量化系统全局保证吸引域兼容次优解的优化架构控制器稳定性、可行性不受优化求解精度影响仅跟踪性能随求解精度变化可根据水下硬件算力灵活调整优化迭代次数实现计算成本与控制效果的动态折中。1.5 论文整体结构安排本文各章节内容安排如下第二章建立目标 AUV 的完整运动学、动力学数学模型梳理模型固有正定、斜对称等关键数学性质为控制器设计提供理论基础第三章构建融合推力分配的 LMPC 滚动时域优化问题给出完整离散滚动控制执行流程第四章基于反步法设计辅助李雅普诺夫镇定控制器完成优化问题递推可行性、闭环系统渐近稳定性完整理论证明第五章设置两组典型曲线轨迹在无扰动、强模型失配叠加海流扰动两类工况下完成仿真对比量化分析 LMPC 与传统反步控制的跟踪误差、动态响应、抗扰性能差异第六章总结全文研究成果规划后续实体样机试验与算法融合优化的研究方向。二、水下航行器系统建模与模型特性分析2.1 试验平台与运动简化说明本文选取 Saab SeaEye Falcon 观测型水下航行器作为研究对象该装备搭载四台固定方位推进器推进器布局仅能实现水平面纵向、横向平移与艏向回转运动无法主动补偿航行过程中产生的横摇、纵倾姿态角因此本文仅针对水平面二维轨迹跟踪问题开展建模与控制器设计忽略垂向、横摇、纵倾三维自由度运动。为完整描述航行器运动状态建立两套正交参考坐标系一是惯性大地坐标系用于表征航行器全局平面位置与艏向角二是固连于航行器重心的载体坐标系用于描述航行器自身相对纵向、横向、艏向角速度。航行器完整运动模型分为坐标变换运动学模型、力与角速度耦合动力学模型两层同时结合推进器推力分配关系整合得到统一六维状态非线性状态方程。2.2 航行器运动学与动力学模型运动学模型用于描述大地坐标系位置姿态与载体坐标系角速度之间的坐标转换关系依靠旋转矩阵完成两组坐标系的映射能够直观表征航行器位置变化速率与自身运动速度的耦合关系。动力学模型基于牛顿刚体运动定律建立完整包含航行器本体惯性质量、水下流体附加惯性质量、科氏向心力与离心力、线性与非线性流体阻尼、水下静恢复力、推进器广义推力六部分物理项。其中惯性矩阵具备对称正定有界特性科氏力矩阵满足斜对称特性阻尼矩阵恒正定静恢复力幅值存在固定上界以上固有数学性质是后续李雅普诺夫稳定性证明的关键理论支撑。四台推进器输出推力通过固定分配矩阵线性映射为航行器纵向、横向、艏向三维广义控制力与力矩结合运动学、动力学微分关系整合得到航行器完整非线性状态演化模型模型输入为四台推进器输出推力状态包含平面位置、艏向角、三轴角速度六类物理量。2.3 模型固有关键数学性质总结通过对完整动力学模型矩阵项的数学推导总结六条不随航行运动状态变化的固有性质包含附加质量的惯性矩阵对称正定所有矩阵元素存在统一上界科氏向心力矩阵满足斜对称特性矩阵与其转置相加为零矩阵姿态旋转矩阵的逆矩阵等于自身转置矩阵坐标变换过程不改变速度向量二范数大小流体阻尼矩阵恒正定保证航行无外力输入时运动速度会自然衰减推力分配矩阵的乘积矩阵可逆保证任意目标广义推力力矩均可通过推进器推力组合实现水下静恢复力的输出幅值存在全局固定上界不会随航行位置无限增大。上述性质全程贯穿控制器设计、可行性推导、稳定性证明全过程能够大幅简化李雅普诺夫函数导数的放缩推导为理论证明提供严谨数学支撑。三、基于李雅普诺夫模型预测的轨迹跟踪控制框架3.1 光滑参考轨迹与全维度参考状态构造本文假设目标跟踪平面轨迹及其一至三阶时间导数连续光滑、全局有界该假设符合水下巡航、管线跟踪等实际任务的轨迹设计标准。仅给出二维平面位置参考无法匹配航行器六维状态模型因此基于位置轨迹导数构造完整参考状态序列包含期望艏向角、期望纵向速度、期望回转角速度横向参考速度固定为零匹配水平面常规巡航运动需求。结合航行器动力学模型反向推导跟踪参考所需的广义推力力矩再通过推力分配矩阵的摩尔 - 彭罗斯伪逆求解四台推进器对应的参考推力输入得到完整参考控制量。从数学角度可严格证明在轨迹光滑有界假设下全部参考状态、参考控制量均存在全局幅值上界为后续稳定性证明中变量放缩提供基础条件。3.2 标准非线性模型预测控制及其缺陷传统非线性模型预测控制以跟踪误差、控制输入代价加权积分为优化目标约束包含系统状态演化方程、初始状态匹配条件、推进器推力饱和幅值限制。通过滚动时域求解有限区间内最优控制序列仅执行第一个采样周期控制量下一时刻重新采集状态更新优化问题实现闭环滚动控制。该基础架构存在无法规避的理论短板有限长度预测时域下优化目标函数最小化仅能保证有限区间内性能最优无法直接推导闭环系统全局稳定现有稳定改进方案均需要在目标轨迹工作点做系统局部线性化构造辅助线性镇定控制器并添加终端约束而连续曲线跟踪过程中航行器工作点持续大范围变化局部线性化假设完全失效终端约束设计不再具备理论有效性无法保证航行器跟踪曲线轨迹时的系统稳定。3.3 带李雅普诺夫收缩约束的 LMPC 优化问题为解决标准 NMPC 稳定缺陷本文在原有优化约束基础上新增李雅普诺夫收缩不等式约束构建全新 LMPC 优化架构。收缩约束依托稳定的反步辅助控制器与对应李雅普诺夫函数设计其核心物理含义为优化求解得到的控制输入对应的李雅普诺夫函数下降速率至少不弱于辅助镇定控制器的收敛速率。优化目标保留状态跟踪误差、控制推力幅值加权积分项与终端误差惩罚项约束完整包含航行器非线性状态演化方程、初始状态匹配条件、推进器输出饱和上限、李雅普诺夫收缩约束四组条件。收缩约束使得滚动优化过程天然继承辅助反步控制器的全局稳定特性无需对系统做任何线性近似处理完美适配大曲率曲线轨迹跟踪场景。3.4 LMPC 滚动时域实时控制算法本文设计离散采样滚动执行算法完整控制执行流程分为五步第一步提前离线确定优化目标加权矩阵、预测时域长度、采样周期、推进器推力上限等固定控制器参数第二步实时采集水下航行器当前全部六维运动状态第三步以当前实测状态为初始条件求解带收缩约束的 LMPC 优化问题得到一段有限时域内推进器推力控制序列算法允许输出局部次优解不强制要求全局最优第四步仅截取控制序列第一个采样周期的推力指令下发至推进器执行第五步等待一个采样周期后重新采集航行器状态循环迭代完成全程轨迹跟踪控制。算法配套补充说明优化求解的全局最优解仅影响轨迹跟踪精度不会改变系统可行性与闭环稳定性。该特性适配水下嵌入式硬件算力受限的工程场景操作人员可根据硬件计算速度限制优化迭代次数牺牲少量跟踪精度换取更快在线求解速度不会引发航行器失稳、振荡等安全问题大幅提升算法工程落地适配性。四、LMPC 控制器递推可行性与闭环稳定性理论分析4.1 基于反步法的辅助李雅普诺夫镇定控制器设计为构造 LMPC 优化所需的收缩约束本文采用反步设计法设计无约束辅助跟踪控制器该控制器仅用于稳定性证明实际航行过程中不会下发至推进器执行。首先定义航行器平面位置跟踪误差、速度误差变换变量分步构造两层李雅普诺夫函数第一层李雅普诺夫函数仅针对平面位置跟踪误差第二层复合李雅普诺夫函数融合位置误差与速度误差项。对复合李雅普诺夫函数求时间导数代入航行器完整动力学模型借助模型斜对称、正定固有性质化简导数表达式反向推导得到广义推力力矩控制律再通过推力分配伪逆映射得到推进器辅助推力指令。数学推导可证明该反步辅助控制器能够使航行器轨迹跟踪误差全局渐近收敛至零李雅普诺夫函数导数恒小于等于零具备完备全局镇定能力可作为收缩约束的理论基准控制器。4.2 LMPC 优化问题递推可行性证明递推可行性是滚动时域控制稳定运行的前提代表每一个采样时刻优化问题均存在满足全部约束的可行控制解。本文假设四台推进器输出幅值上限完全一致结合推力分配矩阵范数有界特性推导广义推力力矩上限与单台推进器推力上限的映射关系给出推力分配始终满足饱和约束的充分条件。基于反步辅助控制器的全局有界特性证明任意航行初始状态下辅助控制器输出推力始终满足推进器饱和约束代表优化问题天然存在一组可行初始解。进一步推导航行状态、参考轨迹全部有界条件下科氏矩阵、阻尼矩阵的全局幅值上界结合反步控制增益参数完整给出保证优化问题全程递推可行的充分不等式条件同时量化给出系统稳定保证吸引域的显式表达范围。只要航行初始状态落在该吸引域内部全程滚动优化均存在可行解控制器可持续稳定工作。4.3 闭环系统全局渐近稳定性证明依托辅助反步控制器对应的径向无界、连续可微李雅普诺夫函数结合非线性系统逆李雅普诺夫定理存在一组标准无穷阶类 K 函数能够对李雅普诺夫函数及其导数做上下界放缩。LMPC 优化内部的收缩约束保证每一步滚动优化得到的实际控制输入都会使李雅普诺夫函数下降速率优于辅助镇定控制器。结合辅助控制器的负定导数特性可直接推导出闭环系统李雅普诺夫函数导数严格负定满足全局渐近稳定判定条件。由此可完整证明在轨迹光滑有界、推进器推力上限满足可行性条件前提下本文 LMPC 滚动控制算法驱动的 AUV 闭环系统全局渐近稳定航行器平面位置、艏向角、三轴角速度跟踪误差将持续收敛至零最终稳定跟随目标参考轨迹。同时可得到工程优化结论减小反步辅助控制器的增益矩阵数值能够进一步扩大系统稳定吸引域兼顾大范围初始偏差跟踪与系统稳定。五、仿真试验与性能对比分析5.1 仿真试验参数与测试工况设置所有仿真试验均基于 Saab SeaEye Falcon 航行器实测辨识动力学模型搭建设置两组差异化目标参考轨迹分别对应低速平滑巡航、高曲率复杂机动两类典型水下任务场景第一组为单频正弦平滑轨迹用于常规匀速巡航工况测试第二组为双频组合八字形高曲率轨迹用于大角度连续转弯机动工况测试。统一设置 LMPC 控制器采样周期、预测时域长度、误差加权矩阵、控制推力惩罚矩阵、终端误差惩罚矩阵单台推进器最大输出推力统一设置为 500 牛对比对照组采用同增益参数的传统反步跟踪控制器两组控制器初始航行状态完全一致保证对比试验公平性。仿真分为两大测试工况工况一为理想无扰动环境无模型参数偏差、无外部洋流干扰用于对比基础动态收敛速度、稳态跟踪误差工况二为强鲁棒性测试工况叠加 30% 全维度动力学模型参数失配同时施加恒定横向、纵向洋流推力扰动模拟真实复杂水下作业环境验证两种控制器的抗干扰能力。优化求解采用序列二次规划算法离散化后求解优化问题的 KKT 最优条件。5.2 无扰动工况跟踪性能对比理想无扰动环境下传统反步控制器与本文 LMPC 控制器均能够最终收敛至目标参考轨迹验证两类控制器的基础稳定能力但动态跟踪性能存在显著差距。从轨迹平面曲线可以直观看出LMPC 控制器收敛速度远快于传统反步控制初始位置偏差消除耗时大幅缩短跟踪初期 LMPC 控制器会充分利用推进器最大输出推力快速抵消跟踪误差推力指令全程严格控制在饱和上限以内不存在超限振荡传统反步控制器受固定增益限制误差衰减缓慢轨迹跟踪滞后现象明显。在八字形高曲率轨迹转弯拐点处反步控制器跟踪滞后误差显著增大而 LMPC 依靠滚动时域提前预判轨迹变化提前调整推进器推力拐点处跟踪偏差始终维持在极低水平复杂机动场景下优势更加突出。5.3 强扰动工况鲁棒性能定量分析叠加模型参数失配与海流扰动后传统反步控制器跟踪性能大幅衰减固定增益无法自适应补偿外部扰动与模型偏差平面位置、艏向角持续存在较大稳态跟踪误差本文 LMPC 控制器依靠滚动时域在线迭代优化能够实时根据当前跟踪误差动态调整推进器推力持续抵消未知扰动带来的偏差全程维持高精度跟踪效果。通过位置、艏向跟踪均方误差量化对比正弦轨迹工况下 LMPC 位置跟踪误差降低幅度最高达到 94.9%艏向角误差降低 94.3%八字形复杂轨迹工况下横向位置均方误差降低 99.5%跟踪精度提升幅度极为显著。数据充分证明滚动时域优化架构天然赋予 LMPC 控制器更强的鲁棒性更适配存在模型不确定性、洋流扰动的真实水下作业环境。六、结论与后续研究展望6.1 全文研究结论本文针对水平面自主水下航行器的高精度轨迹跟踪控制难题提出一套融合李雅普诺夫稳定理论与模型预测控制的 LMPC 一体化控制方案完整完成建模、控制器设计、稳定性理论证明与多工况仿真验证核心研究结论总结如下所提 LMPC 框架可将推力分配优化嵌入滚动时域优化流程实现轨迹跟踪、推力分配同步求解能够直接显式处理推进器推力饱和等工程硬约束充分利用推进器输出极限提升动态响应速度依托反步控制构造李雅普诺夫收缩约束彻底摆脱标准模型预测控制依赖局部线性化的稳定证明局限完整推导优化问题递推可行、闭环系统渐近稳定的充分条件量化给出系统稳定吸引域范围理论完备性优于现有非线性 MPC 方案算法兼容优化次优解控制器稳定与可行性不受优化求解精度影响仅跟踪性能随求解迭代次数变化可灵活适配水下嵌入式低算力硬件工程实用性更强多组仿真对比试验证明无论理想无扰动环境还是强模型失配叠加洋流扰动的复杂工况LMPC 控制器相较于传统反步控制收敛速度更快、稳态跟踪误差更小鲁棒跟踪性能具备压倒性优势可满足各类高精度水下巡航作业需求。6.2 后续研究方向规划基于本文现有理论与仿真成果后续将从实体样机试验、算法融合优化、轻量化改进三个维度开展深入研究实体样机水池与湖试验证依托 Saab SeaEye Falcon 实体水下航行器搭建试验平台将本文 LMPC 算法移植至嵌入式控制器通过水池、湖上实船试验验证算法在真实硬件、真实水流环境下的实际控制效果完善工程参数整定方案多控制算法融合优化结合滑模控制、鲁棒 H∞控制、模糊自适应控制等先进非线性控制理论对 LMPC 框架进行改进进一步强化系统对时变扰动、未知大模型失配的抑制能力在线优化轻量化改进针对水下嵌入式硬件算力瓶颈研究分布式分解、快速数值迭代求解算法降低 LMPC 在线优化的计算耗时适配小型低成本观测级 AUV 的实时控制需求。第二部分——运行结果原文主函数代码%% ------------------------------------------------------------------------ % 代码功能基于线性模型预测控制(LMPC)的自主水下航行器(AUV)轨迹跟踪仿真 % 对比两种控制器非线性反步控制器(Nonlinear BS Control)、LMPC控制器 % 可选择参考轨迹二维正弦轨迹 / 八字形轨迹 % 输出曲线跟踪轨迹、控制力、推进器推力、位置艏向时序、均方误差MSE指标 % ------------------------------------------------------------------------- % 添加工具函数文件夹路径包含AUV模型、控制器、轨迹生成类定义 addpath(./utilis/,./ch4header/) clc; % 清空命令行窗口 clear; % 清空工作区所有变量 close all; % 新增关闭所有已有绘图窗口防止多图重叠混淆 %% % 一、AUV动力学系统参数配置 %% err_model0; % 模型失配系数0代表无模型不确定性可设非零模拟参数扰动 m116; % AUV本体质量(kg) Iz13.1; % AUV绕z轴转动惯量(kg·m²) % 附加质量相关水动力系数 (附加质量矩阵对角线元素) X_udot-167.6*(1err_model); % 纵向附加质量系数 Y_vdot-477.2*(1err_model); % 横向附加质量系数 N_rdot-15.9*(1err_model); % 艏向转动附加惯量系数 % 线性阻尼水动力系数 Xu26.9*(1err_model); % 纵向线性阻尼 Yv35.8*(1err_model); % 横向线性阻尼 Nr3.5*(1err_model); % 艏向转动线性阻尼 % 二次阻尼水动力系数 Du241.3*(1err_model); % 纵向二次阻尼 Dv503.8*(1err_model); % 横向二次阻尼 Dr76.9*(1err_model); % 艏向转动二次阻尼 % 广义质量矩阵对角元素 (M diag([Mx, My, Mψ])) Mxm-X_udot; % 纵向等效质量 Mym-Y_vdot; % 横向等效质量 MpsiIz-N_rdot; % 艏向等效转动惯量 % 推进器分配矩阵B4个推进器输出映射为3维控制力矩[τ_u,τ_v,τ_r] % 维度3行4列输入4个推进器推力u1~u4输出纵向力、横向力、艏向力矩 B[0.7974 0.8643 0.8127 0.8270 0.6032 0.5029 -0.5824 -0.5610 0.2945 -0.3302 -0.2847 0.3505]; Bpluspinv(B); % 求B的伪逆矩阵由期望控制力矩反解4个推进器推力 Mdiag([Mx;My;Mpsi]);% 广义质量矩阵3×3对角阵 PI[0,-1,0;1,0,0;0,0,0];% 坐标转换斜对称矩阵用于机体-大地坐标系转换 g0; % 重力/浮力项水平面运动置0 Lambdadiag([1;1;1]);% 权重对角矩阵本例未启用 % 外部环境扰动向量 [纵向扰动力,横向扰动力,艏向扰动力矩] % disturbance[10;10;0]; % 带扰动工况 disturbance[0;0;0]; % 无扰动仿真工况 %% % 二、创建两台AUV仿真对象 % auv1用于非线性反步控制器仿真 % auv2用于LMPC控制器仿真 %% coef [m; Iz; X_udot; Y_vdot; N_rdot; Xu; Yv; Nr; Du; Dv; Dr];% 整合所有水动力参数向量 ndof 3; % 自由度水平面3自由度(纵移u,横移v,艏摇r) X0[0.5;0;0;0;0;0]; % AUV初始状态向量 [x,y,ψ,u,v,r] Thrust0 [0;0;0;0]; % 初始4推进器推力全为0 auv1 AUV(coef, ndof, B, X0, Thrust0);% 反步控制仿真载体 auv2 AUV(coef, ndof, B, X0, Thrust0);% LMPC控制仿真载体 %% % 三、初始化非线性反步控制器(BS Controller) %% Kpdiag([1;1;1]); % 比例增益矩阵 位置环P增益 Kddiag([1;1;1]); % 微分增益矩阵 速度环D增益 bs1 nonlinearBS_controller(Kp,Kd);% 实例化反步控制器对象 %% % 四、生成参考轨迹对象二选一 %% % Case 1: 二维正弦参考轨迹参数(幅值,周期,初始相位) ref_trajectory sine2D(1,0.5,0); % Case 2: 八字形参考轨迹参数(幅值,纵向周期,横向缩放,初始相位) % ref_trajectory eight_shape2D(1,0.5,0.25,0); %% % 五、非线性反步控制 完整仿真循环 %% % 参考轨迹信息向量9维 [x,y,ψ,dx,dy,dψ,ddx,ddy,ddψ] 位置/速度/加速度 ref [ref_trajectory.xR;ref_trajectory.yR;ref_trajectory.psiR;... ref_trajectory.xRd;ref_trajectory.yRd;ref_trajectory.psiRd;... ref_trajectory.xRdd;ref_trajectory.yRdd;ref_trajectory.psiRdd]; dt0.1; % 仿真步长 0.1s Tstep200; % 总仿真步数200步总时长200*0.120s nxlength(X0); % 状态维度6维 [x,y,ψ,u,v,r] nu3; % 虚拟控制输入维度3维 [τu,τv,τr] n_thrust4; % 物理推进器数量4个 % 预分配存储矩阵加速仿真 Xallzeros(nx,Tstep1);% auv1全部状态时序 (6行201列) Uallzeros(nu,Tstep); % 反步控制器输出虚拟力矩 (3行200列) Thrust_allzeros(n_thrust,Tstep);% 反步控制下4推进器实际推力(4行200列) eta_Ref_allzeros(9,Tstep);% 全时序参考轨迹9维信息 (9行200列) Xall(:,1)X0; % 仿真初始状态赋值 XplusX0; % 反步控制仿真循环 for i1:1:Tstep eta_Ref_all(:,i) ref;% 保存当前时刻参考轨迹 tau bs1.calc_control(ref,auv1.X);% 反步控制器计算期望控制力矩τ Uall(:,i) tau; % 记录虚拟控制力矩 thrust Bplus*tau; % 伪逆分配力矩转4推进器推力 Thrust_all(:,i) thrust; % 保存推进器推力 auv1.advance(thrust,disturbance,dt);% AUV动力学一步积分更新状态 Xall(:,i1) auv1.X; % 保存更新后AUV状态 ref_trajectory.update(dt); % 更新参考轨迹时间生成下一时刻轨迹 ref_trajectory.t; % 读取当前轨迹时间(仅查看无赋值) % 刷新下一时刻完整参考轨迹9维数据 ref [ref_trajectory.xR;ref_trajectory.yR;ref_trajectory.psiR;... ref_trajectory.xRd;ref_trajectory.yRd;ref_trajectory.psiRd;... ref_trajectory.xRdd;ref_trajectory.yRdd;ref_trajectory.psiRdd]; end % 绘图1先绘制参考轨迹与反步控制轨迹(后续LMPC曲线叠加) figure(1) plot(eta_Ref_all(1,:),eta_Ref_all(2,:),k); hold on; plot(Xall(1,:),Xall(2,:),b); hold on; %% % 六、LMPC模型预测控制器参数初始化 %% N5; % LMPC预测时域长度5步 Tstep2 Tstep-N; % LMPC有效仿真步数扣除预测时域前置步数 % 状态代价权重矩阵Q6维状态[x,y,ψ,u,v,r]权重 q111e5; % x位置跟踪权重(最大优先保证位置跟踪) q221e5; % y位置跟踪权重 q331e3; % 艏向角ψ跟踪权重 q441e2; % 纵向速度u权重 q551e2; % 横向速度v权重 q661e2; % 艏摇角速度r权重 Qdiag([q11,q22,q33,q44,q55,q66]); % 控制输入代价权重R4个推进器推力权重 r111e-4; r221e-4; r331e-4; r441e-4; Rdiag([r11,r22,r33,r44]); R10; % 备用控制权重本代码未使用 % 终端代价权重Qf预测时域终点状态惩罚权重 qf111e3; qf221e3; qf331e2; qf441e1; qf551e1; qf661e1; Qfdiag([qf11,qf22,qf33,qf44,qf55,qf66]); weights {Q,R,Qf}; % 打包代价权重传入LMPC类 % 推进器推力上下限约束 T1_max 500; T2_max 500; T3_max 500; T4_max 500; Thrust_max0 [T1_max;T2_max;T3_max;T4_max];% 推力上限 Thrust_min0 [-T1_max;-T2_max;-T3_max;-T4_max];% 推力下限(反向推力) internal_model AUV(coef, ndof, B, X0, Thrust0);% LMPC内部预测AUV模型 auxiliary_controller nonlinearBS_controller(Kp,Kd);% LMPC初始可行解辅助反步控制器 lmpc1 LMPC_controller(N,internal_model,auxiliary_controller,weights,Thrust_max0,Thrust_min0);% 实例化LMPC控制器 %% % 七、LMPC控制器仿真循环 %% Xall2 zeros(nx,Tstep21); % LMPC控制下AUV全状态时序 Uall2 zeros(nu,Tstep2); % LMPC输出虚拟控制力矩τ Thrust_all2 zeros(n_thrust,Tstep);% LMPC下4推进器推力时序 Xall2(:,1) X0; % LMPC仿真初始状态 U0 Uall2(:,1); Thrust0 Bplus*U0; % 初始推力 M nx nu n_thrust; % 参考轨迹生成函数参数维度 t 0; % 仿真时间计数器 u0 zeros(n_thrust*N,1); % LMPC优化初始猜测序列(预测时域内所有推力) tic; % 计时统计LMPC仿真总耗时 % LMPC主仿真循环 for i1:1:Tstep2 t1 t; TIme(i,1) t; % 记录每一步仿真时间戳 P RefGen(M,N,t1,dt,coef,Bplus,ref_trajectory);% 生成预测时域内参考轨迹序列 u lmpc1.calc_control(P,X0,B,u0,dt,ref_trajectory,t1);% LMPC求解优化问题得到预测时域推力序列 u_actual u(1:n_thrust,1);% 取预测序列第一个推力作为当前实际控制量 Thrust_all2(:,i) u_actual;% 保存当前推进器推力 tau_actual B*u_actual; % 推力映射为AUV控制力矩τ Uall2(:,i) tau_actual; % 保存虚拟控制力矩 auv2.advance(u_actual,disturbance,dt);% AUV动力学更新状态 Xall2(:,i1) auv2.X; % 记录更新后状态 X0 auv2.X; % 刷新当前状态给下一时刻LMPC % 基于反步控制器生成下一时刻优化初始可行猜测解加速fmincon求解 t2 tdt; P_ RefGen(M,N,t2,dt,coef,Bplus,ref_trajectory); u0 lmpc1.calc_initial_guess(P_,X0,Bplus,dt); i; % 控制台输出当前循环步数观测仿真进度 t t dt; % 仿真时间向前推进一个步长 end toc; % 输出LMPC仿真总耗时 % % 八、结果绘图模块多维度对比两种控制器效果 % %% 图1水平面x-y轨迹对比 figure(1) plot(eta_Ref_all(1,:),eta_Ref_all(2,:),k,LineWidth,1.2); hold on; plot(Xall(1,1:Tstep2),Xall(2,1:Tstep2),b,LineWidth,1); hold on; plot(Xall2(1,1:Tstep2),Xall2(2,1:Tstep2),r,LineWidth,1); grid on; legend(参考轨迹Ref, 非线性反步控制BS, LMPC模型预测控制,Location,best); xlabel(纵向位置 x [m]); ylabel(横向位置 y [m]); title(AUV水平面轨迹跟踪对比); %% 图2虚拟控制力矩τ时序对比(纵向力、横向力、艏向力矩) figure(2) subplot(3,1,1) plot(TIme(1:Tstep2,1),Uall(1,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Uall2(1,1:Tstep2),r); grid on; ylabel(纵向控制力 F_u [N]); legend(BS反步,LMPC预测,Location,best); title(控制器输出虚拟控制力矩时序); subplot(3,1,2) plot(TIme(1:Tstep2,1),Uall(2,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Uall2(2,1:Tstep2),r); grid on; ylabel(横向控制力 F_v [N]); subplot(3,1,3) plot(TIme(1:Tstep2,1),Uall(3,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Uall2(3,1:Tstep2),r); grid on; xlabel(仿真时间 Time [s]); ylabel(艏向控制力矩 F_r [Nm]); %% 图34个推进器实际推力时序对比 figure(3) subplot(4,1,1) plot(TIme(1:Tstep2,1),Thrust_all(1,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(1,1:Tstep2),r); grid on; ylabel(推进器1推力 u_1 [N]) title(四台推进器输出推力时序对比); subplot(4,1,2) plot(TIme(1:Tstep2,1),Thrust_all(2,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(2,1:Tstep2),r); grid on; ylabel(推进器2推力 u_2 [N]) subplot(4,1,3) plot(TIme(1:Tstep2,1),Thrust_all(3,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(3,1:Tstep2),r); grid on; ylabel(推进器3推力 u_3 [N]) subplot(4,1,4) plot(TIme(1:Tstep2,1),Thrust_all(4,1:Tstep2),b); hold on; plot(TIme(1:Tstep2,1),Thrust_all2(4,1:Tstep2),r); grid on; ylabel(推进器4推力 u_4 [N]) xlabel(仿真时间 Time [s]); %% 图4位置x、y、艏向角ψ时序跟踪曲线 figure(4) subplot(3,1,1) plot(TIme(1:Tstep2,1),eta_Ref_all(1,1:Tstep2),k); hold on; plot(TIme(1:Tstep2,1),Xall(1,1:Tstep2),b); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(1,1:Tstep2),r); ylabel(纵向位置 x [m]); title(位置与艏向角跟踪时序对比); subplot(3,1,2) plot(TIme(1:Tstep2,1),eta_Ref_all(2,1:Tstep2),k); hold on; plot(TIme(1:Tstep2,1),Xall(2,1:Tstep2),b); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(2,1:Tstep2),r); ylabel(横向位置 y [m]); subplot(3,1,3) plot(TIme(1:Tstep2,1),eta_Ref_all(3,1:Tstep2),k); hold on; plot(TIme(1:Tstep2,1),Xall(3,1:Tstep2),b); hold on; grid on; plot(TIme(1:Tstep2,1),Xall2(3,1:Tstep2),r); xlabel(仿真时间 Time [s]); ylabel(艏向角 \psi [rad]); %% 图5注释备用速度u,v,r时序曲线取消注释即可绘图 % figure(5) % subplot(3,1,1) % plot(TIme(1:Tstep2,1),Xall(4,1:Tstep2),b); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(4,1:Tstep2),r); % title(纵向速度u时序对比); % % subplot(3,1,2) % plot(TIme(1:Tstep2,1),Xall(5,1:Tstep2),b); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(5,1:Tstep2),r); % title(横向速度v时序对比); % % subplot(3,1,3) % plot(TIme(1:Tstep2,1),Xall(6,1:Tstep2),b); % hold on; % grid on; % plot(TIme(1:Tstep2,1),Xall2(6,1:Tstep2),r); % title(艏摇角速度r时序对比); % xlabel(Time [s]); %% % 九、计算两种控制器跟踪均方误差MSE量化跟踪性能 %% % 初始化x,y,ψ三个自由度均方误差累加器 MSEx10; MSEy10; MSEpsi10; % BS反步控制误差累加 MSEx20; MSEy20; MSEpsi20; % LMPC控制误差累加 for i1:1:Tstep2 % x方向误差平方累加 MSEx1MSEx1(Xall(1,i)-eta_Ref_all(1,i))^2; MSEx2MSEx2(Xall2(1,i)-eta_Ref_all(1,i))^2; % y方向误差平方累加 MSEy1MSEy1(Xall(2,i)-eta_Ref_all(2,i))^2; MSEy2MSEy2(Xall2(2,i)-eta_Ref_all(2,i))^2; % 艏向角ψ误差平方累加 MSEpsi1MSEpsi1(Xall(3,i)-eta_Ref_all(3,i))^2; MSEpsi2MSEpsi2(Xall2(3,i)-eta_Ref_all(3,i))^2; end % 求均值得到均方误差MSE MSEx1MSEx1/Tstep2; MSEx2MSEx2/Tstep2; MSEy1MSEy1/Tstep2; MSEy2MSEy2/Tstep2; MSEpsi1MSEpsi1/Tstep2; MSEpsi2MSEpsi2/Tstep2; % 命令行输出量化指标 fprintf(跟踪均方误差MSE对比\n); fprintf(非线性反步控制 BSMx%.4f, My%.4f, Mψ%.4f\n,MSEx1,MSEy1,MSEpsi1); fprintf(LMPC模型预测控制Mx%.4f, My%.4f, Mψ%.4f\n,MSEx2,MSEy2,MSEpsi2); fprintf(\n); % 移除自定义工具文件夹路径避免影响后续其他脚本 rmpath(./utilis/,./ch4header/)第三部分——参考文献文章中一些内容引自网络会注明出处或引用为参考文献难免有未尽之处如有不妥请随时联系删除。(文章内容仅供参考具体效果以运行结果为准)第四部分——本文完整资源下载资料获取更多粉丝福利MATLAB|Simulink|Python|数据|文档等完整资源获取本文完整资源下载
 获取与实战)
