动力定位船在有性能约束情况下的轨迹跟踪图代码

由于动力定位船的模型比较复杂，需要考虑船体运动学、动力学、液动力等因素，因此轨迹跟踪算法也比较复杂。下面我简单介绍一下动力定位船在有性能约束情况下的轨迹跟踪方法，并提供一份MATLAB代码，供参考。

1. 动力定位船模型

动力定位船可以看做一个多自由度的非线性动力学系统，其运动学和动力学模型可以表示为：

$$
\begin{aligned}
\dot{\textbf{x}} &= \textbf{f}(\textbf{x},\textbf{u}), \\
\textbf{y} &= \textbf{h}(\textbf{x}),
\end{aligned}
$$

其中，$\textbf{x}=[x,y,\psi,u,v,r]^T$表示船体在惯性坐标系下的位置和速度，$\psi$是船体朝向角，$\textbf{u}=[\delta,\tau]^T$表示舵角和推进力，$\textbf{y}$是船体的传感器测量值。$\textbf{f}(\cdot)$和$\textbf{h}(\cdot)$分别表示运动学和动力学方程。

2. 控制目标

控制目标是让船体沿着给定的轨迹运动，同时满足性能约束。假设给定的轨迹为：

$$
\begin{aligned}
x_d(t) &= x_d(s), \\
y_d(t) &= y_d(s),
\end{aligned}
$$

其中，$t$表示时间，$s$表示弧长。我们需要设计一个控制器，使得船体的位置和朝向角分别跟随轨迹的位置和曲率，即：

$$
\begin{aligned}
e_p &= \sqrt{(x-x_d)^2+(y-y_d)^2} \to 0, \\
e_\psi &= \psi_d - \psi \to 0,
\end{aligned}
$$

其中，$e_p$表示位置误差，$e_\psi$表示朝向角误差，$\psi_d$是轨迹朝向角，可以通过求导得到。

3. 控制器设计

控制器可以分为两部分：参考生成器和控制器。参考生成器负责计算给定轨迹的位置、朝向角和曲率，控制器负责计算舵角和推进力。

参考生成器可以使用样条插值法或者曲线拟合法，这里不再赘述。控制器的设计可以采用线性二次型控制器（LQR），其控制律为：

$$
\textbf{u} = -\textbf{K}\textbf{x} + \textbf{K}_r\textbf{x}_r,
$$

其中，$\textbf{K}$是状态反馈增益矩阵，$\textbf{K}_r$是参考状态反馈增益矩阵，$\textbf{x}_r$是跟踪轨迹的参考状态。为了满足性能约束，LQR控制器还需要加入积分项，得到增强控制律：

$$
\textbf{u} = -\textbf{K}\textbf{x} + \textbf{K}_r\textbf{x}_r - \textbf{K}_i\textbf{e},
$$

其中，$\textbf{e}=[e_p,e_\psi]^T$是误差向量，$\textbf{K}_i$是积分增益矩阵。

4. 性能约束

为了保证船体运行的安全性和稳定性，需要对舵角和推进力进行性能约束。舵角约束可以表示为：

$$
|\delta| \leq \delta_{max},
$$

其中，$\delta_{max}$是舵角最大值。推进力约束可以表示为：

$$
0 \leq \tau \leq \tau_{max},
$$

其中，$\tau_{max}$是推进力最大值。

5. 轨迹跟踪代码

下面是一份MATLAB代码，实现了动力定位船在有性能约束情况下的轨迹跟踪。代码中使用LQR控制器，并加入了积分项和性能约束。代码中的参数可以根据具体情况进行调整。

% 轨迹跟踪控制器
function [delta,tau] = trajectory_tracking_controller(x,x_d,K,K_r,K_i,delta_max,tau_max)

% 计算跟踪轨迹的参考状态
xd = x_d(1);
yd = x_d(2);
psid = atan2(y_d2-y_d1,x_d2-x_d1);
kappad = (y_d2-y_d1)*(x_ddd1-x_dd1)+(x_d1-x_d2)*(y_dd1-y_ddd1);
xr = [xd;yd;psid;0;0;kappad];

% 计算误差向量
ep = sqrt((x(1)-xd)^2 + (x(2)-yd)^2);
epsi = x(3) - psid;
e = [ep;epsi];

% 计算控制律
u = -K*x + K_r*xr - K_i*e;

% 添加性能约束
delta = max(min(u(1),delta_max),-delta_max);
tau = max(min(u(2),tau_max),0);

end

% 主程序
clear all; close all; clc;

% 定义参数
delta_max = deg2rad(30); % 舵角最大值
tau_max = 1000; % 推进力最大值

% 定义跟踪轨迹
x_d1 = 0; y_d1 = 0;
x_d2 = 100; y_d2 = 100;
x_d3 = 200; y_d3 = 0;
x_d4 = 300; y_d4 = 100;
x_d5 = 400; y_d5 = 0;
x_d = [x_d1 x_d2 x_d3 x_d4 x_d5; y_d1 y_d2 y_d3 y_d4 y_d5];
s_d = [0 cumsum(sqrt(diff(x_d(1,:)).^2+diff(x_d(2,:)).^2))]; % 轨迹弧长

