【MATLAB】ILOSpsi制导率的代码解析

ILOSpsi制导率的代码解析

这里记录一下关于fossen的MMS工具箱中，关于ILOSpsi制导率的代码解析内容，结合fossen的marine carft hydrodynamics and motion control这本书来参考看

---

---

前言

相较于传统的制导率，ILOS或积分LOS可用于消除由运动学建模误差引起的x中的稳态偏移。一个典型的例子是由于飞行器的滚动和俯仰运动而忽略了运动耦合项。

---

一、代码全文

function [psi_d, r_d, y_e] = ILOSpsi(x,y,Delta,kappa,h,R_switch,wpt,U,K_f)
% [psi_d, r_d, y_e] = ILOSpsi(x,y,Delta,kappa,h,R_switch,wpt,U) 
% ILOSpsi computes the desired heading angle psi_d, desired yaw rate r_d 
% and cross-track error y_e when the path is  straight lines going through 
% the waypoints (wpt.pos.x, wpt.pos.y). The desired heading angle computed 
% using the classical ILOS guidance law by Børhaug et al. (2008).
%
%    psi_d = pi_h - atan( Kp * (y_e + kappa * y_int) ),  Kp = 1/Delta
%
%    d/dt y_int = Delta * y_e / ( Delta^2 + (y_e + kappa * y_int)^2 )
%
% where pi_h is the path-tangential (azimuth) angle with respect to the 
% North axis and y_e is the cross-track error. The observer/filter for the 
% desired yaw angle psi_d is
%
%    d/dt psi_f = r_d + K_f * ssa( psi_d - psi_f ) )
%
% where the desired yaw rate r_d = d(psi_d)/dt is computed by
%
%    d/dt y_e = -U * y_e / sqrt( Delta^2 + y_e^2 )
%    r_d = -Kp * dy_e/dt / ( 1 + (Kp * y_e)^2 )   
%
% Initialization:
%   The active waypoint (xk, yk) where k = 1,2,...,n is a persistent
%   integer should be initialized to the first waypoint, k = 1, using 
%   >> clear ILOSpsi
%
% Inputs:   
%   (x,y): craft North-East positions (m)
%   Delta: positive look-ahead distance (m)
%   kappa: positive integral gain constant, Ki = kappa * Kp
%   h: sampling time (s)
%   R_switch: go to next waypoint when the along-track distance x_e 
%             is less than R_switch (m)
%   wpt.pos.x = [x1, x2,..,xn]' array of waypoints expressed in NED (m)
%   wpt.pos.y = [y1, y2,..,yn]' array of waypoints expressed in NED (m)
%   U: speed, vehicle cruise speed or time-varying measurement (m/s)
%   K_f: observer gain for desired yaw angle (typically 0.1-0-5)
%
% Feasibility citerion: 
%   The switching parameter R_switch > 0 must satisfy, R_switch < dist, 
%   where dist is the distance between the two waypoints at k and k+1:
%      dist = sqrt(  (wpt.pos.x(k+1)-wpt.pos.x(k))^2 
%                  + (wpt.pos.y(k+1)- wpt.pos.y(k))^2 );
%
% Outputs:  
%    psi_d: desired heading angle (rad)
%    r_d:   desired yaw rate (rad/s)
%    y_e:   cross-track error (m)
%
% For course control use the functions LOSchi.m and ILOSchi.m.
%
% Ref. E. Børhaug, A. Pavlov and K. Y. Pettersen (2008). Integral LOS 
% Control for Path Following of Underactuated Marine Surface Vessels in the 
% presence of Constant Ocean Currents. Proc. of the 47th IEEE Conference 
% on Decision and Control, pp. 4984–4991, Cancun, Mexico.
%  
% Author:    Thor I. Fossen
% Date:      10 June 2021
% Revisions: 26 June 2022 - use constant bearing when last wpt is reached,  
%                           bugfixes and improved documentation
%            17 Oct 2022  - added filter/observer for psi_d to avoid steps 

persistent k;      % active waypoint index (initialized by: clear ILOSpsi)持久变量标注了路点的序列
persistent xk yk;  % active waypoint (xk, yk) corresponding to integer k
persistent psi_f;  % filtered heading angle command
persistent y_int;  % integral state

