目录
AUV速度
受到影响因素:推力---->使用推力图进行查找得到速度
转弯速度损失---->可以使用速度转化率,代码中设置为0.85
// Procedure: propagateSpeed--->计算速度
void SimEngine::propagateSpeed(NodeRecordUVL& record, const ThrustMap& tmap,
double delta_time, double thrust,
double rudder, double max_accel,
double max_decel)
{
if(delta_time <= 0)
return;
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;// 转弯造成的速度损失 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);
}
根据 delta_time进行修改(在USM_Model::propagate()函数中-->USM_MOOSApp::Iterate()中)
计算步骤:SimEngine::propagateSpeed()函数中
AUV最大加速度:
max_acceleration—>在.moos配置文件中,读取值为0;
AUV最大减速度:
max_deceleration->在.moos配置文件中,读取值为0.5;
推力映射关系
TrustMap.cpp文件当中可以设置正向和负向推力最大值;
m_min_thrust = -100; m_max_thrust = 100;
在ThrustMap::addPair()函数中可以增加推力速度关系
在USM_Model::addThrustMapping()函数中使用了增加推力映射关系,
在USM_MOOSApp::OnStartUp()中生成了推力速度关系
计算航向
在SimEngine::propagateHeading()函数中设置在USM_Model::propagate()函数中-->USM_MOOSApp::Iterate()中
void SimEngine::propagateHeading(NodeRecordUVL& record,
double delta_time,
double rudder,
double thrust,
double turn_rate,
double torque_theta)
{
// 假设推进器和方向舵在同一个致动器上,例如,像皮划艇或典型的无人潜水器。旋转的推进器对转弯没有任何作用,除非道具在移动。
double speed = record.getSpeed();
if(speed == 0)
rudder = 0;
// 即使速度为零,也需要在扭矩为零的情况下继续打开。
rudder = vclip(rudder, -100, 100);// 判断推力是否在-100~100之间小于或大于回到100和-100
turn_rate = vclip(turn_rate, 0, 100);
// Step 1: 计算航向的原始增量变化
double delta_deg = rudder * (turn_rate/100) * delta_time;
// Step 2: 计算航向变化因子推力
delta_deg = (1 + ((thrust-50)/50)) * delta_deg;
// Step 3:考虑外力计算航向变化
delta_deg += (delta_time * torque_theta);
// Step 4: 计算[0359]范围内的最终新航向
double prev_heading = record.getHeading();
double new_heading = angle360(delta_deg + prev_heading);
record.setHeading(new_heading);
record.setYaw(-degToRadians(angle180(new_heading)));
}