系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
例如：第一章 Python 机器学习入门之pandas的使用

提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

系列文章目录
uSimMarine简介
一、Propagating the Vehicle Speed, Heading, Position and Depth
- 1.Propagating the Vehicle Speed
- 2.Propagating the Vehicle Heading
- 3.Propagating the Vehicle Position
- 4.Propagating the Vehicle Depth
- 4.Propagating the Vehicle Altitude
总结

uSimMarine简介

uSimMarine应用程序是一个简单的3D车辆模拟器，可以根据当前执行器值和车辆先前状态更新车辆状态、位置和轨迹。典型的使用场景有一个与每个模拟车辆相关联的uSimMarine实例，如图所示。

订阅变量：

发布变量：

其具体的配置文件如下，各参数意义可以查询文档获得

一、Propagating the Vehicle Speed, Heading, Position and Depth

主要对于这些变量的计算依据三个值：
1.前一刻车辆的状态
2.自从上次更新后经过的时间
3.当前执行机构的值：升降舵角、偏航舵角、推进器力矩
4.模型中的参数
.h文件中包含了很多计算的内容

1.Propagating the Vehicle Speed

车辆的速度主要基于当前的推力值进行计算DESIRED_THRUST，速度也会收到偏转舵DESIRED_RUDDER的影响。还有上一时刻的速度NAV_SPEED，以及最大加速度MAX_ACCELERATION和最大减速度MAX_DECLEARATION的影响。
具体计算步骤如下：
1.计算vi（raw），基于推力的速度值，其中推力值与速度的转化由thrust_map来进行转换，具体的转换表可以在usim的MOOS配置中进行设置

 double next_speed  = tmap.getSpeedValue(thrust);

2.计算vi（turn），基于vi（raw）和DESIRED_RUDDER计算的值，会比vi（raw）略低，若AUV进行偏转则进行计算，若无偏转则无变化

  rudder = vclip(rudder, -100, 100);double rudder_magnitude = fabs(rudder);double vpct = (rudder_magnitude / 100) * 0.85;next_speed *= (1.0 - vpct);

3.计算vi(FINAL)，基于vi(TURN)以及之前一刻的速度，收到最大加减速的限制。将计算的速度与上一时刻速度进行比较，用时间间隔算出加速度，若跟设定的最大最小速度有偏差，则进行修正。

if(next_speed > prev_speed) {double acceleration = (next_speed - prev_speed) / delta_time;if((max_accel > 0) && (acceleration > max_accel))next_speed = (max_accel * delta_time) + prev_speed;}if(next_speed < prev_speed) {double deceleration = (prev_speed - next_speed) / delta_time;if((max_decel > 0) && (deceleration > max_decel))next_speed = (max_decel * delta_time * -1) + prev_speed;}

4.设置该速度，并且用于后面关于位置、深度、航向角的计算

  record.setSpeed(next_speed);

此段程序的源码如下：

void SimEngine::propagateSpeed(NodeRecord& record, const ThrustMap& tmap,double delta_time, double thrust,double rudder, double max_accel, double max_decel)
{if(delta_time <= 0)return;if(m_thrust_mode_reverse) {thrust = -thrust;rudder = -rudder;}double next_speed  = tmap.getSpeedValue(thrust);double prev_speed  = record.getSpeed();// Apply a slowing penalty proportional to the rudder/turnrudder = vclip(rudder, -100, 100);double rudder_magnitude = fabs(rudder);double vpct = (rudder_magnitude / 100) * 0.85;next_speed *= (1.0 - vpct);if(next_speed > prev_speed) {double acceleration = (next_speed - prev_speed) / delta_time;if((max_accel > 0) && (acceleration > max_accel))next_speed = (max_accel * delta_time) + prev_speed;}if(next_speed < prev_speed) {double deceleration = (prev_speed - next_speed) / delta_time;if((max_decel > 0) && (deceleration > max_decel))next_speed = (max_decel * delta_time * -1) + prev_speed;}record.setSpeed(next_speed);
}

2.Propagating the Vehicle Heading

和速度类似，航向角也收到DESIRED_RUDDER和DESIRED_THRUST的影响
计算算法如下：
1.计算∆θi(RAW)，受到当前的rudder值影响，TURN_RATE转弯速率可以在配置文件中进行设置。

  // Step 1: Calculate raw delta change in headingdouble delta_deg = rudder * (turn_rate/100) * delta_time;

2.计算∆θi(THRUST)，会根据∆θi(RAW)和当前的推力THRUST进行调整，推力大于百分之五十时，航向角变化增加，小于百分之五十时变化减小

并且会根据推力的方向改变航向角的变化方向。

  // Step 2: Calculate change in heading factoring thrustdelta_deg = (1 + ((thrust-50)/50)) * delta_deg;

3.计算∆θi(EXTERNAL)，航向角变化可能会受到外部转速ROTATE_SPEED的影响，

  // Step 3: Calculate change in heading factoring external driftdelta_deg += (delta_time * rotate_speed);