%% Initialization of (xk, yk) and (xk_next, yk_next), and integral state 
if isempty(k)

    % check if R_switch is smaller than the minimum distance between the waypoints
    if R_switch > min( sqrt( diff(wpt.pos.x).^2 + diff(wpt.pos.y).^2 ) )
        error("The distances between the waypoints must be larger than R_switch");
    end

    % check input parameters
    if (R_switch < 0); error("R_switch must be larger than zero"); end
    if (Delta < 0); error("Delta must be larger than zero"); end

    y_int = 0;              % initial states
    psi_f = 0;
    k = 1;                  % set first waypoint as the active waypoint
    xk = wpt.pos.x(k);
    yk = wpt.pos.y(k);
    fprintf('Active waypoints:\n')
    fprintf('  (x%1.0f, y%1.0f) = (%.2f, %.2f) \n',k,k,xk,yk);

end

%% Read next waypoint (xk_next, yk_next) from wpt.pos 
n = length(wpt.pos.x);
if k < n                        % if there are more waypoints, read next one 
    xk_next = wpt.pos.x(k+1);  
    yk_next = wpt.pos.y(k+1);    
else                            % else, continue with last bearing
    bearing = atan2((wpt.pos.y(n)- wpt.pos.y(n-1)), (wpt.pos.x(n)-wpt.pos.x(n-1)));
    R = 1e10;
    xk_next = wpt.pos.x(n) + R * cos(bearing);
    yk_next = wpt.pos.y(n) + R * sin(bearing); 
end

%% Compute the path-tangnetial angle w.r.t. North
pi_h = atan2( (yk_next-yk), (xk_next-xk) ); 

% along-track and cross-track errors (x_e, y_e) 
x_e =  (x-xk) * cos(pi_h) + (y-yk) * sin(pi_h);
y_e = -(x-xk) * sin(pi_h) + (y-yk) * cos(pi_h);

% if the next waypoint satisfy the switching criterion, k = k + 1
d = sqrt( (xk_next-xk)^2 + (yk_next-yk)^2 );
if ( (d - x_e < R_switch) && (k < n) )
    k = k + 1;
    xk = xk_next;       % update active waypoint
    yk = yk_next; 
    fprintf('  (x%1.0f, y%1.0f) = (%.2f, %.2f) \n',k,k,xk,yk);
end

% ILOS guidance law
Kp = 1/Delta;
psi_d = psi_f;
psi_ref = pi_h - atan( Kp * (y_e + kappa * y_int) ); 

% desired yaw rate r_d
Dy_e = -U * y_e / sqrt( Delta^2 + y_e^2 );    % d/dt y_e
r_d = -Kp * Dy_e / ( 1 + (Kp * y_e)^2 );      % d/dt psi_d

% integral state: y_int[k+1]
y_int = y_int + h * Delta * y_e / ( Delta^2 + (y_e + kappa * y_int)^2 );

% observer/filter: psi_f[k+1]
psi_f = psi_f + h * ( r_d + K_f * ssa( psi_ref - psi_f ) );


end

二、内容解析

1.persistent变量

下面是持久变量的定义，持久变量会被保存在内存中，函数的持久变量不会因为函数结束调用而清零，而是会在代码运行过程中不断累计。

下面先定义了四个持久变量

persistent k;      % active waypoint index (initialized by: clear ILOSpsi) 标注了路点的序列
persistent xk yk;  % active waypoint (xk, yk) corresponding to integer k 标注了第k个路点的序列
persistent psi_f;  % filtered heading angle command 滤波后的航向角命令
persistent y_int;  % integral state 积分状态

2.初始化

代码如下（初始化部分）：

%% Initialization of (xk, yk) and (xk_next, yk_next), and integral state 
if isempty(k) % 确定数组k是否为空，若为空则进入初始化步骤

    % check if R_switch is smaller than the minimum distance between the waypoints
    % 检查一下R是否小于最小的路点间的距离
    if R_switch > min( sqrt( diff(wpt.pos.x).^2 + diff(wpt.pos.y).^2 ) )
        error("The distances between the waypoints must be larger than R_switch");% 小于航路点间的距离，说明R选取的不合适，报错
    end

    % check input parameters
    if (R_switch < 0); error("R_switch must be larger than zero"); end
    % R必须选择大于0的圆
    if (Delta < 0); error("Delta must be larger than zero"); end
	% 参数delta也必须大于0
	% 初始化状态
    y_int = 0;              % initial states
    psi_f = 0;
    k = 1;                  % set first waypoint as the active waypoint 设定第一个航路点为活跃点
    xk = wpt.pos.x(k);
    yk = wpt.pos.y(k);
    fprintf('Active waypoints:\n')
    fprintf('  (x%1.0f, y%1.0f) = (%.2f, %.2f) \n',k,k,xk,yk);

