引言: 在实际工程应用中,不会是仿真一样的理想情况,通常会存在各种干扰,为了降低干扰的影响,一般采用软件滤波的方式进行,条件允许的话,也可以直接采用硬件滤波的方式。需要指出的是,不同的干扰类型,一般采用不同的滤波算法,或者采用叠加的滤波算法进行抗干扰,以此提高系统的稳定性。
本次实验的背景是在进行图像识别的时候,较高的图像发布帧率会导致返回的目标位置数据出现丢失的情况。基于此,本文汉主要采取了递推滤波算法和均值滤波算法
原理:相当于“递推平均滤波法”。这种方法是,把连续N个采样值看成一个队列,队列的长度固定为N。每次采样到一个新数据放入队尾,并扔掉原来队首的一次数据(先进先出原则)。把队列中的N个数据先去掉一个最大值和一个最小值,然后计算N-2个数据的平均值。这里根据实际情况进行简单的修改,不是去掉最大值和最小值,首先确保是否可信,在基础上再进行滤波
数据分析:本次采样的数据,是获取目标物的位置距离信息,偶尔会出现目标丢失的情况,此时返回的数值是0,可以基于此判断此时是丢失了目标。
步骤一、每次获取N个数据,根据目前观测到的实际目标丢失情况,代码每次采样五个数据为一组
(1)、 返回数据分别是三维位置信息,为此设置3个数组,分别接收保存,并进行计数。为了简便代码,也可以设置二维数组。此处设置3个数组
double positon_x_table[5]={};
double positon_y_table[5]={};
double distance_table[5] ={};
int count_positon_x = 0;
int count_positon_y = 0;
int count_distance = 0;
(2)、每次回去最新数值,并且进行保存和计数,确保每次都是按照5个数据进行处理//此处将距离由单位米改称毫米,方便提高控制精度
positon_x_table[count_positon_x] = temp_current_position_x;
count_positon_x++;
if(count_positon_x>=5)
{
count_positon_x = 0;
}
positon_y_table[count_positon_y] = temp_current_position_y;
count_positon_y++;
if(count_positon_y>=5)
{
count_positon_y = 0;
}
distance_table[count_distance] = temp_current_distance;
count_distance++;
if(count_distance>=5)
{
count_distance = 0;
}
(3)、遍历数组,判断是否存在无效数组,并进行计数
double temp_positon_x_table[5]={};
double temp_positon_y_table[5]={};
double temp_distance_table[5] ={};
int temp_i = 0;
for(int i=0;i<=4;i++)
{
if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0)
{
count_target_lost++;
ROS_INFO("count_target_lost = %d",count_target_lost);
}
else
{
temp_positon_x_table[temp_i] = positon_x_table[i];
temp_positon_y_table[temp_i] = positon_y_table[i];
temp_distance_table[temp_i] = distance_table[i];
temp_i++;
}
}
if(count_target_lost>3)
{
current_position_x = 0;
current_position_y = 0;
current_distance = 0;
}
else
{
current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;
current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;
current_distance = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;
}
通过以上算法可以实现简单的滤波了,从实际无人机测试视频效果看,无人机的识别跟随效果有了明显改善,根本原因是系统基本不再出现无效数据,消除了数据毛刺给无人机控制带来的抖动
整体代码如下:
void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{
double temp_current_position_x;
double temp_current_position_y;
double temp_current_distance;
int count_target_lost = 0;
object_pos = *msg;
current_frame_id = object_pos.header.frame_id;
tf::quaternionMsgToTF(local_pos.pose.pose.orientation, quat);
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
temp_current_position_x = object_pos.point.x*(-1000);
temp_current_position_y = object_pos.point.y*(-1000);
temp_current_distance = object_pos.point.z*(1000)/cos(pitch);
positon_x_table[count_positon_x] = temp_current_position_x;
count_positon_x++;
if(count_positon_x>=5)
{
count_positon_x = 0;
}
positon_y_table[count_positon_y] = temp_current_position_y;
count_positon_y++;
if(count_positon_y>=5)
{
count_positon_y = 0;
}
distance_table[count_distance] = temp_current_distance;
count_distance++;
if(count_distance>=5)
{
count_distance = 0;
}
double temp_positon_x_table[5]={};
double temp_positon_y_table[5]={};
double temp_distance_table[5] ={};
int temp_i = 0;
for(int i=0;i<=4;i++)
{
if(positon_x_table[i]==0 && positon_y_table[i]==0 && distance_table[i]==0)
{
count_target_lost++;
ROS_INFO("count_target_lost = %d",count_target_lost);
}
else
{
temp_positon_x_table[temp_i] = positon_x_table[i];
temp_positon_y_table[temp_i] = positon_y_table[i];
temp_distance_table[temp_i] = distance_table[i];
temp_i++;
}
}
if(count_target_lost>3)
{
current_position_x = 0;
current_position_y = 0;
current_distance = 0;
}
else
{
current_position_x = (temp_positon_x_table[0]+temp_positon_x_table[1]+temp_positon_x_table[2])/3;
current_position_y = (temp_positon_y_table[0]+temp_positon_y_table[1]+temp_positon_y_table[2])/3;
current_distance = (temp_distance_table[0] +temp_distance_table[1] +temp_distance_table[2])/3;
}
if(tmp_flag_frame == 1)
{
pid_control();
}
}