无人机定高算法

大家好，我是讯享网，很高兴认识大家。

高度控制:高于目标值时应该减速，要输出负值，所以高度差=目标高度-当前高度

速度控制：向上时(速度为正)应该减速，要输出负值，所以速度差=高度差的速度-当前速度

如果只有高度控制时：出现高度变化时，高度差要么是正数要么是负数。即使高度差会慢慢变小，但是高度差在没有有到达目标高度时，高度差的符号是不会变的，符号不变就一直加速，尽管加速度在变小，有加速就一定会越过目标值。越过目标值才会开始减速。在减速开始的瞬间，就意味着已经过越过目标点。高度差又出现了，再次重复上面的过程。所以不管怎么调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 * .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,看看定高效果

相关推荐