% 定义初始状态
x0 = [0;0;0;0;0;0];

% 定义控制器参数
Q = diag([10 10 10 1 1 1]); % 状态反馈矩阵
R = diag([1 1]); % 控制反馈矩阵
[K,~,~] = lqr_dyn(x0,0,Q,R); % 计算状态反馈增益矩阵
[K_r,~,~] = lqr_dyn(x0,0,Q,0); % 计算参考状态反馈增益矩阵
K_i = eye(2); % 积分增益矩阵

% 运行轨迹跟踪
tspan = [0 1000]; % 时间区间
options = odeset('RelTol',1e-6,'AbsTol',1e-6); % 设置ODE求解器参数
[t,x] = ode45(@(t,x) dynamics(x,delta,tau),tspan,x0,options); % ODE求解器求解船体运动方程
x_d_interp = interp1(s_d',x_d',t); % 插值计算跟踪轨迹的位置和朝向角
delta = zeros(size(t)); tau = zeros(size(t)); % 初始化舵角和推进力
for i = 1:length(t)
    [delta(i),tau(i)] = trajectory_tracking_controller(x(i,:)',x_d_interp(i,:)',K,K_r,K_i,delta_max,tau_max); % 计算舵角和推进力
end

% 绘制轨迹
figure;
plot(x(:,1),x(:,2),'b','LineWidth',2); hold on;
plot(x_d(1,:),x_d(2,:),'k--','LineWidth',2); hold off;
axis equal; grid on;
xlabel('x'); ylabel('y');
legend('Actual Trajectory','Desired Trajectory');
title('Trajectory Tracking of DPV');

% 绘制舵角和推进力
figure;
subplot(2,1,1);
plot(t,delta,'b','LineWidth',2);
xlabel('Time (s)'); ylabel('Rudder Angle (rad)');
title('Rudder Angle vs. Time');
grid on;
subplot(2,1,2);
plot(t,tau,'r','LineWidth',2);
xlabel('Time (s)'); ylabel('Propulsion Force (N)');
title('Propulsion Force vs. Time');
grid on;

其中，`dynamics.m`文件实现了船体的运动方程，`lqr_dyn.m`文件实现了动态系统的LQR控制器设计。

% 船体运动方程
function xdot = dynamics(x,delta,tau)

% 定义参数
m = 1000; % 质量
Iz = 1000; % 惯性矩
xg = 0; % 重心位置
L = 10; % 船长
B = 3; % 船宽
D = 1; % 吃水深度
Xu = -50; Xuu = 0; Xvr = 0; Xrr = 0;
Yv = -100; Yr = 100; Yvv = 0; Yrr = 0;
Nv = 0; Nr = -100; Nvv = 0; Nrr = 0;

% 计算运动方程
u = x(4); v = x(5); r = x(6);
cospsi = cos(x(3)); sinpsi = sin(x(3));
xdot(1,1) = u*cospsi - v*sinpsi;
xdot(2,1) = u*sinpsi + v*cospsi;
xdot(3,1) = r;
xdot(4,1) = -Xu*u + Xuu*abs(u)*u + Xvr*v + tau/m*cos(delta);
xdot(5,1) = -Yv*v - Yr*r + Yvv*abs(v)*v + Yrr*abs(r)*r;
xdot(6,1) = -Nv*v - Nr*r + Nvv*abs(v)*v + Nrr*abs(r)*r + L*cos(delta)/Iz*tau;

end

% 动态系统的LQR控制器设计
function [K,P,E] = lqr_dyn(xbar,ubar,Q,R)

% 定义参数
m = 1000; % 质量
Iz = 1000; % 惯性矩
xg = 0; % 重心位置
L = 10; % 船长
B = 3; % 船宽
D = 1; % 吃水深度
Xu = -50; Xuu = 0; Xvr = 0; Xrr = 0;
Yv = -100; Yr = 100; Yvv = 0; Yrr = 0;
Nv = 0; Nr = -100; Nvv = 0; Nrr = 0;

% 计算A、B、E矩阵
A = [0 0 -m*xbar(5)*cos(xbar(3))/(m*xbar(4)-xbar(5)^2)  cos(xbar(3)) -sin(xbar(3)) 0;
     0 0  m*xbar(4)*sin(xbar(3))/(m*xbar(4)-xbar(5)^2)  sin(xbar(3))  cos(xbar(3)) 0;
     0 0  0                                            0            0           1;
     0 0  0                                            Xu/m         Xvr/m       0;
     0 0  0                                            Yv/m         Yr/m        0;
     0 0  L*cos(xbar(3))/Iz                             0            0           0];
B = [0 0;
     0 0;
     0 0;
     cos(xbar(3))/m 0;
     0              1/m;
     L*sin(xbar(3))/Iz 0];
E = eye(6);

% 计算LQR增益矩阵
[P,E,K] = care(A,B,Q,R,[],E);

end