4.最后需要对新的航向角进行计算，使其范围在【0,360】

  // Step 4: Calculate final new heading in the range [0,359]double prev_heading = record.getHeading();double new_heading  = angle360(delta_deg + prev_heading);record.setHeading(new_heading);record.setYaw(-degToRadians(angle180(new_heading)));

源码如下：

void SimEngine::propagateHeading(NodeRecord& record,double delta_time, double rudder,double thrust,double turn_rate,double rotate_speed)
{// Assumption is that the thruster and rudder are on the same// actuator, e.g., like the kayaks, or typical UUVs. A rotated// thruster contributes nothing to a turn unless the prop is // moving.double speed = record.getSpeed();if(speed == 0) rudder = 0;if(m_thrust_mode_reverse) {thrust = -thrust;rudder = -rudder;}// Even if speed is zero, need to continue on in case the // torque is non-zero.rudder    = vclip(rudder, -100, 100);turn_rate = vclip(turn_rate, 0, 100);// Step 1: Calculate raw delta change in headingdouble delta_deg = rudder * (turn_rate/100) * delta_time;// Step 2: Calculate change in heading factoring thrustdelta_deg = (1 + ((thrust-50)/50)) * delta_deg;// Step 3: Calculate change in heading factoring external driftdelta_deg += (delta_time * rotate_speed);// Step 4: Calculate final new heading in the range [0,359]double prev_heading = record.getHeading();double new_heading  = angle360(delta_deg + prev_heading);record.setHeading(new_heading);record.setYaw(-degToRadians(angle180(new_heading)));
}

3.Propagating the Vehicle Position

车辆位置的传播主要基于新计算的车辆方向和速度、以前的车辆位置，以及自更新以前的车辆位置以来所经过的时间来进行计算的
步骤如下：
1.计算用于更新新车辆位置的车辆方向 θ¯,和速度v，并将方向转换为弧度

其中s和c意义如下，该式子可以解决角度换算的问题，1和360的平均值为0而不是180

2.根据航向、速度和运行时间计算新位置xi和yi，考虑到在海上航行中，0度为正北，90度为正东的习惯差异，计算是在此基础上得出的也就是说，从海洋到传统三角的映射如下:0◦→90◦，90◦→0,180◦→270◦，270◦→180◦

3.最后一步是考虑到可能存在的任何外部漂移，从上面调整x和y位置。

源码如下：

void SimEngine::propagateHeading(NodeRecord& record,double delta_time, double rudder,double thrust,double turn_rate,double rotate_speed)
{// Assumption is that the thruster and rudder are on the same// actuator, e.g., like the kayaks, or typical UUVs. A rotated// thruster contributes nothing to a turn unless the prop is // moving.double speed = record.getSpeed();if(speed == 0) rudder = 0;if(m_thrust_mode_reverse) {thrust = -thrust;rudder = -rudder;}// Even if speed is zero, need to continue on in case the // torque is non-zero.rudder    = vclip(rudder, -100, 100);turn_rate = vclip(turn_rate, 0, 100);// Step 1: Calculate raw delta change in headingdouble delta_deg = rudder * (turn_rate/100) * delta_time;// Step 2: Calculate change in heading factoring thrustdelta_deg = (1 + ((thrust-50)/50)) * delta_deg;// Step 3: Calculate change in heading factoring external driftdelta_deg += (delta_time * rotate_speed);// Step 4: Calculate final new heading in the range [0,359]double prev_heading = record.getHeading();double new_heading  = angle360(delta_deg + prev_heading);record.setHeading(new_heading);record.setYaw(-degToRadians(angle180(new_heading)));
}

4.Propagating the Vehicle Depth

根据一些输入参数，模拟了uSimMarine的深度变化。从一个迭代到下一个迭代的主要参数变化是ELEVATOR升降舵角的值，从MOOS变量DESIRED_ELEVATOR。在任何给定的迭代中，新的车辆深度zi由下面的式子进行计算

每次新的深度由过去的深度加上深度变化率乘以时间间隔来进行计算。
而深度变化率则由AUV的速度、升降舵角的值以及三个关于物理模拟的变量决定，这三个变量为：1.buoyancy_rate以米每秒作为单位，当该值大于0时说明AUV正处于正浮力
2.max_depth_rate最大深度变化率
3.max_depth_rate_speed最大深度速度变化率
上面两个变量将会影响深度变化率，其具体影响如下图所示：

AUV会在高速行驶的情况下有更快的深度变化率，直到到达最大速度上限后，速度不再影响深度变化率。真实情况下的深度变化率取决于升降舵角和AUV的速度。深度变化率公式如下：

据此写成的深度仿真代码如下所示：

