您的当前位置：首页关于车路协同目标定位（含通过定位计算速度、加速度、航向角）

关于车路协同目标定位（含通过定位计算速度、加速度、航向角）

来源：好走旅游网

关于车路协同目标定位（含通过定位计算速度、加速度、航向角）

通过视觉标定图像的方法，获取像素对应经纬度。有一硬性条件为相机安装及相关属性不能随意变化，尤其涉及像素级别变化的相关属性。

1 视觉目标定位

1.1 方案原理

（1）需现场标定地点范围（区域）一圈的经纬度。行驶到区域内的车辆可计算出经纬度。

（2）相机及相机视野需保持固定不变。

（3）标定像素需与实际世界位置的经纬度匹配。

（4）标定的点需满足一定的规律。标定的点可与一个M * N的矩阵匹配。如标定的点可唯一的填充到一个M * N矩阵的第一行和第M行，第一列和第N列的各个元素。

（5）上述（4）相当于存在一个矩阵，其第一行和最后一行，第一列和最后一列值为像素及经纬度值，而第二行到第M-1行，第二列到第N-1列则需要通过插值计算。这里是考虑行列两个方向采用已有数据的中值插值方法。

（6）默认上述（5）中的插值次数为2。如果路口太难标定难度高，可以将插值次数酌情提高。

1.2 从算法端考虑方案测试方法

输入目标像素位置，通过assign_attribute()（目标的GPS属性赋值函数），即可对一帧图像各个目标的经纬度属性赋值。

涉及的量如下：

frame[k].box.left       //一帧图像上第k个目标框左上角x坐标（像素级）
frame[k].box.top        //... ...左上角y坐标（像素级）
frame[k].box.width      //... ...宽度（像素级）
frame[k].box.height     //... ...高度（像素级）
frame[k].pos.longitude  // 一帧图像上第k个目标的经度
frame[k].pos.latitude   // 纬度
frame[k].pos.altitude   // 海拔高度

assign_attribute()流程如下：

（1）遍历一帧图像上的所有目标；
（2）标定的数据源和图像源对应（适用于多源相机传感器状态）；
（3）取目标位置，并确认是否包含在标定区间，若不在则转到下一个目标；
（4）寻找与当前目标最近的标定点或插值的点，将最近点的经纬度值赋给目标经纬度属性；
（5）循环（2）--（4）。

1.2.1 mec系统输出结果测试

通过整个系统，对齐frame属性接口，通过外侧接口输出frame信息，查看相应属性。

1.2.2 源码进行测试

若在mec系统中，由于调用或输出等原因，没有输出结果的话，可以先用源码静态测试。

如下为使用源码测试需要注意的地方：

（1）如果目标在标定范围内，但其经纬度值是-1，可以试一下将traffic_detector.cpp文件中的矩阵插值次数参数const int extension_num = 2改为3试一下。

（2）静态测试需将标定的像素及其经纬度写到main.cpp中createdata(）函数中，依照示例格式依次写入像素及其经纬度。

（3）静态测试需将目标的信息写到creat_frame()函数中，由于静态测试是取点的形式，而实际mec获取的是目标框的信息来获取像素的。如如下代码，可见计算目标经纬度所使用的像素为目标框下边缘中心点位置y方向减2个像素。

obj_pos.x = frame[k].box.left + frame[k].box.width / 2;
obj_pos.y = frame[k].box.top + frame[k].box.height -2;

示例：

若想测试计算像素点（400,230）的经纬度，那么需要如下4个值经上面公式计算结果为（400，230）
b.left = 1000; b.top = 389; b.width = 300; b.height = 310;
那么，可设：b.left = 300; b.top = 200; b.width = 200; b.height = 32; 
这样满足 400=300+200/2；  230=200+32-2

（4）经过上述2、3即可运行代码进行测试了，但要看对应的经纬度值，在traffic_detector.cpp中的assign_attribute()函数结束即可将frame中所有目标的经纬度计算出来，在此函数内部添加相应打印即可实现。==>代码已添加且注释掉了，使用的时候取消注释就可以了。

2 视觉计算速度、加速度、航向角

此处计算目标速度、加速度、航向角准确度依赖上述视觉目标定位的准确度。

2.1 速度

速度 = 距离 / 时间

距离：通过两个点的经纬度带入公式计算距离

// Calculate the distance between two positions
float TrafficDetector::cal_distance(double lat1, double lon1, double lat2, double lon2)
{
	double lat1Deg = degree2radians(lat1);
	double lon1Deg = degree2radians(lon1);
	double lat2Deg = degree2radians(lat2);
	double lon2Deg = degree2radians(lon2);
	double COS = cos(lat1Deg)*cos(lat2Deg)*cos(lon1Deg - lon2Deg) + (sin(lat1Deg)*sin(lat2Deg));
	return earthR * acos(COS);   // 单位：米（m）
}

时间：通过目标前后位置对应时间戳或通过帧间隔与相机帧率得到。

2.2 加速度

设匀变速直线运动则： a = (vt-v0)/t 或 a = ▲s/t/t

使用方法：a = deta_s/t/t 法计算:（deta_s 为连续相邻相等时间的位移差）

for (int m = 0; m < frames[int(frames_count/2)].size(); ++m){
    if (frames.back()[j].tracker_id == frames[int(frames_count/2)][m].tracker_id){
        //cout<<"m: "<<m<<endl;
        float d1 = cal_distance(frames.front()[i].pos.latitude, frames.front()[i].pos.longitude, 
        frames[int(frames_count/2)][m].pos.latitude, frames[int(frames_count/2)][m].pos.longitude);
        float deta_s = distance - d1 - d1;
        frames.back()[j].acce = deta_s * 4 / time_start_end / time_start_end; //(m/s^2)
        //frames.back()[j].acce = frames.back()[j].acce * 36 * 360;  // (km/h/h)
						}
					}

2.3 航向角

通过两点的经纬度计算航向角。所得航向角为车辆行驶方向与正北方向的夹角。