超维空间S2无人机使用说明书——55、代码详解：基础PID算法控制无人机的跟随代码详解

流浪者1015

已于 2024-01-02 14:11:16 修改

阅读量1.6k

点赞数 12

分类专栏：超维空间S2无人机使用说明文章标签：无人机算法 ROS c++

于 2023-12-30 12:26:38 首次发布

本文链接：https://blog.csdn.net/qq_35598561/article/details/135303142

版权

超维空间S2无人机使用说明专栏收录该内容

17 篇文章 11 订阅

订阅专栏

引言：PID算法作为运动控制的经典算法，有着不错的效果。目前PID算法也有了各种升级版，这里不涉及到串级PID和增量式PID控制，仅做最基础的PID控制，有需要的话，可以自行优化，或者联系我们一起开发。对于研发人员来讲，熟练掌握了PID算法的设计与实现过程，就可以应对一般的研发问题了。

PID概念

PID，即比例 Proportion、积分 Integral 和微分 Derivative 三个单词的缩写；比例积分微分控制，简称PID控制。

简单讲，根据给定值和实际输出值构成控制偏差，将偏差按比例、积分和微分通过线性组合构成控制量，对被控对象进行控制。

常规 PID 控制器作为一种线性控制器。

在这里插入图片描述

这里仅仅进行简单的介绍，具体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;            //实时高度误差

步骤三、为了降低无人机跟随的抖动，可以给一定的允许误差阈值

//首先设置全局变量
//角度允许误差，判定已经满足，不需要调整，将误差置为0
if(error_x_angle > -x_angle_threshold && error_x_angle < x_angle_threshold)  
{
	error_x_angle = 0;
}
//距离允许误差，判定已经满足，不需要调整，将误差置为0
if(error_distance > -distance_threshold && error_distance < distance_threshold) 
{
	error_distance = 0;
}
//高度允许误差，判定已经满足，不需要调整，将误差置为0
if(error_hgt > -hgt_threshold && error_hgt < hgt_threshold)
{
	error_hgt = 0;
}

步骤四、将当前的差值送入PID控制器，得到输出控制信息

注意：这里对输出的控制信息进行了限幅，是为了提高安全性，否则可能会因为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 = /*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，表明此时无人机在目标物的下方
				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;

}