MOOS程序解析记录（7）pMarinePID解析

MOOS程序解析记录（7）pMarinePID解析

---

花了一天多的时间，对pMarinePID的源码进行了阅读，这里做一个分析记录，为后面改进控制算法做好基础。

前言

pMarinePID应用程序实现了一个简单的PID控制器，根据舵机的输入产生适合执行器控制的值。在仿真中，输出量被仿真模型所使用而不是真实的AUV。
pMarinePID通常从pHelmIvP这里获取数据，读取一些的航向角和速度，并输出执行器的期望舵角和期望推力值。

一、pMarinePID概况

这是pMarinePID的文件框架，其中文件内容如下所示：

主要运行的代码在MarinePID程序中，而PIDEngine为在MarinePID程序中调用的类。而ScalarPID为PIDEngine中调用的类(套娃了属于是)

1.ScalarPID

该部分定义了核心的PID控制器的相关结构，设置了三个PID参数，增益，期望，限幅。给出一点下面用到的c++知识，因为下面重载了一下=运算符。核心代码会在之后
友元函数
运算符重载

头文件代码如下：

#ifndef MOD_SCALARPID_HEADER
#define MOD_SCALARPID_HEADER

#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000

#include <string>
#include <list>
#include <fstream>

class ScalarPID  
{
public:
  ScalarPID();
  ScalarPID(double dfKp, double dfKd,//定义了构造函数，三种重载形式
	     double dfKi, double dfIntegralLimit,
	     double dfOutputLimit);
  ScalarPID(const ScalarPID&);    // **new **
  virtual ~ScalarPID();

  const ScalarPID &operator=(const ScalarPID&);  // **new**//定义了一下=运算符

  void SetGains(double dfKp,double dfKd,double dfKi);
  void SetLimits(double dfIntegralLimit, double dfOutputLimit);

  void SetGoal(double dfGoal);
  void SetLogPath(std::string & sPath);
  void SetLog(bool bLog);
  void SetName(std::string sName);
  bool Run(double dfeIn, double dfErrorTime, double& dfOut);//核心代码

  void setDebug(bool v=true) {m_debug=v;}
  bool getDebug() const      {return(m_debug);}
  bool getMaxSat() const     {return(m_max_sat);}
  
  std::string getDebugStr() const {return(m_debug_str);}
  
protected:  // Core parameters
  double m_dfKi;
  double m_dfKd;
  double m_dfKp;
  double m_dfIntegralLimit;
  double m_dfOutputLimit;

protected: // Data persistent between runs
  double m_dfeOld;
  double m_dfOldTime;
  double m_dfOut;
  unsigned int m_nHistorySize;
  std::list<double> m_DiffHistory;
  
  double m_dfe;
  double m_dfeSum;
  double m_dfeDiff;
  double m_dfDT;

  
protected:
  bool Log();
  
  //note this is just for logging purposes...
  double  m_dfGoal;
  int     m_nIterations;

  bool          m_bLog;
  std::string   m_sName;
  std::string   m_sLogPath;
  std::ofstream m_LogFile;

  // Added April 2019, mikerb, for optional additional debugging
  std::string   m_debug_str;
  bool          m_debug;

  // Added April 2019, mikerb, for optional additional debugging
  bool          m_max_sat;  
};

#endif

核心代码：
PID算法

