基于LOS导引律的无人船路径点跟踪控制：MATLAB实现与解析

基于LOS导引律的无人船路径点跟踪控制 考虑横向误差以及船舶操纵性 MATLAB编程实现mmg模型 无人船模型为KVLCC2 注释很详细 适合新手学习且易扩展在无人船控制领域路径点跟踪是一项关键任务。本文将基于LOSLine - Of - Sight视线导引律结合考虑横向误差以及船舶操纵性利用MATLAB实现无人船的路径点跟踪控制采用mmgManeuvering Mathematical Group模型无人船模型则选定为KVLCC2 。这对于刚接触无人船控制的新手来说是个不错的学习案例而且代码也具有较好的易扩展性。一、LOS导引律基础LOS导引律的核心思想是让无人船朝着目标路径点的视线方向行驶。假设无人船当前位置为$(x,y)$目标路径点为$(xd,yd)$则视线方向与x轴的夹角$\theta_{LOS}$可以通过以下公式计算$\theta{LOS} \arctan2(yd - y, x_d - x)$基于LOS导引律的无人船路径点跟踪控制 考虑横向误差以及船舶操纵性 MATLAB编程实现mmg模型 无人船模型为KVLCC2 注释很详细 适合新手学习且易扩展在实际应用中我们会结合无人船的航向角$\psi$来计算控制量引导无人船驶向目标路径点。二、mmg模型与KVLCC2无人船模型mmg模型是一种常用的船舶操纵性数学模型它将船舶的受力分为船体力、螺旋桨力和舵力等部分通过建立动力学方程来描述船舶的运动。KVLCC2是一种特定的无人船模型它的参数是经过实验和研究确定的。在mmg模型框架下我们利用KVLCC2模型的参数来准确描述无人船的动力学特性。三、MATLAB代码实现% 基于LOS导引律的无人船路径点跟踪控制代码 % 作者[你的名字] % 日期[具体日期] % 定义参数 % KVLCC2无人船模型参数 m 1000; % 船舶质量 Ix 500; % 绕x轴转动惯量 L 20; % 船长 u0 1; % 初始前进速度 % 控制参数 k 0.5; % LOS导引律增益 dt 0.1; % 时间步长 % 路径点定义 waypoints [0 0; 10 10; 20 20]; % 一系列路径点 num_waypoints size(waypoints, 1); current_waypoint 1; % 初始状态 x 0; y 0; psi 0; u u0; v 0; r 0; % 预分配存储变量 x_history zeros(num_waypoints, 1); y_history zeros(num_waypoints, 1); psi_history zeros(num_waypoints, 1); for i 1:num_waypoints % 当前目标路径点 x_d waypoints(current_waypoint, 1); y_d waypoints(current_waypoint, 2); % 计算LOS导引律角度 theta_LOS atan2(y_d - y, x_d - x); % 计算控制输入这里简化为只考虑航向控制 delta_psi k * (theta_LOS - psi); % mmg模型动力学更新这里进行了简化 r_dot (1 / Ix) * (-0.5 * rho * L * u * v * L - 0.5 * rho * L^2 * r * u 0.5 * rho * L^2 * u * u * delta_psi); v_dot (1 / m) * (-0.5 * rho * L * u * r - 0.5 * rho * L * v * u); u_dot 0; % 假设前进速度不变 % 更新状态 r r r_dot * dt; v v v_dot * dt; u u u_dot * dt; psi psi r * dt; x x (u * cos(psi) - v * sin(psi)) * dt; y y (u * sin(psi) v * cos(psi)) * dt; % 存储历史数据 x_history(i) x; y_history(i) y; psi_history(i) psi; % 判断是否到达当前路径点 if sqrt((x - x_d)^2 (y - y_d)^2) 0.5 current_waypoint current_waypoint 1; if current_waypoint num_waypoints break; end end end % 绘图 figure; plot(waypoints(:, 1), waypoints(:, 2), ro--, DisplayName, 目标路径点); hold on; plot(x_history, y_history, b-, DisplayName, 无人船轨迹); xlabel(X 坐标 (m)); ylabel(Y 坐标 (m)); legend; title(基于LOS导引律的无人船路径点跟踪);代码分析参数定义部分- 首先定义了KVLCC2无人船模型相关的物理参数像船舶质量m、绕x轴转动惯量Ix、船长L以及初始前进速度u0。这些参数是mmg模型模拟无人船动力学特性的基础。- 接着设置了LOS导引律的增益k和时间步长dt。增益k决定了无人船对目标路径点的跟踪响应速度时间步长dt则影响模拟的精度和计算效率。- 定义了路径点数组waypoints这里简单设置了三个路径点。numwaypoints记录路径点数量currentwaypoint用于标记当前正在跟踪的路径点。初始状态部分- 设定无人船的初始位置x 0; y 0;初始航向角psi 0;以及初始横向速度v 0;和初始角速度r 0;。前进速度u设为u0。循环跟踪部分- 在循环中每次确定当前目标路径点(xd,yd)。- 计算LOS导引律角度thetaLOS通过atan2函数实现该函数可以根据坐标差值准确计算出视线方向与x轴的夹角。- 计算控制输入deltapsi这里简化为只考虑航向控制k倍的目标视线方向与当前航向角的差值作为控制输入k越大无人船转向目标路径点的速度越快但可能导致控制不稳定。- 基于mmg模型更新无人船的动力学状态。这里虽然对mmg模型进行了简化但仍通过简单的力和力矩平衡方程计算出角速度r、横向速度v和前进速度u的变化率并根据时间步长更新它们的值进而更新位置x和y以及航向角psi。- 判断无人船是否到达当前路径点通过计算当前位置与目标路径点的距离来判断如果距离小于设定值这里设为0.5则切换到下一个路径点。绘图部分- 使用MATLAB的绘图函数plot将目标路径点以红色圆圈和虚线表示无人船实际行驶轨迹以蓝色线条表示直观展示无人船的路径跟踪效果。添加坐标轴标签、标题和图例使图形更易理解。这样通过上述代码和分析新手可以初步了解基于LOS导引律的无人船路径点跟踪控制并且可以根据自己的需求对代码进行扩展例如进一步完善mmg模型、增加更多的控制约束等。