引言：PID算法作為運動控制的經(jīng)典算法，有著不錯的效果。目前PID算法也有了各種升級版，這里不涉及到串級PID和增量式PID控制，僅做最基礎(chǔ)的PID控制，有需要的話，可以自行優(yōu)化，或者聯(lián)系我們一起開發(fā)。對于研發(fā)人員來講，熟練掌握了PID算法的設(shè)計與實現(xiàn)過程，就可以應(yīng)對一般的研發(fā)問題了。
PID概念
PID，即比例 Proportion、積分 Integral 和微分 Derivative 三個單詞的縮寫；比例積分微分控制，簡稱PID控制。
簡單講，根據(jù)給定值和實際輸出值構(gòu)成控制偏差，將偏差按比例、積分和微分通過線性組合構(gòu)成控制量，對被控對象進(jìn)行控制。
常規(guī) PID 控制器作為一種線性控制器。
文章來源地址http://www.zghlxwxcb.cn/news/detail-811425.html
這里僅僅進(jìn)行簡單的介紹，具體PID原理可以自行參考其他相關(guān)教程，本文僅從代碼進(jìn)行介紹
PID控制原理
PID控制的基本原理是通過測量控制系統(tǒng)的實際輸出與期望輸出之間的誤差，
并根據(jù)比例、積分和微分三個參數(shù)計算控制量，調(diào)整系統(tǒng)的輸入，使輸出盡可能接近期望值。
具體來說，PID控制器的三個控制項分別是比例項（P項）、積分項（I項）和微分項（D項）。
比例項（P項）：通過將誤差與一個比例常數(shù)相乘，得到一個控制量，該控制量與誤差成正比。
P項的作用是使控制量與誤差呈線性關(guān)系，比例常數(shù)的大小決定了系統(tǒng)的響應(yīng)速度和穩(wěn)定性。
積分項（I項）：通過將誤差與一個積分常數(shù)相乘，得到一個控制量，該控制量與誤差積分之和成正比。
I項的作用是消除誤差的持久性，防止系統(tǒng)處于穩(wěn)態(tài)誤差狀態(tài)。
微分項（D項）：通過將誤差與一個微分常數(shù)相乘，得到一個控制量，該控制量與誤差的變化率成正比。
D項的作用是消除誤差的瞬時變化，增強(qiáng)系統(tǒng)的穩(wěn)定性和控制精度。
三個項的綜合作用，可以根據(jù)誤差大小、持久性和變化率，快速、準(zhǔn)確地調(diào)節(jié)系統(tǒng)的輸入，
使輸出值穩(wěn)定地達(dá)到期望狀態(tài)。
PID控制器通常會根據(jù)具體的應(yīng)用情況和系統(tǒng)要求，對三個項的比例、積分和微分常數(shù)進(jìn)行調(diào)整，
以達(dá)到最優(yōu)的控制效果。
代碼詳解：由于識別到的目標(biāo)不存在靜態(tài)誤差，因此這里僅僅進(jìn)行PD調(diào)節(jié)即可
步驟一、設(shè)置全局或者靜態(tài)變量，用于保存誤差信息，如下：
static float last_error_x_angle = 0;
static float last_error_distance = 0;
static float last_error_hgt = 0;
步驟二、設(shè)置局部變量，獲取實施的輸入信息和誤差數(shù)據(jù)信息
float x_angle; //實時角度
float distance; //實時距離
float hgt; //實時高度
float error_x_angle = x_angle - target_x_angle; //實時角度誤差
float error_distance = distance - target_distance; //實時距離誤差
float error_hgt = hgt - target_hgt; //實時高度誤差
步驟三、為了降低無人機(jī)跟隨的抖動，可以給一定的允許誤差閾值
//首先設(shè)置全局變量
//角度允許誤差，判定已經(jīng)滿足，不需要調(diào)整，將誤差置為0
if(error_x_angle > -x_angle_threshold && error_x_angle < x_angle_threshold)
{
error_x_angle = 0;
}
//距離允許誤差，判定已經(jīng)滿足，不需要調(diào)整，將誤差置為0
if(error_distance > -distance_threshold && error_distance < distance_threshold)
{
error_distance = 0;
}
//高度允許誤差，判定已經(jīng)滿足，不需要調(diào)整，將誤差置為0
if(error_hgt > -hgt_threshold && error_hgt < hgt_threshold)
{
error_hgt = 0;
}
步驟四、將當(dāng)前的差值送入PID控制器，得到輸出控制信息
注意：這里對輸出的控制信息進(jìn)行了限幅，是為了提高安全性，否則可能會因為PID參數(shù)給的不合適，導(dǎo)致無人機(jī)的快速運動，造成損失
//定義全局變量，對PID輸出的控制信息進(jìn)行限制
float max_velocity_z = 0.3;
float max_raw_velocity_x = 2.0;
float max_raw_yaw_rate = 1.0;
//距離跟隨控制
setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;
if(setpoint_raw.velocity.x < -max_raw_velocity_x)
{
setpoint_raw.velocity.x = -max_raw_velocity_x;
}
else if(setpoint_raw.velocity.x > max_raw_velocity_x)
{
setpoint_raw.velocity.x = max_raw_velocity_x;
}
//高度跟隨控制
setpoint_raw.velocity.z = error_hgt * linear_z_p/1000 + (error_hgt - last_error_hgt) * linear_z_d/1000;
if (setpoint_raw.velocity.z < -max_velocity_z)
{
setpoint_raw.velocity.z = -max_velocity_z;
}
else if (setpoint_raw.velocity.z > max_velocity_z)
{
setpoint_raw.velocity.z = max_velocity_z;
}
//角度跟隨控制
setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;
if(setpoint_raw.yaw_rate < -max_raw_yaw_rate)
{
setpoint_raw.yaw_rate = -max_raw_yaw_rate;
}
else if(setpoint_raw.yaw_rate > max_raw_yaw_rate)
{
setpoint_raw.yaw_rate = max_raw_yaw_rate;
}
if(fabs(setpoint_raw.yaw_rate) < 0.1)
{
setpoint_raw.yaw_rate = 0;
}
整體代碼如下：
void pid_control()
{
static float last_error_x_angle = 0;
static float last_error_distance = 0;
static float last_error_hgt = 0;
float x_angle;
float distance;
float hgt;
if(current_position_x == 0 && current_position_y == 0 && current_distance == 0)
{
flag_no_object = true;
x_angle = target_x_angle;
distance = target_distance;
hgt = target_hgt;
}
else
{
flag_no_object = false;
x_angle = current_position_x / current_distance;
distance = current_distance;
hgt = current_position_y;
}
float error_x_angle = x_angle - target_x_angle;
float error_distance = distance - target_distance;
float error_hgt = hgt - target_hgt;
//角度控制
if(error_x_angle > -x_angle_threshold && error_x_angle < x_angle_threshold)
{
error_x_angle = 0;
}
//距離控制
if(error_distance > -distance_threshold && error_distance < distance_threshold)
{
error_distance = 0;
}
//高度控制
if (error_hgt > -hgt_threshold && error_hgt < hgt_threshold)
{
error_hgt = 0;
}
printf("hgt = %f\r\n",hgt);
printf("error_hgt = %f\r\n",error_hgt);
printf("error_x_angle = %f\r\n",error_x_angle);
//距離跟隨控制
setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;
if(setpoint_raw.velocity.x < -max_raw_velocity_x)
{
setpoint_raw.velocity.x = -max_raw_velocity_x;
}
else if(setpoint_raw.velocity.x > max_raw_velocity_x)
{
setpoint_raw.velocity.x = max_raw_velocity_x;
}
//高度跟隨控制
setpoint_raw.velocity.z = error_hgt * linear_z_p/1000 + (error_hgt - last_error_hgt) * linear_z_d/1000;
if (setpoint_raw.velocity.z < -max_velocity_z)
{
setpoint_raw.velocity.z = -max_velocity_z;
}
else if (setpoint_raw.velocity.z > max_velocity_z)
{
setpoint_raw.velocity.z = max_velocity_z;
}
//角度跟隨控制
setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;
if(setpoint_raw.yaw_rate < -max_raw_yaw_rate)
{
setpoint_raw.yaw_rate = -max_raw_yaw_rate;
}
else if(setpoint_raw.yaw_rate > max_raw_yaw_rate)
{
setpoint_raw.yaw_rate = max_raw_yaw_rate;
}
if(fabs(setpoint_raw.yaw_rate) < 0.1)
{
setpoint_raw.yaw_rate = 0;
}
//沒檢測到目標(biāo)的時候，直接保持原地不動即可，后續(xù)可能會改稱旋轉(zhuǎn)尋找目標(biāo)
if(flag_no_object)
{
printf("no_object\r\n");
setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;
if(temp_flag_no_object)
{
temp_flag_no_object = false;
current_x_record = local_pos.pose.pose.position.x;
current_y_record = local_pos.pose.pose.position.y;
current_z_record = local_pos.pose.pose.position.z;
}
setpoint_raw.position.x = current_x_record;
setpoint_raw.position.y = current_y_record;
setpoint_raw.position.z = current_z_record;
setpoint_raw.coordinate_frame = 1;
}
else
{
printf("yes_object\r\n");
temp_flag_no_object = true;
if(fabs(error_hgt)<=hgt_threshold)
{
//hgt_threshold = temp_hgt_threshold + 300;
//distance_threshold = temp_distance_threshold;
if(temp_flag_hgt)
{
temp_flag_hgt = false;
current_z_record = local_pos.pose.pose.position.z;
}
//error_hgt大于等于0，表明此時無人機(jī)在目標(biāo)物的下方
if(error_hgt>0)
{
setpoint_raw.position.z = current_z_record + 0.4*hgt_threshold*0.001;
}
else
{
setpoint_raw.position.z = current_z_record - 0.4*hgt_threshold*0.001;
}
setpoint_raw.type_mask = 1 + 2 /*+ 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;
setpoint_raw.coordinate_frame = 8;
}
else
{
//hgt_threshold = temp_hgt_threshold;
//distance_threshold = temp_distance_threshold;
temp_flag_hgt = true;
setpoint_raw.type_mask = 1 + 2 + 4 /*+ 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 /*+ 2048*/;
setpoint_raw.coordinate_frame = 8;
}
//if(fabs(error_hgt)<=hgt_threshold)
//if(error_x_angle)
}
printf("error_hgt = %f\r\n",error_hgt);
printf("current_x_record = %f\r\n",current_x_record);
printf("current_y_record = %f\r\n",current_y_record);
printf("current_z_record = %f\r\n",current_z_record);
printf("setpoint_raw.position.x = %f\r\n",setpoint_raw.position.x);
printf("setpoint_raw.position.y = %f\r\n",setpoint_raw.position.y);
printf("setpoint_raw.position.z = %f\r\n",setpoint_raw.position.z);
mavros_setpoint_pos_pub.publish(setpoint_raw);
last_error_x_angle = error_x_angle;
last_error_distance = error_distance;
last_error_hgt = error_hgt;
}
文章來源：http://www.zghlxwxcb.cn/news/detail-811425.html
到了這里，關(guān)于超維空間S2無人機(jī)使用說明書——55、代碼詳解：基礎(chǔ)PID算法控制無人機(jī)的跟隨代碼詳解的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
