高度控制:高于目標值時應該減速，要輸出負值，所以高度差=目標高度-當前高度

速度控制：向上時(速度為正)應該減速，要輸出負值，所以速度差=高度差的速度-當前速度

如果只有高度控制時：出現高度變化時，高度差要么是正數要么是負數。即使高度差會慢慢變小，但是高度差在沒有有到達目標高度時，高度差的符號是不會變的，符號不變就一直加速，盡管加速度在變小，有加速就一定會越過目標值。越過目標值才會開始減速。在減速開始的瞬間，就意味著已經過越過目標點。高度差又出現了，再次重復上面的過程。所以不管怎么調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;//時間差,usdt = timestamp - timestampPre;//時間差,ustimestampPre = timestamp;velocity = mesurementHeight - mesurementPre;//計算測量值的變化量，即速度velocity = velocity * 1000000.0f/ dt;//實際速度cm/svelocity = constrain(velocity, -1500, 1500);// 限制 velocity ±1500cm/svelocity = velocity * 0.2 + velocityPre * 0.8;//濾波一下velocityPre = velocity;mesurementPre = mesurementHeight;outH = PID_Compute(targetHeight - mesurementHeight, _pidH);//如果存在高度差，則會輸出速度outV = PID_Compute(outH - velocity, _pidH);//達到上面的速度result += outV;//輸出是調節值，要穩定輸出則要累計。位置型PIDresult = constrainf(result, 1000, 2000);//限制輸出范圍result = result * 0.1 + resultPre * 0.9;//輸出濾波，限制突變resultPre = result;return result; }

高度計用是的歌爾的SPL06,加速度計是MPU6050,看看定高效果

總結

以上是生活随笔為你收集整理的无人机定高算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。