end

检查R和航路点间的距离

对于Enclosure-based LOS 的制导率来说，通过包围载体的圆的半径来确定载体和载体路径之间的距离关系。R一定要选择比行路点序列中两点间距离更大的点。

若选择的R足够大，那么圆就会和直线相交于两个端点上，那么该制导率的策略就是ye指向焦点pn

3.读取下一个航路点

%% Read next waypoint (xk_next, yk_next) from wpt.pos 
n = length(wpt.pos.x);% 返回航路点的个数
if k < n                        % if there are more waypoints, read next one 若是k小于航路点的总个数，那么继续读取下一个航路点
    xk_next = wpt.pos.x(k+1);  
    yk_next = wpt.pos.y(k+1);    
else                            % else, continue with last bearing 若是达到了航路点的尽头，那么沿着最后一个航路点的方位继续前进
    bearing = atan2((wpt.pos.y(n)- wpt.pos.y(n-1)), (wpt.pos.x(n)-wpt.pos.x(n-1)));
    R = 1e10;
    xk_next = wpt.pos.x(n) + R * cos(bearing);
    yk_next = wpt.pos.y(n) + R * sin(bearing); 
end

4.计算路径的切向角

%% Compute the path-tangnetial angle w.r.t. North
pi_h = atan2( (yk_next-yk), (xk_next-xk) ); % 求解旋转角pi_p

% along-track and cross-track errors (x_e, y_e) 求解路径坐标系下的x轴误差和y轴误差
x_e =  (x-xk) * cos(pi_h) + (y-yk) * sin(pi_h);
y_e = -(x-xk) * sin(pi_h) + (y-yk) * cos(pi_h);

% if the next waypoint satisfy the switching criterion, k = k + 1
d = sqrt( (xk_next-xk)^2 + (yk_next-yk)^2 );% 求解一下当前位置和下一点位置间的距离
if ( (d - x_e < R_switch) && (k < n) )% 若是距离delta小于圆的半径，以及k还在序列范围内
    k = k + 1;% k进一步更新
    xk = xk_next;       % update active waypoint 更新当前的目标点
    yk = yk_next; 
    fprintf('  (x%1.0f, y%1.0f) = (%.2f, %.2f) \n',k,k,xk,yk);
end

% ILOS guidance law
% 这里是引导率的核心算法处
Kp = 1/Delta;
psi_d = psi_f;
psi_ref = pi_h - atan( Kp * (y_e + kappa * y_int) ); % 制导率的公式

% desired yaw rate r_d
Dy_e = -U * y_e / sqrt( Delta^2 + y_e^2 );    % d/dt y_e
r_d = -Kp * Dy_e / ( 1 + (Kp * y_e)^2 );      % d/dt psi_d

% integral state: y_int[k+1]
y_int = y_int + h * Delta * y_e / ( Delta^2 + (y_e + kappa * y_int)^2 );

% observer/filter: psi_f[k+1]
psi_f = psi_f + h * ( r_d + K_f * ssa( psi_ref - psi_f ) );

（1）计算xe ye
根据fossen书中所讲，先求解一下 path-tangential coordinate 坐标系相对应的xe和ye误差。
先求解以下pi_p，再利用二维的坐标旋转矩阵来求解：

代码中，先对pi_p进行了求解，之后求解了xe和ye

评判是否到达下一个航路点的公式：

上述代码显示，当r比路径减去xe要大的时候，就该切换到下一个路径点了。

（2）制导率公式

进一步设计，为下面的非线性ILOS公式，如代码所示：

（3）ye导数
对ye求导，可得ye导数的公式：

对期望航向角求导，就可以得到期望航向角速度的大小

（4）ye_int

% integral state: y_int[k+1]
y_int = y_int + h * Delta * y_e / ( Delta^2 + (y_e + kappa * y_int)^2 );

具体计算ye int的公式如下图所示：

其中，KP KI kappa都属于被设计的参数。

总结

例如：以上就是今天要讲的内容，本文仅仅简单介绍了pandas的使用，而pandas提供了大量能使我们快速便捷地处理数据的函数和方法。