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

ILOSpsi制导率的代码解析

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

文章目录

  • ILOSpsi制导率的代码解析
  • 前言
  • 一、代码全文
  • 二、内容解析
    • 1.persistent变量
    • 2.初始化
    • 3.读取下一个航路点
    • 4.计算路径的切向角
  • 总结

前言

提示：这里可以添加本文要记录的大概内容：

相较于传统的制导率，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
在这里插入图片描述

总结

提示：这里对文章进行总结：

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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/47383.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

opencv-27 阈值处理 cv2.threshold()

opencv-27 阈值处理 cv2.threshold()

怎么理解阈值处理? 阈值处理&#xff08;Thresholding&#xff09;是一种常用的图像处理技术&#xff0c;在机器学习和计算机视觉中经常被用于二值化图像或二分类任务。它基于设定一个阈值来将像素值进行分类&#xff0c;将像素值大于或小于阈值的部分分为两个不同的类别&…
阅读更多...
Redis持久化机制 RDB、AOF、混合持久化详解！如何选择？| JavaGuide

Redis持久化机制 RDB、AOF、混合持久化详解！如何选择？| JavaGuide

本文已经收录进 JavaGuide（「Java学习+面试指南」一份涵盖大部分 Java 程序员所需要掌握的核心知识。） Redis 持久化机制属于后端面试超高频的面试知识点，老生常谈了，需要重点花时间掌握。即使不是准备面试，日常开发也是需要经常用到的。 最近抽空对之前写的 Redis 持久化…
阅读更多...
【ES】---ES的聚合(aggregations)

【ES】---ES的聚合(aggregations)

目录 一、前言1、聚合分类2、聚合的实现方式二、RestAPI--bucket聚合案例11、按照类型分bucket2、按照(String)时间分bucket三、RestAPI-- metric聚合案例11、metric指标统计四、RestAPI-- pipeline聚合案例1一、前言 聚合是对文档数据的统计、分析、计算。 注意：参与聚合的字…
阅读更多...
Java中I/O流是什么？输入/输出流又是什么？

Java中I/O流是什么？输入/输出流又是什么？

在 Java中所有数据都是使用流读写的。流是一组有序的数据序列&#xff0c;将数据从一个地方带到另一个地方。根据数据流向的不同&#xff0c;可以分为输入&#xff08;Input&#xff09;流和输出&#xff08;Output&#xff09;流两种。 在学习输入和输出流之前&#xff0c;我们…
阅读更多...
PDF怎么转成Excel？4个方法非常实用！

PDF怎么转成Excel？4个方法非常实用！

如何使用记灵在线工具将PDF转成Excel&#xff1f;在日常工作中&#xff0c;我们经常需要转换PDF文件为Excel文件以方便我们处理数据。虽然PDF格式对于文本和图片的可视化效果效果不错&#xff0c;但是在处理数据时&#xff0c;Excel表格更加便捷。当我们将PDF文件转换成Excel文…
阅读更多...
JDBC的的使用

JDBC的的使用

首先导入jar包。 https://downloads.mysql.com/archives/c-j/ package com.test.sql;import java.sql.*;public class StudySql {public static void init() throws SQLException {Statement stmt null;Connection conn null;ResultSet res null;PreparedStatement pstm…
阅读更多...
LeetCode Top100 Liked 题单（序号1~17）

LeetCode Top100 Liked 题单（序号1~17）

01Two Sum - LeetCode 我自己写的代码【193ms】 因为不知道怎么加cmp函数&#xff0c;就只能pair的first设为值了&#xff0c;但其实这也是瞎做&#xff0c;应该也是O(n&#xff09;吧 class Solution { public:vector<int> twoSum(vector<int>& nums, int …
阅读更多...
【渗透测试】PNG图片隐藏部分恢复

【渗透测试】PNG图片隐藏部分恢复

1、图片原尺寸还原方法一 缺点就是有点慢&#xff0c;毕竟遍历的次数比较多 import binascii import struct import sysfilename sys.argv[1] crcbp open(filename, "rb").read() # 打开图片 crc32frombp int(crcbp[29:33].hex(), 16) # 读取图片中的CRC校验值 …
阅读更多...
HarmonyOS学习路之方舟开发框架—学习ArkTS语言（状态管理 一）

HarmonyOS学习路之方舟开发框架—学习ArkTS语言（状态管理 一）

状态管理概述 在前文的描述中&#xff0c;我们构建的页面多为静态界面。如果希望构建一个动态的、有交互的界面&#xff0c;就需要引入“状态”的概念。 图1 效果图 上面的示例中&#xff0c;用户与应用程序的交互触发了文本状态变更&#xff0c;状态变更引起了UI渲染&#x…
阅读更多...
力扣热门100题之最大子数组和【中等】【动态规划】

力扣热门100题之最大子数组和【中等】【动态规划】

