怀化网站推广wordpress4.9.8火车头

怀化网站推广,wordpress4.9.8火车头,凡客有家,安徽建设项目建设工程在线12-文献代码复现#xff0c;基于非线性模型预测控制nmpc的多无人船USV编队控制formation control 。 本代码由matlab中的simulink 模块搭建#xff0c;采用船舶的fossen模型建模#xff0c;具体过程可以参见文献的内容。 输出的算例为5条船的情况如下图清晨的渔港飘着海腥味…12-文献代码复现基于非线性模型预测控制nmpc的多无人船USV编队控制formation control 。 本代码由matlab中的simulink 模块搭建采用船舶的fossen模型建模具体过程可以参见文献的内容。 输出的算例为5条船的情况如下图清晨的渔港飘着海腥味五艘无人船在晨雾中悄然启动。它们的运动轨迹看似随意却始终保持菱形编队稳定前行——这背后是NMPC算法在持续求解最优控制量。今天咱们拆解这个用Simulink实现的五船编队控制方案从动力学模型到预测控制器看看如何用代码实现海上芭蕾。船舶动力学建模打开Simulink库里的黄色模块Fossen模型的核心是这组微分方程function dxdt fossenModel(t,x,u) m 120; % 质量 D diag([70 80 30]); % 阻尼系数 M diag([m m m]); nu x(4:6); % 速度向量 tau u(1:3); % 控制力 dxdt(1:3,1) R(x(3))*nu; % 位置导数 dxdt(4:6,1) M\(tau - D*nu - cross(nu, M*nu)); end function R R(psi) R [cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1]; end这个旋转矩阵R把船体坐标系的速度转换到惯性坐标系。注意cross(nu, M*nu)项处理了科里奥利力实测中发现当航速超过2m/s时忽略这项会导致模型发散。预测控制器设计NMPC的核心是滚动优化这个代价函数模块计算未来N步的误差function J costFunction(predictedStates, references) Q diag([10,10,5,1,1,0.5]); % 状态权重 R diag([0.1,0.1,0.05]); % 控制量权重 J 0; for k 1:predictionHorizon state_error predictedStates(:,k) - references(:,k); J J state_error*Q*state_error u_sequence(:,k)*R*u_sequence(:,k); end end调试时发现Q矩阵对航向角的权重过高会导致控制量剧烈震荡最终采用指数衰减权重前3步位置权重是后3步的3倍这样既保证初始快速响应又避免末端过冲。编队拓扑实现五船采用leader-follower结构这个通讯拓扑生成模块动态计算期望位姿function des_pose formation_pattern(leader_pose) % 菱形编队参数 offsets [0 0 0; % Leader 15 -8 pi/6; 15 8 -pi/6; -10 12 pi/4; -10 -12 -pi/4]; for i 2:5 des_pose(i,:) leader_pose ... [offsets(i,1)*cos(leader_pose(3)) - offsets(i,2)*sin(leader_pose(3)), ... offsets(i,1)*sin(leader_pose(3)) offsets(i,2)*cos(leader_pose(3)), ... offsets(i,3)]; end end实际运行中出现过跟随船画圈现象排查发现是坐标变换时漏掉了航向角偏移量的坐标系转换。修正后加入的offsets(i,3)项确保了期望航向角的相对计算。仿真结果显示在3级海况波浪干扰约0.5m下编队位置误差能稳定在1.2米以内。有意思的是当把预测时域从10步缩短到5步时虽然计算量降低40%但转弯时的跟踪误差增大了3倍——这说明足够的预测步长对海上运动的惯性补偿至关重要。文末彩蛋尝试在风浪干扰模块中加入周期性横摇扰动后发现控制器会自动生成蛇形航线来抵消侧向漂移这或许解释了现实中船舶在风浪中为何会走出之字路线。