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

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
例如：第一章 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
总结

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由下面的式子进行计算

总结

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

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

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

Python相关栏目本月热门文章