//-------------------------------------------------------------------
bool ScalarPID::Run(double dfeIn, double dfErrorTime, double &dfOut)
{
  // Reset max_sat flag on every interation
  m_max_sat = false;
  
  if(m_debug) {
    m_debug_str =  "dfeIn=" + doubleToString(dfeIn);
    m_debug_str += ", dfErrorTime=" + doubleToString(dfErrorTime);
  }
    
  m_dfe  = dfeIn;//输入赋值
  
  //figure out time increment...
  if(m_nIterations++!=0) {
        
    m_dfDT = dfErrorTime-m_dfOldTime;//计算时间差，下面都是时间差出现bug的情况报错

    if(m_debug)
      m_debug_str += ", m_dfDT=" + doubleToString(m_dfDT);
      
    
    if(m_dfDT<0) {
      MOOSTrace("ScalarPID::Run() : negative or zero sample time\n");
      return(false);
    }
    else if(m_dfDT ==0) {
      //nothing to do...
      dfOut = m_dfOut;
      Log();
      return(true);
    }
    
    //figure out differntial//计算微分
    double dfDiffNow = (dfeIn-m_dfeOld)/m_dfDT;//求取当前时刻的微分
    m_DiffHistory.push_front(dfDiffNow);//列表内增加当前时刻的微分
    while(m_DiffHistory.size() >= m_nHistorySize) {//按照初始化是list数量为10
      m_DiffHistory.pop_back();//删除过时的微分
    }
    if(m_debug) {
      m_debug_str += ", dfDiffNow=" + doubleToString(dfDiffNow);
      m_debug_str += ", DiffHistSize=" + uintToString(m_DiffHistory.size());
    }
      
    m_dfeDiff = 0;
    list<double>::iterator p;
    for(p = m_DiffHistory.begin();p!=m_DiffHistory.end();p++) {
      m_dfeDiff   += *p;   //10个最近时间的微分求和
    }
    if(m_debug) 
      m_debug_str += ", mdfeDiff(1)=" + doubleToString(m_dfeDiff);

    m_dfeDiff/=m_DiffHistory.size();//10个最近时间的微分求和再取平均
    if(m_debug) 
      m_debug_str += ", mdfeDiff(2)=" + doubleToString(m_dfeDiff);
  }
  else {
    //this is our first time through
    m_dfeDiff = 0;
  }
  
  
  if(m_dfKi>0) {//如果用到积分的话
    //calculate integral term  
    m_dfeSum    +=  m_dfKi*m_dfe*m_dfDT;//求取该段时间内的积分值

    if(m_debug) {
      m_debug_str += ", m_dfKi" + doubleToString(m_dfKi);
      m_debug_str += ", m_dfeSum(1)" + doubleToString(m_dfeSum);
      m_debug_str += ", m_dfIntegralLimit" + doubleToString(m_dfIntegralLimit);
    }
    
    //prevent integral wind up...//防止积分饱和
    if(fabs(m_dfeSum)>=fabs(m_dfIntegralLimit)) {//求得积分超过限幅
      int nSign = (int)(fabs(m_dfeSum)/m_dfeSum);//求积分的正负号
      m_dfeSum = nSign*fabs(m_dfIntegralLimit);//按照限幅的值输出
      if(m_debug) 
	m_debug_str += ", m_dfeSum(2)" + doubleToString(m_dfeSum);
    }
  }
  else {
    m_dfeSum = 0;
  }

  // m_dfOut = (m_dfKp*m_dfe) + (m_dfKd*m_dfeDiff) + m_dfeSum; 
  //        m_dfKp =
  //        m_dfe  =
  //     (m_dfKp * m_dfe) =
  //        m_dfKd = 
  //     m_dfeDiff = 0.10223 
  //     (m_dfKd * m_dfeDiff) =
  //     m_dfeSum  = 

  // dfeIn=145.88560, dfErrorTime=1556750915.09088, m_dfDT=0.10035, dfDiffNow=0.09397, DiffHistSize=9,
  // mdfeDiff(1)=0.92005, mdfeDiff(2)=0.10223, m_dfOut(1)218.84885, m_dfOut(1)100 

  
  
  //do pid control
  m_dfOut = (m_dfKp*m_dfe) + (m_dfKd*m_dfeDiff) + m_dfeSum; //PID公式求输出值
  if(m_debug) 
    m_debug_str += ", m_dfOut(1)" + doubleToStringX(m_dfOut);
  //note Ki is already in dfeSum
  
  //prevent saturation..防止输出饱和
  if(fabs(m_dfOut)>=fabs(m_dfOutputLimit) ) {        
    int nSign =(int)( fabs(m_dfOut)/m_dfOut);
    m_dfOut = nSign*fabs(m_dfOutputLimit);
    if(m_debug) 
      m_debug_str += ", m_dfOut(1)" + doubleToStringX(m_dfOut);
    m_max_sat = true;
  }
  
  //save old value..保留上一次的结果
  m_dfeOld    = m_dfe;
  m_dfOldTime = dfErrorTime;
  
  dfOut = m_dfOut;
  
  //do logging..
  Log();
  
  return(true);
}

