高度控制:高于目标值时应该减速，要输出负值，所以高度差=目标高度-当前高度
速度控制：向上时(速度为正)应该减速，要输出负值，所以速度差=高度差的速度-当前速度
如果只有高度控制时：出现高度变化时，高度差要么是正数要么是负数。即使高度差会慢慢变小，但是高度差在没有有到达目标高度时，高度差的符号是不会变的，符号不变就一直加速，尽管加速度在变小，有加速就一定会越过目标值。越过目标值才会开始减速。在减速开始的瞬间，就意味着已经过越过目标点。高度差又出现了，再次重复上面的过程。所以不管怎么调PID参数，都不可能稳定高度。
那么要稳定高度，就必须要到达目标高度之前，输出反向的力刹住。
重新分析高度差控制的环节会发现，到回到目标高的时候，输出减少到最小值了，但是因为是一直加速度回来，所以速度也达到到了最大值。那么就可以利用此时的速度来做刹车了。所以有了下面的速度控制。
如果只有速度控制器时：出现向上的速度时，输出负值，抵制加速，出现向下的速度时，输出正值，上升回来。那么高度控制实际上就是对速度的控制.
然而只有速度控制的话，由于高度计的精度就在那，微小的速度变化是检测不出来的。那么在缓慢下降和上升时就没办法调节了。
所以前面的高度差就有作用了，高度差作为微调添加到速度那边去计算。就算速度为0时，由于高度差的存在，输出也不会为0。直到高度差和速度都为0时才不输出。
现在思路就很清晰了，速度作为动态快速响应，高度作为微调校正。所以速度为主，高度为辅助，作为微调的高度差的PID的输出绝对不能越过速度。这么看的话高度环只有比例一个调节也可以了。
算法有了，剩下的问题是速度和高度怎么测量，用什么传感器了。
typedef struct {
float kp;
float ki;
float kd;
float max;
float min;
float error;
float integral;
float differential;
float maxIntegral;
float minIntegral;
float lastError;
float lastInput;
}PID_InitTypedef;
long CONSTRAIN(long input, long min, long max)
{
if (input < min)
input = min;
if (input > max)
input = max;
return input;
}
float PID_ComputeInt(float error, PID_InitTypedefInt16* pid)
{
pid->error = error;
pid->integral += pid->error;
pid->integral = CONSTRAIN(pid->integral, pid->minIntegral, pid->maxIntegral);
pid->differential = pid->error - pid->lastError;
//pid->lastInput = input;
pid->lastError = pid->error;
float result = pid->kp * pid->error + pid->ki * pid->integral/1000 + pid->kd * pid->differential;
result = CONSTRAIN(result, pid->min, pid->max);
return result;
}
/*定高串级PID计算*/
/*mesurement:测量到的高度数据*/
/*targetPoint：目标高度*/
/*_pidH：高度环PID*/
/*_pidV：速度环PID*/
/*Output: 速度*/
/*timestamp: 时间戳（us）*/
float KeepHeight(float mesurementHeight, float targetHeight, PID_InitTypedef* _pidH, PID_InitTypedef *_pidV, uint32_t timestamp)
{
static float velocity;//高度的变化速度，
static float velocityPre;//上次高度的变化速度，
static float mesurementPre;//上次的高度
static float result;
static float resultPre;
static float timestampPre;//上次时间戳
float outH, outV;//高度度环输出，速度环输出
float dt;//时间差,us
dt = timestamp - timestampPre;//时间差,us
timestampPre = timestamp;
velocity = mesurementHeight - mesurementPre;//计算测量值的变化量，即速度
velocity = velocity * 1000000.0f/ dt;//实际速度cm/s
velocity = constrain(velocity, -1500, 1500);// 限制 velocity ±1500cm/s
velocity = velocity * 0.2 + velocityPre * 0.8;//滤波一下
velocityPre = velocity;
mesurementPre = mesurementHeight;
outH = PID_Compute(targetHeight - mesurementHeight, _pidH);//如果存在高度差，则会输出速度
outV = PID_Compute(outH - velocity, _pidH);//达到上面的速度
result += outV;//输出是调节值，要稳定输出则要累计。位置型PID
result = constrainf(result, 1000, 2000);//限制输出范围
result = result * 0.1 + resultPre * 0.9;//输出滤波，限制突变
resultPre = result;
return result;
}高度计用是的歌尔的SPL06,加速度计是MPU6050,看看定高效果
扫描二维码关注公众号，回复：
12401013 查看本文章
