引言:PID算法作为运动控制的经典算法,有着不错的效果。目前PID算法也有了各种升级版,这里不涉及到串级PID和增量式PID控制,仅做最基础的PID控制,有需要的话,可以自行优化,或者联系我们一起开发。对于研发人员来讲,熟练掌握了PID算法的设计与实现过程,就可以应对一般的研发问题了。
PID概念
PID,即比例 Proportion、积分 Integral 和微分 Derivative 三个单词的缩写;比例积分微分控制,简称PID控制。
简单讲,根据给定值和实际输出值构成控制偏差,将偏差按比例、积分和微分通过线性组合构成控制量,对被控对象进行控制。
常规 PID 控制器作为一种线性控制器。
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/48db107aabcf250627308dbfc979e3b8.png)
这里仅仅进行简单的介绍,具体PID原理可以自行参考其他相关教程,本文仅从代码进行介绍
PID控制原理
PID控制的基本原理是通过测量控制系统的实际输出与期望输出之间的误差,
并根据比例、积分和微分三个参数计算控制量,调整系统的输入,使输出尽可能接近期望值。
具体来说,PID控制器的三个控制项分别是比例项(P项)、积分项(I项)和微分项(D项)。
比例项(P项):通过将误差与一个比例常数相乘,得到一个控制量,该控制量与误差成正比。
P项的作用是使控制量与误差呈线性关系,比例常数的大小决定了系统的响应速度和稳定性。
积分项(I项):通过将误差与一个积分常数相乘,得到一个控制量,该控制量与误差积分之和成正比。
I项的作用是消除误差的持久性,防止系统处于稳态误差状态。
微分项(D项):通过将误差与一个微分常数相乘,得到一个控制量,该控制量与误差的变化率成正比。
D项的作用是消除误差的瞬时变化,增强系统的稳定性和控制精度。
三个项的综合作用,可以根据误差大小、持久性和变化率,快速、准确地调节系统的输入,
使输出值稳定地达到期望状态。
PID控制器通常会根据具体的应用情况和系统要求,对三个项的比例、积分和微分常数进行调整,
以达到最优的控制效果。
代码详解:由于识别到的目标不存在静态误差,因此这里仅仅进行PD调节即可
步骤一、设置全局或者静态变量,用于保存误差信息,如下:
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;
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;
}
步骤四、将当前的差值送入PID控制器,得到输出控制信息
注意:这里对输出的控制信息进行了限幅,是为了提高安全性,否则可能会因为PID参数给的不合适,导致无人机的快速运动,造成损失
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;
}
if(flag_no_object)
{
printf("no_object\r\n");
setpoint_raw.type_mask = + 64 + 128 + 256 + 512 + 1024 ;
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)
{
if(temp_flag_hgt)
{
temp_flag_hgt = false;
current_z_record = local_pos.pose.pose.position.z;
}
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 + 64 + 128 + 256 + 512 + 1024 ;
setpoint_raw.coordinate_frame = 8;
}
else
{
temp_flag_hgt = true;
setpoint_raw.type_mask = 1 + 2 + 4 + 64 + 128 + 256 + 512 + 1024 ;
setpoint_raw.coordinate_frame = 8;
}
}
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;
}