2.PIDEngine

由图可知，PIDEngine的主要作用就是求解期望的方向舵角rudder、升降舵角elevator以及推进器推力thrust，分别可以控制航向角、俯仰角以及推进速度，在三个自由度上控制AUV，便可以实现AUV的运动控制。
下面是PIDEngine的主要代码

//------------------------------------------------------------
// Procedure: getDesiredRudder求解期望方向舵角
// Rudder angles are processed in degrees

double PIDEngine::getDesiredRudder(double desired_heading,
				   double current_heading,
				   double max_rudder)//期望航向，当前航向，最大方向舵角
{
  desired_heading = angle180(desired_heading);
  double heading_error = current_heading - desired_heading;
  heading_error = angle180(heading_error);//将舵角转化在-180到180之间
  double desired_rudder = 0;
  m_heading_pid.Run(heading_error, m_current_time, desired_rudder);//使用pid求解方向舵角的值
  desired_rudder *= -1.0;

  // Added 4/19 mikerb: monitor and report max saturation events
  m_max_sat_hdg_str.clear();
  m_max_sat_hdg = m_heading_pid.getMaxSat();//检查是否出现输出饱和事件
  if(m_max_sat_hdg)
    m_max_sat_hdg_str = m_heading_pid.getDebugStr();//出现的话输出bug情况
  
  // Enforce limit on desired rudder
  MOOSAbsLimit(desired_rudder,max_rudder);

  string rpt = "PID_COURSE: ";
  rpt += " (Want):" + doubleToString(desired_heading);
  rpt += " (Curr):" + doubleToString(current_heading);
  rpt += " (Diff):" + doubleToString(heading_error);
  rpt += " RUDDER:" + doubleToString(desired_rudder);
  m_pid_report.push_back(rpt);
  return(desired_rudder);
}

//------------------------------------------------------------
// Procedure: getDesiredThrust

double PIDEngine::getDesiredThrust(double desired_speed, 
				   double current_speed,
				   double current_thrust,
				   double max_thrust)
{
  double speed_error  = desired_speed - current_speed;
  double delta_thrust = 0;
  double desired_thrust = current_thrust;

  // If NOT using PID control, just apply multiple to des_speed
  if(m_speed_factor != 0) {//速度因子不为0，这里假设速度和推力呈线性关系
    desired_thrust = desired_speed * m_speed_factor;
  }
  // ELSE apply the PID contoller to the problem.
  else {//速度因子为0，那么速度和推力采用PID进行计算
    m_speed_pid.Run(speed_error,  m_current_time, delta_thrust);
    desired_thrust += delta_thrust;

    // Added 4/19 mikerb: monitor and report max saturation events
    m_max_sat_spd_str.clear();
    m_max_sat_spd = m_speed_pid.getMaxSat();
    if(m_max_sat_spd)
      m_max_sat_spd_str = m_speed_pid.getDebugStr();
  }
  
  if(desired_thrust < 0)
    desired_thrust = 0;

  if(m_speed_factor != 0) {
    string rpt = "PID_SPEED: ";
    rpt += " (Want):" + doubleToString(desired_speed);
    rpt += " (Curr):" + doubleToString(current_speed);
    rpt += " (Diff):" + doubleToString(speed_error);
    rpt += " (Fctr):" + doubleToString(m_speed_factor);
    rpt += " THRUST:" + doubleToString(desired_thrust);
    m_pid_report.push_back(rpt);
  }    
  else {
    string rpt = "PID_SPEED: ";
    rpt += " (Want):" + doubleToString(desired_speed);
    rpt += " (Curr):" + doubleToString(current_speed);
    rpt += " (Diff):" + doubleToString(speed_error);
    rpt += " (Delt):" + doubleToString(delta_thrust);
    rpt += " THRUST:" + doubleToString(desired_thrust);
    m_pid_report.push_back(rpt);
  }

  // Enforce limit on desired thrust
  MOOSAbsLimit(desired_thrust,max_thrust);

  return(desired_thrust);
}

//------------------------------------------------------------
// Procedure: getDesiredElevator
// Elevator angles and pitch are processed in radians

