-
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- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
核心代码：
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); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
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); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
上述三个函数具体内容大差不差：
关于推力，通过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); }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
总结
本节基本上对pMarinePID的架构有了一个较为清晰的说明，重点注释了一下三个程序文件中的主要函数，并且对三个自由度控制方式进行了讲解，总体上梳理了MOOS-IVP自带的较常见的控制方法。
-
相关阅读:
YOLOv5算法进阶改进（5）— 主干网络中引入SCConv | 即插即用的空间和通道维度重构卷积
HarmonyOS鸿蒙原生应用开发设计- 图标库
yolox
iOS Facebook SDK 安装
云南美食介绍 简单静态HTML网页作品 美食餐饮网站设计与实现 学生美食网站模板
huggingface使用bert
【go从入门到精通】常用标准库的用法
ESP32-C3入门教程 IoT篇⑦——微软云 Microsoft Azure 物联网 IoT 中心 EspAzureIoT 实战
Spring实例化源码解析之ConfigurationClassParser(三)
户外LED大屏推广的精确受众分析-华媒舍
- 原文地址：https://blog.csdn.net/weixin_44151170/article/details/125432949