题目描述 给你一个整数数组 nums &#xff0c;请你找出一个具有最大和的连续子数组&#xff08;子数组最少包含一个元素&#xff09;&#xff0c;返回其最大和。 子数组 是数组中的一个连续部分。 示例 1&#xff1a; 输入&#xff1a;nums [-2,1,-3,4,-1,2,1,-5,4] 输出&a…
阅读更多...
Linux推出Debian 12.1，并进行多方面系统修复

Linux推出Debian 12.1，并进行多方面系统修复

据了解&#xff0c;Debian是最古老的 GNU / Linux 发行版之一&#xff0c;也是许多其他基于 Linux 的操作系统的基础&#xff0c;包括 Ubuntu、Kali、MX 和树莓派 OS 等。 此外&#xff0c;该操作系统以稳定性为重&#xff0c;不追求花哨的新功能&#xff0c;因此新版本的发布…
阅读更多...
echarts制作多个纵轴的折线图

echarts制作多个纵轴的折线图

代码 <script type"text/javascript"> $(function (){var myChart echarts.init(document.getElementById(main));option {color: ["#9bbb59","#0B438B","#4141F1","#F81945","#4bacc6","#F89E19&q…
阅读更多...
PHP8知识详解：PHP的历史版本

PHP8知识详解：PHP的历史版本

PHP&#xff08;全称&#xff1a;Hypertext Preprocessor&#xff09;是一种广泛应用于web开发的脚本语言。它最初由Rasmus Lerdorf在1994年开发&#xff0c;并于1995年发布了第一个版本。以下是PHP的一些历史大版本及其介绍&#xff1a; PHP 1.0&#xff08;1995年&#xff09…
阅读更多...
【安全狗】linux免费服务器防护软件安全狗详细安装教程

【安全狗】linux免费服务器防护软件安全狗详细安装教程

在费用有限的基础上&#xff0c;复杂密码云服务器基础防护常见端口替换安全软件&#xff0c;可以防护绝大多数攻击 第一步&#xff1a;下载服务器安全狗Linux版&#xff08;下文以64位版本为例&#xff09; 官方提供了两个下载方式&#xff0c;本文采用的是 方式2 wget安装 方…
阅读更多...
如何在数据中台中提高效率并节省成本？

如何在数据中台中提高效率并节省成本？

上节讨论了如何保障数据中台的数据质量&#xff0c;让数据“准”。除了“快”和“准”&#xff0c;数据中台还离不开“省”。随数据规模越来越大&#xff0c;成本越来越高&#xff0c;如不合理控制成本&#xff0c;还没等你挖掘出数据应用价值&#xff0c;企业利润就被消耗完。…
阅读更多...
vant-ui，DatetimePicker时间选择器选择到秒

vant-ui，DatetimePicker时间选择器选择到秒

vant-ui的DatetimePicker 组件只能选择年月日时分&#xff0c;可能是组件维护者认为秒的选择用途不多&#xff0c;但是今天的需求中就是需要选择年月日时分秒所以就对DatetimePicker的组件封装成了可以选择年月日时分秒&#xff0c;直接上代码&#xff1a; 封装成组件&#xf…
阅读更多...
共用体类型

共用体类型

共用体&#xff08;union&#xff09;是一种成员共享存储空间的结构体类型。 union 共用体类型名 {成员列表 } 共用体内存长度是所有成员内存长度的最大值。 #include <iostream> using namespace std;int main() {//先声明共用体类型再定义共用体对象 union A {int m,…
阅读更多...
【stable diffusion】保姆级入门课程05-Stable diffusion（SD）图生图-涂鸦重绘的用法

【stable diffusion】保姆级入门课程05-Stable diffusion（SD）图生图-涂鸦重绘的用法

1.什么是涂鸦重绘 涂鸦重绘又称手涂蒙版。 简单来说&#xff0c;局部重绘手涂蒙版 就是涂鸦局部重绘的结合体&#xff0c;这个功能的出现是为了解决用户不想改变整张图片的情况下&#xff0c;对多个元素进行修改。 功能支持&#xff1a; 1.支持蒙版功能 2.笔刷决定绘制的元素…
阅读更多...
玩转回文：探索双下标法解谜，揭秘验证回文串的智慧攻略

玩转回文：探索双下标法解谜，揭秘验证回文串的智慧攻略

本篇博客会讲解力扣“125. 验证回文串”的解题思路&#xff0c;这是题目链接。 验证回文串&#xff0c;我们最容易想到的思路就是&#xff0c;使用两个下标left和right&#xff0c;分别表示字符串的第一个字符和最后一个字符。接着&#xff0c;让两个下标不断向中间移动&#x…
阅读更多...
怎样计算一个算法的复杂度？

怎样计算一个算法的复杂度？

分析一个算法主要看这个算法的执行需要多少机器资源。在各种机器资源中&#xff0c;时间和空间是两个最主要的方面。因此&#xff0c;在进行算法分析时&#xff0c;人们最关心的就是运行算法所要花费的时间和算法中使用的各种数据所占用的空间资源。算法所花费的时间通常称为时…
阅读更多...
最新文章

Copyright @ 2022~2023