double PIDEngine::getDesiredElevator(double desired_depth,
				     double current_depth,
				     double current_pitch,
				     double max_pitch,
				     double max_elevator)
{
  double desired_elevator = 0;
  double desired_pitch = 0;
  double depth_error = current_depth - desired_depth;
  m_z_to_pitch_pid.Run(depth_error, m_current_time, desired_pitch);
  m_max_sat_dep = m_z_to_pitch_pid.getMaxSat();

  // Enforce limits on desired pitch
  MOOSAbsLimit(desired_pitch,max_pitch);

  double pitch_error = current_pitch - desired_pitch;
  m_pitch_pid.Run(pitch_error, m_current_time, desired_elevator);
  m_max_sat_dep = m_max_sat_dep || m_pitch_pid.getMaxSat();


  // Added 4/19 mikerb: monitor and report max saturation events
  m_max_sat_dep_str.clear();
  if(m_z_to_pitch_pid.getMaxSat()) {
    m_max_sat_dep = true;
    m_max_sat_dep_str = "Z:" + m_z_to_pitch_pid.getDebugStr();
  }
  if(m_pitch_pid.getMaxSat()) {
    m_max_sat_dep = true;
    m_max_sat_dep_str += "P:" + m_z_to_pitch_pid.getDebugStr();
  }
  
  // Convert desired elevator to degrees
  desired_elevator=MOOSRad2Deg(desired_elevator);

  // Enforce elevator limit
  MOOSAbsLimit(desired_elevator,max_elevator);
  
  string rpt = "PID_DEPTH: ";
  rpt += " (Want):" + doubleToString(desired_depth);
  rpt += " (Curr):" + doubleToString(current_depth);
  rpt += " (Diff):" + doubleToString(depth_error);
  rpt += " ELEVATOR:" + doubleToString(desired_elevator);
  m_pid_report.push_back(rpt);

  return(desired_elevator);
}

上述三个函数具体内容大差不差：
关于推力，通过PID通过速度偏差计算推力差值，在当前推力的基础上进行加和，求得期望的推力值。
关于航向角是PID通过航向角偏差直接计算期望方向舵角，对应着现实中方向舵可以直接改变。
关于升降舵，采用双闭环的计算方式，外环是深度和俯仰角之间的PID计算，内环是俯仰角和升降舵之间的PID计算，最后计算得到期望的升降舵角

3.MarinePID

最后一个文件，也是MOOSAPP的主要文件，其中包含着PID计算过程及其bug报告的相关内容。
其主要作用就是从MOOSDB里订阅需要的AUV当前航速、航向、深度等变量，经过计算之后，再发布出AUV期望的推力、方向舵、升降舵等相关变量。并且对PID计算中的各种限幅进行了输入。

//--------------------------------------------------------------------
// Procedure: Iterate()

