（IEEE trans复现 1）自主水下航行器(AUV)，动态路径规划，非线性模型预测控制(MPC)，后退时域优化(RHO)，轨迹跟踪

内容包括：自主水下航行器(AUV)，动态路径规划，非线性模型预测控制(MPC)，后退时域优化(RHO)，轨迹跟踪

中文名称：水下机器人综合路径规划与跟踪控制:一种一致的模型预测控制优化方法

英文名称：Integrated Path Planning and Tracking Control  of an AUV: A Unified Receding Horizon Optimization Approach

一、代码及文献获取

1. 代码链接：哔哩哔哩工房 (bilibili.com)

2. 视频链接：IEEE trans：水下机器人AUV的路径规划和基于模型预测控制MPC的跟踪框架_哔哩哔哩_bilibili

本代码包括水下机器人AUV路径规划和MPC路径跟踪两个模块，两个模块均采用优化求解器求解，考虑了AUV的水动力学模型，结果是2D空间内的平面路径。

二、主要内容

摘要:  针对自主水下航行器(AUV)的综合路径规划和跟踪控制问题，提出了一种统一的后退地平线优化(RHO)方案。考虑到机载传感器的有效传感范围较短，我们将路径规划问题转化为基于样条路径模板的RHO问题。规划的路径随后被视为与AUV具有相同运动学和动力学特性的虚拟参考系统的状态轨迹。适当构造误差动力学，使水下航行器跟踪控制等同于误差动力学系统的调节问题，便于利用非线性MPC技术推导理论结果。为了保证系统的闭环稳定性，设计了模型预测控制(MPC)跟踪控制器。由于其固有的RHO特性，将路径规划和跟踪控制整合到一个统一的方案中。仿真研究使用了Falcon AUV的真实动态模型，该模型是在我们之前的实验工作中创建的。仿真结果验证了所提控制算法的有效性。

三、主要代码

%% ========================================================================
% ========================= Main function =================================
% =========================================================================
for i = 1:1:Tstep2
    % Planning
    if mod(i+N-1,M) == 0
        everdone = iter_round*1;
        if everdone == 30
            ord = 5;
            mlti = 5;
        end
        bbrks = everdone+0:1:everdone+Nbrk;
        X = (everdone+0:1:everdone+Nbrk)';
        
        for l = 1:1:Nbrk+1
            Y(l) = data1_together(1,100*(everdone+l-1)+1);
        end
        
        knots = augknt(bbrks,mlti);
        nknt = length(knots);
        nco = nknt-ord;
        
        x = plannar.calc_path2(x0,bbrks,X,Y,y0,y0d,y02d,eps,T);
        x = x';
        sp = spmak(knots,x);
        
        y0 = fnval(sp,everdone+1);
        y0d = fnval(fnder(sp,1),everdone+1);
        y02d = fnval(fnder(sp,2),everdone+1);
        SpAry(everdone+1,1) = {sp};
        iter_round = iter_round+1;
    end
    
    % Generate the path reference
    [eta_Ref_all(:,i),P] = plannar.reference(t,Spline_map2,SpAry,i,M,T_plt,y_scale,T);
    ord = 4;
    mlti = 4;
    
    % Calculate the control input for the AUV
    tic
    u = mpc1.calc_control(u0,X0,P,T);
    toc
    u_actual = u(1:nu,1);
    Uall2(:,i) = u_actual;
    
    % Update the system state
    auv.advance( Uall2(:,i), disturbance, T);
    Xplus = auv.X;
    Xall2(:,i+1) = Xplus;
    X0 = Xplus;