//--------------------------------------------------------------------
// Procedure: propagateDepthvoid SimEngine::propagateDepth(NodeRecord& record,double delta_time,double elevator_angle, double buoyancy_rate,double max_depth_rate, double max_depth_rate_speed)
{double speed = record.getSpeed();double prev_depth = record.getDepth();elevator_angle = vclip(elevator_angle, -100, 100);if(speed <= 0) {double new_depth = prev_depth + (-1 * buoyancy_rate * delta_time);record.setDepth(new_depth);record.setPitch(0.0);}else {double pct = 1.0;if(max_depth_rate_speed > 0) {pct = (speed / max_depth_rate_speed);if(pct > 1.0)pct = 1.0;}if(pct < 0)pct = -1 * sqrt(-1 * pct);elsepct = sqrt(pct);double depth_rate = pct * max_depth_rate;double speed = record.getSpeed();double pitch_depth_rate = - sin(record.getPitch())*speed;double actuator_depth_rate = (elevator_angle/100) * depth_rate;double total_depth_rate = (-buoyancy_rate) +  pitch_depth_rate + actuator_depth_rate;double new_depth = prev_depth + (1 * total_depth_rate * delta_time);record.setDepth(new_depth);// Pitch added by HS 120124double pitch =0 ;if (speed > 0 && (fabs(pitch_depth_rate+actuator_depth_rate) <= speed))pitch = - asin((pitch_depth_rate+actuator_depth_rate)/speed);record.setPitch(pitch);}if(record.getDepth() < 0)record.setDepth(0);
}

4.Propagating the Vehicle Altitude

默认的水深为100米，水深完全由当前AUV的位置所决定

default water depth = 100

总结

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

其他内容基本上类似，后面有时间再继续写

MOOS程序解析记录（6）uSimMarine解析1相关推荐

DNS服务器（DNS服务器构建，特殊的解析记录，多域名DNS服务器架构，DNF主从架构，DNS主从数据同步）
学前提示: 需要两台虚拟机虚拟机A:主机名位svr7 IP:192.168.4.7 虚拟机B:主机名pc207 IP:192.168.207 一.进行环境的设置 1.修改两台虚拟机的SELinux ...
域名申请·多域名SSL证书申请·SSL证书认证流程·CAA解析记录添加
环境:VPS:CentOS8 1.申请域名通过阿里云申请一个域名就好,博主申请了个人域名.top,第一年6元 2.添加一条IPV4的DNS解析记录阿里云-控制台-域名解析-解析设置-添加新纪录这 ...
MOOS程序解析记录（6）uSimMarine解析2
MOOS程序解析记录(6)uSimMarine解析2 文章目录 MOOS程序解析记录(6)uSimMarine解析2 前言一.Simulation of External Drift 1.Exter ...
MOOS程序解析记录（6）pLogger
MOOS程序解析记录(6)pLogger 最近在使用数据记录的时候,发现了自己对于PLogger并不是很熟悉,很多语法规则并不是很懂,于是便升起了记录一下该模块的心思,虽然这个模块并不能算多重要,但是 ...
MOOS程序解析记录（7）pMarinePID解析
MOOS程序解析记录(7)pMarinePID解析花了一天多的时间,对pMarinePID的源码进行了阅读,这里做一个分析记录,为后面改进控制算法做好基础. 前言 pMarinePID应用程序实现了 ...
MOOS程序解析记录（1）
系列文章目录我将自己学习MOOS程序中的一些记录记载到这里文章目录系列文章目录前言一.C enum(枚举) 二.std::vector 1.vector 2.容器特性 3.基本函数实现三. ...
GPS使用记录NMEA数据解析、RTK定位以及ntrip协议
目录 1. 概述 2. 泰斗GPS定位模组 3. RTK GPS定位 4. ntrip协议 5. 千寻 1. 概述本文主要是记录下GPS相关的内容.最开始叫做<泰斗GPS使用记录>,那时 ...
动态更新阿里云DDNS解析记录的IPv6地址，随时随地用域名远程访问自己的电脑【如何远程访问家里的电脑】
远程访问电脑日志简介要求 1. 获取两台电脑 2.IPv6网络 2.1检查光猫 2.2检查路由器 2.3配置电脑防火墙 2.3.1添加ICMPv6协议 2.3.2配置SMB协议 2.4配置远程桌 ...
【Python文本处理】基于运动路线记录GPX文件解析，心率、速度、时间、功率、踏频、海拔等参数的生成和更改，以及GPX循环拼接
[Python文本处理]基于运动路线记录GPX文件解析,心率.速度.时间.功率.踏频.海拔等参数的生成和更改,以及GPX循环拼接 GPX文件本身其实就是坐标.海拔.时间.心率等综合性的xml文件如图 ...
冯诺依曼计算机程序及其执行,第4章冯.诺依曼计算机：机器级程序与其执行练习题答案解析...
第4章冯.诺依曼计算机:机器级程序与其执行练习题答案解析 (20页) 本资源提供全文预览,点击全文预览即可全文预览,如果喜欢文档就下载吧,查找使用更方便哦! 14.9 积分第 4 章冯.诺依曼计算 ...

MOOS程序解析记录（6）uSimMarine解析1