#前言
问题：本人在进行实车实验的时候，上一期的毫米波雷达筛选策略很容易受到周围车辆的影响，比如后面来了个电动车从左边车道超车并返回本车道时，仅仅依靠横向的简单筛选策略，很容易发生目标障碍物误检的情况，比如想要跟踪前方的车辆，前方车辆的横向距离为1m，远处的电动车的横向距离可能为0.5m，此时就出现了误检的情况，对于这种情况我们应该怎么避免？
图1
文章目录
一、在进行目标筛选前的数据处理工作
通过查阅大陆ARS408-21XX毫米波雷达文档资料可知，大陆ARS408-21XX毫米波雷达以object模式向用户输出其检测到的目标物的信息时，它是以0x60A为起始或结束的标志，随后所有检测到的目标物都以0x60B的ID输出给用户使用（而其他的ID比如0X60C啥的我暂时用不到），具体细节如图2所示：
图2
所以我们在对目标障碍物进行筛选之前，需要将上一轮检测到的所有障碍物都存进一个数组中，方便后期从该数组获取障碍物信息并进行筛选。想法是这么个想法，但是代码怎么写呢？首先，我们先定义一个结构体，用于存储每一条0x60B的can帧。然后定义一个vector向量（一个容器），用于存放上一周期radar检测到的所有目标物的0x60B的can帧。（即把一个检测周期内检测到的目标物都存储到vector向量中）。
自定义的数据类型和vector向量如下所示：
//首先自定义一个数据类型
struct frameQjj{
uint8_t data_0;
uint8_t data_1;
uint8_t data_2;
uint8_t data_3;
uint8_t data_4;
uint8_t data_5;
uint8_t data_6;
uint8_t data_7;
};
//然后定义一个数据类型为frameQjj的vector数组，
//用于存储上一周期毫米波发给用户的所有目标物
vector<frameQjj> Frame_buf_store;
二、筛选策略
2.1 大陆ARS408-21XX毫米波雷达为用户提供的初步筛选的选项
我们通过翻阅技术文档可知，我们可对该款毫米波雷达下发0X200的can帧，来对毫米波雷达进行配置（如图3所示）
图3
翻阅资料我们可知，0x200帧中的RadarCfg_SortIndex字段为我们提供了目标物的输出顺序，而我这边的配置是，让毫米波雷达先输出距离最近的目标物。
图4
配置帧0x200的具体参数的代码如下所示：
/*radar配置函数*/
bool radar_cfg(int & skt){
struct can_frame frame[2];
//radar配置使能0x200
radar_200.DATA0.bit.RadarCfg_MaxDistance_Valid = 1;//允许配置最大距离
radar_200.DATA0.bit.RadarCfg_SensorID_Valid = 1;//允许配置radarID
radar_200.DATA0.bit.RadarCfg_RadarPower_Valid = 1;//允许配置radar功率
radar_200.DATA0.bit.RadarCfg_OutputType_Valid = 1;//允许配置radar输出模式
radar_200.DATA0.bit.RadarCfg_SendQuality_Valid = 1;//允许配置radar输出cluster和object的质量信息
radar_200.DATA0.bit.RadarCfg_SendExtInfo_Valid = 1;//允许配置radar输出object的扩展信息
radar_200.DATA0.bit.RadarCfg_SortIndex_Valid = 1;//允许Object目标列表的当前排序索引值配置
radar_200.DATA0.bit.RadarCfg_StoreInNVM_Valid = 1;//使能；
//radar相关参数配置
//最大距离设置
radar_200.DATA12.bit.RadarCfg_MaxDistance = ((150 - 0)/2);//设定最大探测150m
radar_200.DATA12.bit.reserved =0;
//保留位
radar_200.DATA3.bit.reserved =0;
//设置radarID，输出类型，radar功率
radar_200.DATA4.bit.RadarCfg_sensorID = (0-0)/1;//设置该radar的ID：为0
radar_200.DATA4.bit.RadarCfg_OutputType =(1-0)/1;//object模式
radar_200.DATA4.bit.RadarCfg_RadarPower =(1-0)/1;//标准发射功率（参数具体含义请看用户手册）
//
radar_200.DATA5.bit.RadarCfg_CtrlRelay_Valid = 0;
radar_200.DATA5.bit.RadarCfg_CtrlRelay = 0;
radar_200.DATA5.bit.RadarCfg_SendQuality = 1;
radar_200.DATA5.bit.RadarCfg_SendExtInfo = 1;
//*******************按距离排序输出:经过测试，它是先输出近的目标在输出远的目标
radar_200.DATA5.bit.RadarCfg_SortIndex = 1;//1表示按距离输出。
radar_200.DATA5.bit.RadarCfg_StoreInNVM = 1;//使能；
//
radar_200.DATA6.bit.RadarCfg_RCS_threshold_Valid = 1;
radar_200.DATA6.bit.RadarCfg_RCS_threshold = 0;//标准灵敏度
radar_200.DATA6.bit.RadarCfg_InvalidClusters_Valid = 0;
radar_200.DATA6.bit.reserved = 0;
//
radar_200.DATA7.bit.RadarCfg_InvalidClusters = 0;
//radar基本属性配置
frame[0].can_id = 0x200;//这样赋值默认标准帧。如果为扩展帧，那么 frame.can_id = CAN_EFF_FLAG | 0x123;
frame[0].can_dlc = 8; //数据长度为 8个字节
frame[0].data[0] = radar_200.DATA0.D;
frame[0].data[2] = radar_200.DATA12.D;
frame[0].data[1] = radar_200.DATA12.D >> 8;
frame[0].data[3] = radar_200.DATA3.D;
frame[0].data[4] = radar_200.DATA4.D;
frame[0].data[5] = radar_200.DATA5.D;
frame[0].data[6] = radar_200.DATA6.D;
frame[0].data[7] = radar_200.DATA7.D;
//radar过滤器配置0x202----------
frame[1].can_id = 0x202;//这样赋值默认标准帧
frame[1].can_dlc = 8; //数据长度为 8个字节
//过滤器使能
radar_202.DATA0.bit.reserved = 0;
radar_202.DATA0.bit.FilterCfg_Valid = 1;//使能过滤器
radar_202.DATA0.bit.FilterCfg_Active = 1;//激活过滤器
radar_202.DATA0.bit.FilterCfg_Index = 0x9;//0x9表示以筛选y方向的距离作为筛选条件（参数具体含义请看用户手册）
radar_202.DATA0.bit.FilterCfg_Type = 1;//object过滤器
//横向最小距离设置
radar_202.DATA12.bit.FilterCfg_Min_X =(-1.8 + 409.5)/0.2;//车头右边1.8米以外的目标筛选掉
radar_202.DATA12.bit.reserved = 0;
//横向最大距离设置
radar_202.DATA34.bit.FilterCfg_Max_X =(1.8 + 409.5)/0.2;//车头左边1.8米以外的目标筛选掉
radar_202.DATA34.bit.reserved = 0;
//
radar_202.DATA5.bit.reserved = 0;
radar_202.DATA6.bit.reserved = 0;
radar_202.DATA7.bit.reserved = 0;
frame[1].data[0] = radar_202.DATA0.D;
frame[1].data[2] = radar_202.DATA12.D;
frame[1].data[1] = radar_202.DATA12.D >> 8;
frame[1].data[4] = radar_202.DATA34.D;
frame[1].data[3] = radar_202.DATA34.D >> 8;
frame[1].data[5] = radar_202.DATA5.D;
frame[1].data[6] = radar_202.DATA6.D;
frame[1].data[7] = radar_202.DATA7.D;
int nbytes = write(skt, &frame[0], sizeof(frame[0])); //发送数据,第三个参数表示：需要发送的字节数
if(nbytes != sizeof(frame[0])){ //如果 nbytes 不等于帧长度，就说明发送失败
printf("Error\n!");
return false;
}//配置失败，返回false
int nbytes2_ = write(skt, &frame[1], sizeof(frame[1])); //发送数据,第三个参数表示：需要发送的字节数
if(nbytes != sizeof(frame[1])){ //如果 nbytes 不等于帧长度，就说明发送失败
printf("Error\n!");
return false;
}//配置失败，返回false
return true;
}
2.2 筛选策略与想法（仅供参考）
从上文可知，我们可以把毫米波雷达上一个周期检测到的目标物信息都存入vector向量中。因此我们可以从vector向量中逐帧筛选出想要跟踪的目标物信息。OK，现在想法有了，怎么用代码来实现呢？我们可以把0X60A的ID号作为一个标志位，当我读取到0X60A的can帧的时候，意味着上一周期的0X60B已经输出完毕，也都存进vector向量中（当然我们还要面对的一个问题是当我第一次读取到0X60A的时候，0X60A之前是没有0X60B数据的，所以我们还要简单的做一个初始化，这将在代码中体现），接下来我们就要对vector向量中的0X60B进行筛选，因为vector向量中的第一帧是距离radar最近的目标物，以他作为备选的追踪目标，让接下来vector数组中的每一帧都与之作对比，若同时满足这两个条件：1.纵向距离比第一帧更小；2.横向距离比第一帧更小。那么，该帧为最优帧（即希望跟踪的目标）。然后循环如此。
具体的实现代码如下所示：
struct frameQjj{
uint8_t data_0;
uint8_t data_1;
uint8_t data_2;
uint8_t data_3;
uint8_t data_4;
uint8_t data_5;
uint8_t data_6;
uint8_t data_7;
};
//然后定义一个数据类型为frameQjj的vector数组，
//用于存储上一周期毫米波发给用户的所有目标物
vector<frameQjj> Frame_buf_store;//全局变量
int frame_number = 0;//全局变量
///*can解析函数*/
void Radar_can2value(can_frame & frame, ros::Publisher & pub, ros::Publisher & markerArrayPub){
vector<frameQjj> chongzhi;//用于刷新全局变量Frame_buf_store
frameQjj frame_buf;
int ii = 1;//$$$$$$$$$$$$$$$$$$$$$$此变量的位置别乱动
if (frame.can_id == 0x60b){//存can帧
// printf("(更新)开存---\n");
frame_buf.data_0 = frame.data[0];
frame_buf.data_1 = frame.data[1];
frame_buf.data_2 = frame.data[2];
frame_buf.data_3 = frame.data[3];
frame_buf.data_4 = frame.data[4];
frame_buf.data_5 = frame.data[5];
frame_buf.data_6 = frame.data[6];
frame_buf.data_7 = frame.data[7];
frame_number = frame_number + 1;
// printf("(更新)帧数量%d\n",frame_number);
Frame_buf_store.push_back(frame_buf);
}
else if(frame.can_id == 0x60a){//解析上一周期的can帧
int id ;
float lat_distance ;
float long_distance ;
float Vx;
float Vy;
double rcs;
if(init_jiexi == 1){//若第一次读到0x60a则直接return;往后再读到0x60a则解析存入vector的数据
init_jiexi = 0;
// printf("新的周期++++++\n");
return;
}
else {
//printf("解析了---\n");
//-----开始筛选
float lat_distance_temporary ;
float long_distance_temporary ;
while(ii <= frame_number){//筛选vector向量里面的can帧
radar_60b.DATA23.D = (Frame_buf_store.at(ii-1).data_2 << 8) + Frame_buf_store.at(ii-1).data_3;//y
lat_distance_temporary = radar_60b.DATA23.bit.obj_lat*0.2 - 204.6;
radar_60b.DATA12.D = (Frame_buf_store.at(ii-1).data_1 << 8) + Frame_buf_store.at(ii-1).data_2;
long_distance_temporary = radar_60b.DATA12.bit.obj_long*0.2 - 500;
// printf("long_distance_temporary=%f, lat_distance_temporary%f\n",long_distance_temporary, lat_distance_temporary);
if (ii == 1){//第一帧
//横向距离
lat_distance = lat_distance_temporary;
//纵向距离
long_distance = long_distance_temporary;
//横向速度
radar_60b.DATA45.D = (Frame_buf_store.at(ii-1).data_4 << 8) + Frame_buf_store.at(ii-1).data_5;
Vx = (radar_60b.DATA45.bit.obj_vlong)*0.25 - 128;
//纵向速度
radar_60b.DATA56.D = (Frame_buf_store.at(ii-1).data_5 << 8) + Frame_buf_store.at(ii-1).data_6;
Vy = radar_60b.DATA56.bit.obj_vlat*0.25 - 64;
ii = ii+1;//$$$$$$$$$$$$$$$$$$$$$$此变量的位置别乱动
}
else{//逐帧筛选出最优帧
if (abs(lat_distance_temporary) <= abs(lat_distance) && abs(long_distance_temporary) <= abs(long_distance)){//新一帧的横向距离的绝对值与旧一帧作比较 且 新一帧的纵向距离的绝对值与旧一帧作比较
//横向距离
lat_distance = lat_distance_temporary;
//纵向距离
long_distance = long_distance_temporary;
//横向速度
radar_60b.DATA45.D = (Frame_buf_store.at(ii-1).data_4 << 8) + Frame_buf_store.at(ii-1).data_5;
Vx = radar_60b.DATA45.bit.obj_vlong*0.25 - 128;
//纵向速度
radar_60b.DATA56.D = (Frame_buf_store.at(ii-1).data_5 << 8) + Frame_buf_store.at(ii-1).data_6;
Vy = radar_60b.DATA56.bit.obj_vlat*0.25 ;
// rcs = radar_60b.DATA7.bit.rcs*0.5 ;
}
ii = ii + 1;//$$$$$$$$$$$$$$$$$$$$$$此变量的位置别乱动
// printf("ii=%d\n",ii);
}
}// while(ii <= frame_number)
Frame_buf_store.swap(chongzhi);
frame_number = 0;//$$$$$$$$$$$$$$$$$$$$$$此变量的位置别乱动
//
sensor_data.sensorType = 1;//------1表示radar
sensor_data.obj_ID = id;
sensor_data.X = long_distance;
sensor_data.Y = lat_distance;
sensor_data.Vx = Vx;
sensor_data.Vy = Vy;
if(abs(sensor_data.Y) > 1.0){//没有数据时候就不下发数据；
return;
}
if(sensor_data.X < 0.09){
return;
}
//--------获取系统时间戳
uint64_t sys_time=std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
int32_t time_second = sys_time / 1000000;
int32_t time_nsecs = sys_time % 1000000 * 1000;
double timestamp_ = (double)time_second+1e-9*(double)time_nsecs;
sensor_data.timestamp = timestamp_;//单位为：s
int obj_ID = sensor_data.obj_ID;
visualization(sensor_data.X, sensor_data.Y, sensor_data.Vx, obj_ID, markerArrayPub);
//--------发往数据链队列
pub.publish(sensor_data);
printf("Topic(sensorRawData)----radar:目标ID%d------纵向距离%f-----横向距离%f-------纵向速度%f\n", sensor_data.obj_ID, sensor_data.X,sensor_data.Y, sensor_data.Vx);
//这么处理是为了让数据清零而已
sensor_data.sensorType = 0;//------1表示radar
sensor_data.obj_ID = 0;
sensor_data.X = 0.0;//相对纵向距离
sensor_data.Y = 0.0;
sensor_data.Vx = 0.0;//相对纵向速度
sensor_data.Vy = 0.0;
// printf("dabug=\n");
}
}//else if(frame.can_id == 0x60a)
return;
}
完整代码链接如下：代码传送门，仅供参考。
三、本期radar筛选策略的效果展示
程序的使用步骤，麻烦读者返回去阅读一下第一期的大陆毫米波雷达ARS408-21xx使用记录。因为当时是晚上11点了，只留下了这个照片和截图，后面的实验没有留下图片，现在懒得开车去外面测试，只在室内做了测试（啥时候闲得慌，再把效果图给替换上，不过这室内的杂物也挺多的，就当模拟路上的干扰了）。显然，离我们径向距离最近的目标是实验室刘某的性感原谅色无人叉车，然后车辆正前方是蓝色托盘，很显然，蓝色托盘的横向距离离我们最近，然而我们想要追踪的目标绿色小叉车，所以程序筛选出来给我们的应该是绿色小叉车，效果如下所示：
图5
图6
我这边拿了皮尺简单测量了一下，图5中的红色方块确实是叉车，说明筛选程序还是有点效果的。
总结
欢迎大家分享自己的一些建议，也欢迎大家分享自己的想法，共同进步，一起白嫖。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