bool MarinePID::Iterate()
{
  m_iteration++;//循环次数 初始值为0
  postCharStatus();

  if(!m_has_control) {//这个标志代表着手动控制MOOS_MANUAL_OVERIDE的值为false时，m_has_control为true，表示开始自动控制
    postAllStop();//进入手动控制，循环失效
    return(false);
  }

  double current_time = MOOSTime();

  if(m_verbose == "verbose") {//计算每次进行控制的周期，并进行报告
    double hz = m_iteration / (MOOSTime() - m_start_time);
    cout << endl << endl << endl;
    cout << "PID REPORT: (" << m_iteration << ")";
    cout << "(" << hz << "/sec)" << endl;
  }

  if((current_time - m_time_of_last_helm_msg) > m_tardy_helm_thresh) {//当前时间与最近一次收到helm状态（heading、depth、speed）的时间比m_tardy_helm_thresh（2s）大时，说明内部通信有延时。
    if(!m_paused)
      MOOSDebugWrite("Paused Due To Tardy HELM Input: THRUST=0");
    cout << "Paused Due To Tardy HELM Input: THRUST=0" << endl;
    m_paused = true;//延时表示为true
    Notify("DESIRED_THRUST", 0.0);//发布期望推力为0，不对AUV进行控制
    m_current_thrust = 0;
    return(true);
  }
  
  if((current_time - m_time_of_last_nav_msg) > m_tardy_nav_thresh) {//当前时间与最近一次收到NAV状态（NAV_X、NAV_Y等等）的时间比m_tardy_helm_thresh（2s）大时，说明内部通信有延时。
    if(!m_paused)
      MOOSDebugWrite("Paused Due To Tardy NAV Input: THRUST=0");
    cout << "Paused Due To Tardy NAV Input: THRUST=0" << endl;
    m_paused = true;//延迟标志位置为true
    Notify("DESIRED_THRUST", 0.0);
    m_current_thrust = 0;
    return(true);
  }

  double rudder = 0;
  double thrust = 0;
  double elevator = 0;

  m_pengine.updateTime(current_time);//给m_pengine的currenttime赋值

  rudder = m_pengine.getDesiredRudder(m_desired_heading, m_current_heading, 
				    m_max_rudder);//计算期望方向舵
  
  //--------------------//以下情况针对方向舵板不太稳定的情况
  //对方向舵角进行微调，根据两次计算的时间间隔不同，分为两种情况
  double rbias_duration = current_time - m_rudder_bias_timestamp;
  if(rbias_duration > m_rudder_bias_duration) {//时间间隔大于预设时间
    rbias_duration = 0;
    m_rudder_bias_timestamp = current_time;
    m_rudder_bias_side *= -1;//减少一下调节幅度
  }
  double pct = rbias_duration / m_rudder_bias_duration;//按照规定时间间隔比例增加或减小一定的舵板调节角度
  double bias = m_rudder_bias_side * (pct * m_rudder_bias_limit);//m_rudder_bias_limit该变量表示舵板轻微的晃动幅度
  rudder += bias;
  //--------------------
  

  thrust = m_pengine.getDesiredThrust(m_desired_speed, m_current_speed, 
				    m_current_thrust, m_max_thrust);//计算期望推力


  if(m_depth_control)
    elevator = m_pengine.getDesiredElevator(m_desired_depth, m_current_depth,
					  m_current_pitch, m_max_pitch, 
					  m_max_elevator);//计算期望方升降舵
  
  if((m_desired_speed <= 0.001) && (m_desired_speed >= -0.001))//速度过小时视为0
    thrust = 0;

  vector<string> pid_report;
  if(m_verbose == "verbose") {//详细汇报PID计算情况
    pid_report = m_pengine.getPIDReport();
    for(unsigned int i=0; i<pid_report.size(); i++)
      cout << pid_report[i] << endl;
  }
  m_pengine.clearReport();

  m_paused = false;//延迟标志位置为false

  if(thrust ==0)//推力为0时，方向舵角也为0
    rudder = 0;
  Notify("DESIRED_RUDDER", rudder);
  Notify("DESIRED_THRUST", thrust);
  m_current_thrust = thrust;
  if(m_depth_control)
    Notify("DESIRED_ELEVATOR", elevator);

  m_allstop_posted = false;

  // Added April 2019 by mikerb
  // Simple quick check if max saturation event occurred
  // If so, and debug was turned on, report debug info for the event
  if(m_pengine.getMaxSatHdg()) {//针对PID超调情况，进行查看
    Notify("PID_MAX_SAT_HDG", "true");
    string debug_info = m_pengine.getMaxSatHdgStr();
    if(debug_info != "")
      Notify("PID_MAX_SAT_HDG_DEBUG", debug_info);
  }
  if(m_pengine.getMaxSatSpd()) {
    Notify("PID_MAX_SAT_SPD", "true");
    string debug_info = m_pengine.getMaxSatSpdStr();
    if(debug_info != "")
      Notify("PID_MAX_SAT_SPD_DEBUG", debug_info);
  }
  if(m_pengine.getMaxSatDep()) {
    Notify("PID_MAX_SAT_DEp", "true");
    string debug_info = m_pengine.getMaxSatDepStr();
    if(debug_info != "")
      Notify("PID_MAX_SAT_DEP_DEBUG", debug_info);
  }
  
  return(true);
}
  

---

总结

本节基本上对pMarinePID的架构有了一个较为清晰的说明，重点注释了一下三个程序文件中的主要函数，并且对三个自由度控制方式进行了讲解，总体上梳理了MOOS-IVP自带的较常见的控制方法。