## **6. PID算法的一点简化**
为了在SE中更好的使用，在下对PID算法做了一点改进，从而让参数更容易被理解。
同时也做了简单的实验来验证结果。
简化后的公式如下：
> `$ U = P(e + Ie_{i}+De_{d}) $`
其中
>`$ e_{i} = (e_1 + e_2 + e_3 + ... + e_T)/T $`
>`$ e_{d} = e_T - e_{T-1} $`
U是输出
P是比例系数
I是积分系数
D是微分系数
T是统计周期（例如30帧）
e是误差
ei 是统计周期内所有误差的累加
ed 是当前帧误差 - 前一帧误差
附一段代码
```
const int T = 30; //周期，这个周期不是公式中的时间间隔T，而是储存累加结果的周期
const double P = 1; //比例系数
const double I = 10; //积分系数
const double D = 10; //微分系数
List<double> d_data = new List<double>(); //储存误差的数组
double getPID(double d){
d_data.Add(d);
if(d_data.Count > T){
d_data.Remove(d_arr[0]);
}
double sum_d = 0;
foreach(double i in d_data){
sum_d += i;
}
return P * (d + I*(sum_d/T) + D*(d_data[d_data.Count - 1] - d_data[d_data.Count - 2]) ); //输出结果
}
```
<br>
## 实验
实验中采用了转子，转子头端挂载了重物。为了排除干扰，设置了两个完全一样，挂载的重物也完全一样的转子。
让它们分别做相同的运动，记录不同算法所需的运动时间。
装置如图：
先说几个概念：
* 判定到位：控制转子运动后，转子抵达指定位置，并且速度抵达指定范围时，判定转子到位。
程序开始执行后运行如下步骤：
1. 将两个转子默认位于120度位置。
2. 程序控制两个转子转到0度位置，当两个转子判定到位后，记录转子1开始时间
3. 转子1转到120度位置，判定到位后记录转子1结束时间。同时记录转子2开始时间
4. 转子2转到120度，判定到位后记录转子2结束时间。
先简单说下结果，在普通算法（输出 = 误差）中，由于大部分都无法最终停止，所以就没做它的数据记录。
然后分别采用了误差累加算法和PID算法分别测试了几次，结果如下：
R1开始表示过程2中两个转子从120度归位到0度的时间帧，
R2结束表示转子1从0度转到120度，归位时候的时间帧，
R2开始表示转子2开始从0度转到120度的时间帧，
R2结束表示转子2抵达120度判定归位的时间帧，
误差累加算法
PID算法
## 完整的实验代码
代码要求两个转子，一个命名为Rotor_1，一个命名为Rotor_2
代码中封装了一些对方块的操作，这个封装是下一步准备做的一个项目。
```
int t;
bool init;
Rotor rt1;
Rotor rt2;
int rt1_start;
int rt2_start;
int rt1_end;
int rt2_end;
int step;
void Main(string arguments){
if(!init){GetBlocks(); return;}
t ++;
if(step == 0){
bool a = rt1.TurnToPID(0);
bool b = rt2.TurnToPID(0);
if(a && b){
step = 1;
}
}else if(step == 1){
rt1_start = t;
step = 2;
}else if(step == 2){
rt2.TurnToPID(0);
if(rt1.TurnToPID(120)){
rt1_end = t;
rt2_start = t;
step = 3;
}
}else if(step == 3){
rt1.TurnToPID(120);
if(rt2.TurnToPID(120)){
rt2_end = t;
step = 4;
}
}else if(step == 4){
rt1.TurnToPID(120);
rt2.TurnToPID(120);
}
Echo("模式: PID");
Echo("P参数: "+rt1.turn_pid_p);
Echo("I参数: "+rt1.turn_pid_i);
Echo("D参数: "+rt1.turn_pid_d);
Echo("T参数: "+rt1.turn_sum_t);
Echo("=======================");
Echo("R1开始:"+rt1_start.ToString());
Echo("R1结束:"+rt1_end.ToString());
Echo("R2开始:"+rt2_start.ToString());
Echo("R2结束:"+rt2_end.ToString());
}
void GetBlocks(){
rt1 = new Rotor(GridTerminalSystem.GetBlockWithName("Rotor1"));
rt2 = new Rotor(GridTerminalSystem.GetBlockWithName("Rotor2"));
init = true;
}
Program(){
Runtime.UpdateFrequency = UpdateFrequency.Update1;
}
class Block
{
// == 构造方法
public Block(IMyTerminalBlock b){
this._block = b;
}
// == 成员属性
public IMyTerminalBlock _block; //方块
public string Name{
get { return this._block.CustomName; }
set { this._block.CustomName = value; }
}
public string Data{
get { return this._block.CustomData; }
set { this._block.CustomData = value; }
}
public Vector3D Position{
get { return this._block.GetPosition(); }
}
public bool ShowInTerminal{
get { return this._block.ShowInTerminal; }
set { this._block.ShowInTerminal = value; }
}
public bool ShowInInventory{
get { return this._block.ShowInInventory; }
set { this._block.ShowInInventory = value; }
}
// 静态方法
public static List<Block> createListBlock(List<IMyTerminalBlock> blocks){
List<Block> list = new List<Block>();
foreach(IMyTerminalBlock b in blocks){
list.Add(new Block(b));
}
return list;
}
}
class LCD: Block
{
// == 构造方法
public LCD(IMyTerminalBlock b):base(b){
this._block = b;
this._lcd = b as IMyTextPanel;
}
public LCD(IMyTextPanel b):base(b){
this._block = b;
this._lcd = b as IMyTextPanel;
}
// == 静态方法
public static List<LCD> createListBlock(List<IMyTextPanel> blocks){
List<LCD> list = new List<LCD>();
foreach(IMyTextPanel b in blocks){
list.Add(new LCD(b));
}
return list;
}
// == 成员属性
public IMyTextPanel _lcd; //lcd方块
public string Title{
get { return this._lcd.GetPublicTitle(); }
set { this._lcd.WritePublicTitle(value); }
}
public string Text{
get { return this._lcd.GetPublicText(); }
set { this._lcd.WritePublicText(value); }
}
public bool ShowText{
get { return this._lcd.GetValueBool("ShowTextOnScreen"); }
set { this._lcd.SetValue("ShowTextOnScreen", value); }
}
// == 成员方法
public void AppendText(string str){
this._lcd.WritePublicText(str, true);
}
}
class Rotor: Block
{
// == 构造方法
public Rotor(IMyTerminalBlock b):base(b){
this._block = b;
this._rotor = b as IMyMotorStator;
}
public Rotor(IMyMotorStator b):base(b){
this._block = b;
this._rotor = b as IMyMotorStator;
}
// == 静态方法
public static List<Rotor> createListBlock(List<IMyMotorStator> blocks){
List<Rotor> list = new List<Rotor>();
foreach(IMyMotorStator b in blocks){
list.Add(new Rotor(b));
}
return list;
}
// 角度转换
public static double AngleToRad(double angle){
return angle*Math.PI/180;
}
public static double RadToAngle(double rad){
return rad*180/Math.PI;
}
public static double NormalizeAngle(double angle){
if(angle > 360){
return angle%360;
}else if(angle < 0){
return 360 + (angle%360);
}else{
return angle;
}
}
// == 成员属性
public IMyMotorStator _rotor; //转子方块
public double TurnToAngleAccuracy = 0.1; //转动到位判定角度精度（角度）
public double TurnToVelocityAccuracy = 0.01; //转动到位判定速度精度（RPM，圈每分钟）
public double Angle{
get { return Rotor.RadToAngle(this._rotor.Angle); }
}
public double AngleRad{
get { return this._rotor.Angle; }
}
public double Velocity{
get { return this._rotor.TargetVelocityRPM; }
set { this._rotor.TargetVelocityRPM = (float)value; }
}
// public double VelocityRad{
// get { return this._rotor.TargetVelocity; }
// set { this._rotor.TargetVelocity = (float)value; }
// }
// == 成员方法
// 转到某个角度
// input angle 目标角度，使用角度而不是弧度
// input mode 运动方式，1顺时针，-1逆时针，0自动
// input velocity 指定速度，不指定默认用误差累加算法
public double turn_sum_p = 5; //误差累加控制比例系数
public double turn_sum_t = 20; //误差累加控制周期
private List<double> turn_sum_diff_i = new List<double>();
public bool TurnTo(double angle = 0, int mode = 0, double velocity = 0){
angle = Rotor.NormalizeAngle(angle);
if(mode == 0){
if(angle > this.Angle){
mode = angle - this.Angle > 180 ? -1 : 1;
}else{
mode = this.Angle - angle > 180 ? 1 : -1;
}
}
//指定速度方式
if(velocity != 0){
this.Velocity = mode * velocity;
return Math.Abs(angle - this.Angle) <= 0.5;
}
// diff是当前角度和目标角度的转动角度差，范围是0-1
double diff = 0;
if(mode == 1){
if(angle > this.Angle){
diff = angle - this.Angle;
}else{
diff = (360 - this.Angle) + angle;
}
}else if(mode == -1){
if(this.Angle > angle){
diff = this.Angle - angle;
}else{
diff = this.Angle + (360 - angle);
}
}
diff /= 360;
//对角度差进行逐帧累加，累加次数为 PID控制积分系数
double diff_i = 0; //积分求和项
if(this.turn_sum_diff_i.Count >= this.turn_sum_t){
this.turn_sum_diff_i.Remove(this.turn_sum_diff_i[0]);
}
this.turn_sum_diff_i.Add(diff);
foreach(double d in this.turn_sum_diff_i){
diff_i += d;
}
//输出结果是 运动方向 * PID比例系数 * 角度差的逐帧累加和
this.Velocity = mode * this.turn_sum_p*diff_i;
return diff*360 <= this.TurnToAngleAccuracy && Math.Abs(this.Velocity) <= this.TurnToVelocityAccuracy;
}
// 转到某个弧度
public bool TurnToRad(double angle = 0, int mode = 0, double velocity = 0){
return this.TurnTo(Rotor.RadToAngle(angle), mode, velocity);
}
//PID算法的旋转
public double turn_pid_p = 5; //比例系数
public double turn_pid_i = 20; //积分系数(增加后降低动态误差，增加震荡)
public double turn_pid_d = 20; //微分系数（增加后降低震荡）
public double turn_pid_t = 30; //误差累加控制周期
private List<double> turn_pid_diff_data = new List<double>();
public bool TurnToPID(double angle = 0, int mode = 0, double velocity = 0){
angle = Rotor.NormalizeAngle(angle);
if(mode == 0){
if(angle > this.Angle){
mode = angle - this.Angle > 180 ? -1 : 1;
}else{
mode = this.Angle - angle > 180 ? 1 : -1;
}
}
//指定速度方式
if(velocity != 0){
this.Velocity = mode * velocity;
return Math.Abs(angle - this.Angle) <= 0.5;
}
// diff是当前角度和目标角度的转动角度差，范围是0-1
double diff = 0;
if(mode == 1){
if(angle > this.Angle){
diff = angle - this.Angle;
}else{
diff = (360 - this.Angle) + angle;
}
}else if(mode == -1){
if(this.Angle > angle){
diff = this.Angle - angle;
}else{
diff = this.Angle + (360 - angle);
}
}
diff /= 360;
//对角度差进行逐帧累加，累加次数为 PID控制积分系数
double diff_i = 0; //积分求和项
if(this.turn_pid_diff_data.Count >= this.turn_pid_t){
this.turn_pid_diff_data.Remove(this.turn_pid_diff_data[0]);
}
this.turn_pid_diff_data.Add(diff);
foreach(double d in this.turn_pid_diff_data){
diff_i += d;
}
//输出结果是
this.Velocity = mode*this.turn_pid_p*(diff + diff_i*this.turn_pid_i/turn_pid_t + (diff - this.turn_pid_diff_data[this.turn_pid_diff_data.Count-1])*this.turn_pid_d);
return diff*360 <= this.TurnToAngleAccuracy && Math.Abs(this.Velocity) <= this.TurnToVelocityAccuracy;
}
}
class Wheel: Block
{
// == 构造方法
public Wheel(IMyTerminalBlock b):base(b){
this._block = b;
this._wheel = b as IMyMotorSuspension;
}
public Wheel(IMyMotorSuspension b):base(b){
this._block = b;
this._wheel = b as IMyMotorSuspension;
}
// == 静态方法
public static List<Wheel> createListBlock(List<IMyMotorSuspension> blocks){
List<Wheel> list = new List<Wheel>();
foreach(IMyMotorSuspension b in blocks){
list.Add(new Wheel(b));
}
return list;
}
// == 成员属性
public IMyMotorSuspension _wheel;
public bool SteeringOnOff{ //是否允许转向
get { return this._wheel.Steering; }
set { this._wheel.Steering = value; }
}
public bool PropulsionOnOff{ //是否允许加速
get { return this._wheel.Propulsion; }
set { this._wheel.Propulsion = value; }
}
public bool InvertSteerOnOff{ //是否反向转向
get { return this._wheel.InvertSteer; }
set { this._wheel.InvertSteer = value; }
}
public bool InvertPropulsionOnOff{ //是否反向加速
get { return this._wheel.InvertPropulsion; }
set { this._wheel.InvertPropulsion = value; }
}
public double Power{ //功率
get { return this._wheel.Power; }
set { this._wheel.Power = (float)value; }
}
public double Strength{ //强度
get { return this._wheel.Strength; }
set { this._wheel.Strength = (float)value; }
}
public double Friction{ //摩擦力
get { return this._wheel.Friction; }
set { this._wheel.Friction = (float)value; }
}
public double SpeedLimit{ //速度限制，0~360，其中360为无限制
get { return this._wheel.GetValueFloat("Speed Limit"); }
set { this._wheel.SetValueFloat("Speed Limit", (float)value); }
}
public double Speed{ //加速越级值，0~1
get { return this._wheel.GetValueFloat("Propulsion override"); }
set { this._wheel.SetValueFloat("Propulsion override", (float)value); }
}
public double Steer{ //转向越级值，0~1
get { return this._wheel.GetValueFloat("Steer override"); }
set { this._wheel.SetValueFloat("Steer override", (float)value); }
}
public double Height{ //悬架高度，-1.5~1.3
get { return this._wheel.Height; }
set { this._wheel.Height = (float)value; }
}
}
```
- 序言
- 写在前面的话
- 太空工程师
- MEA小组
- 一、入门
- 1.1 基础概念
- 1.2 编程工具
- 1.3 变量
- 1.4 函数 Function
- 1.5 基本语法
- 1.5.1 运算符
- 1.5.2 if
- 1.5.3 for
- 1.5.4 其他语法
- 1.3 类 Class
- 二、编程块
- 2.1 方块的概念
- 2.2 List<T>结构
- 2.3 获取方块
- 2.4 方块的使用
- 三、Ship 类
- 3.1 简介
- Ship v0.5
- 代码
- 手册（待更新）
- 例子（待更新）
- Ship v1.0
- 代码
- 例子
- 文档
- 实例化
- 内置变量
- 内置方法
- Target类
- 四、运动控制算法在SE中的应用
- 4.1 运动控制介绍
- 4.2 过程控制
- 4.3 震荡和动态误差
- 4.4 误差累加方案
- 4.5 PID算法
- 4.6 对PID算法的一点点简化
- 4.7 一阶惯性系统的PID算法优化的研究
- 五、MEA方块类
- 5.0 核心代码目录
- v1.0核心代码
- v1.1 核心代码
- v2.0 核心代码
- 5.1 类的概念
- 5.2 MEA的方块类（Block）
- 5.3 方块类文档
- 5.4 方块类2.0 全教程
- 5.4.1 安装和使用
- 5.4.2 方块类（Block）
- 5.4.3 显示屏类（DisplayScreen）
- 5.4.4 LCD类（LCD）
- 5.4.5 主控座椅类（Cockpit）
- 六、疯猴的编程笔记
- 第一个程序
- 获取和控制其他块
- 物流与生产
- 界面与通信
- 运动与姿态
- 侦测与导航
- 七、SteamZhou的笔记
- 有趣而花里胡哨的IDEA
- 八、质子对撞炮的笔记
- 属性 Property
- 接口 interface