系列文章目录
提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
例如：第一章 Python 机器学习入门之pandas的使用
提示：写完文章后，目录可以自动生成，如何生成可参考右边的帮助文档
文章目录
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/turn
rudder = 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 heading
double delta_deg = rudder * (turn_rate/100) * delta_time;
2.计算∆θi(THRUST)，会根据∆θi(RAW)和当前的推力THRUST进行调整，推力大于百分之五十时，航向角变化增加，小于百分之五十时变化减小
并且会根据推力的方向改变航向角的变化方向。
// Step 2: Calculate change in heading factoring thrust
delta_deg = (1 + ((thrust-50)/50)) * delta_deg;
3.计算∆θi(EXTERNAL)，航向角变化可能会受到外部转速ROTATE_SPEED的影响，
// Step 3: Calculate change in heading factoring external drift
delta_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 heading
double delta_deg = rudder * (turn_rate/100) * delta_time;
// Step 2: Calculate change in heading factoring thrust
delta_deg = (1 + ((thrust-50)/50)) * delta_deg;
// Step 3: Calculate change in heading factoring external drift
delta_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 heading
double delta_deg = rudder * (turn_rate/100) * delta_time;
// Step 2: Calculate change in heading factoring thrust
delta_deg = (1 + ((thrust-50)/50)) * delta_deg;
// Step 3: Calculate change in heading factoring external drift
delta_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: propagateDepth
void 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);
else
pct = 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 120124
double 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
总结
提示：这里对文章进行总结：
其他内容基本上类似，后面有时间再继续写
346
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
