无人机对地面运动目标定位---获取目标的移动方向和速度

一、引子

我们利用单目无人机通过等时间间隔拍照的形式对地面某移动目标进行定位，当前，我们已经获得了每张相片上该目标的三维坐标，并且知道该无人机在飞行过程中拍照的时间间隔，那么我们就可以通过一定的计算，得到目标的运动方向和运动速度。

二、代码解释

1.导入的数据是由相片名称及目标点的三维数据构成的txt文件，所以我们需要建立字符串分割函数，获取txt文件内的数据。

2.定义圆周率数值

3.定义方向计算函数

4.从txt文件提取所需信息

5.计算目标在相邻相片间的方向变化角

6.计算目标在相邻相片间的移动距离及运动速度

三、完整代码展示

四、结果展示

本文所有代码均由CSDN用户CV-X.WANG提供，任何个人或者团体，不得进行商用和教学活动，引用或部分引用，均需获得授权。

一、引子

我们利用单目无人机通过等时间间隔拍照的形式对地面某移动目标进行定位，当前，我们已经获得了每张相片上该目标的三维坐标，并且知道该无人机在飞行过程中拍照的时间间隔，那么我们就可以通过一定的计算，得到目标的运动方向和运动速度。

二、代码解释

1.导入的数据是由相片名称及目标点的三维数据构成的txt文件，所以我们需要建立字符串分割函数，获取txt文件内的数据。

//字符串分割
vector<string> split(const string &s, char delimiter) {
	vector<string> tokens;
	string token;
	istringstream tokenStream(s);
	while (getline(tokenStream, token, delimiter)) {
		tokens.push_back(token);
	}
	return tokens;
}

2.定义圆周率数值

#define M_PI       3.14159265358979323846   // pi

3.定义方向计算函数

为获取目标在平面方向的移动方向，本文采用了在军事领域常见的360°方向法。即以正北为0°方向，顺时针方向为0-360°，比如说，正东方向：在我们的方向系统中即为90°方向。

这其中，

double lon1_rad = lon1 * M_PI / 180.0;
double lat1_rad = lat1 * M_PI / 180.0;
double lon2_rad = lon2 * M_PI / 180.0;
double lat2_rad = lat2 * M_PI / 180.0;

为弧度制。

//方向函数
double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
	// Convert degrees to radians
	double lon1_rad = lon1 * M_PI / 180.0;
	double lat1_rad = lat1 * M_PI / 180.0;
	double lon2_rad = lon2 * M_PI / 180.0;
	double lat2_rad = lat2 * M_PI / 180.0;

	// Calculate delta longitude and convert to radians
	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;

	// Calculate y and x components
	double y = sin(delta_lon_rad) * cos(lat2_rad);
	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);

	// Calculate direction angle in radians
	double direction_rad = atan2(y, x);

	// Convert direction angle to degrees
	double direction_deg = direction_rad * 180.0 / M_PI;

	// Ensure direction angle is within [0, 360) degrees
	if (direction_deg < 0) {
		direction_deg += 360.0;
	}

	return direction_deg;
}

4.从txt文件提取所需信息


	ifstream file("LBH.txt");
	if (!file.is_open()) {
		cerr << "Could not open the file!" << endl;
		return 1;
	}

	string line;
	// Skip the header line
	getline(file, line);

	vector<vector<string>> extractedData;

	// Read each line from the file
	while (getline(file, line)) {
		vector<string> columns = split(line, '\t');
		if (columns.size() < 16) {
			cerr << "Invalid line format" << endl;
			continue;
		}

		// Extract the required columns: 0, 13, 14, 15
		vector<string> extractedColumns;
		extractedColumns.push_back(columns[0]);  // Image Name
		extractedColumns.push_back(columns[13]); // Longitude
		extractedColumns.push_back(columns[14]); // Latitude
		extractedColumns.push_back(columns[15]); // Altitude

		extractedData.push_back(extractedColumns);
	}

	file.close();

5.计算目标在相邻相片间的方向变化角

cout << "Direction angles between adjacent image centers:" << endl;
	for (size_t i = 1; i < extractedData.size(); ++i) {
		//三角函数计算用弧度制
		double lon1 = (stod(extractedData[i - 1][1]))* M_PI/180; // Longitude 
		double lat1 = (stod(extractedData[i - 1][2]))* M_PI / 180; // Latitude 
		double lon2 = (stod(extractedData[i][1]))* M_PI / 180;   // Longitude 
		double lat2 = (stod(extractedData[i][2]))* M_PI / 180;   // Latitude 
		//计算方向变化角也要用弧度制
		double direction_angle = calculateDirectionAngle(lon1, lat1, lon2, lat2);
		cout << "lon1=" << lon1 << endl << "lat1=" << lat1 << endl << "lon2=" << lon2 << endl << "lat2=" << lat2 << endl;
		// Output Direction
		cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << direction_angle << " degrees" << endl;

6.计算目标在相邻相片间的移动距离及运动速度

请注意：此处我们获得距离的计算式为：

这只是最简单的一个演示，实际情况下，我们需要考虑坐标系统、测区位置等等一系列的条件，从而获得更为精准的Distance。

double lon2_1 = lon2 - lon1;
		double lat2_1 = lat2 - lat1;
		double lon_ = lon2_1 / 2;//1/2的Δlon
		double lat_ = lat2_1 / 2; //1 / 2的Δlat
		double sin2lon_ = sin(lon_)*sin(lon_);//sin²（1/2Δlon）
		double sin2lat_ = sin(lat_)*sin(lat_); //sin²（1 / 2Δlat）
		double cos_lat1 = cos(lat1);
		double cos_lat2 = cos(lat2);
		double sqrtA = sqrt(sin2lat_+ cos_lat1* cos_lat2*sin2lon_);
		//cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "sqrtA =" << sqrtA << endl;
		double asinA = asin(sqrtA);
		//长半轴 短半轴  单位是m
		int a_r = 6378137.0;
		int b_r = 6356752;
		double Earth_R = (2 * a_r + b_r) / 3;
		double Distance = 2 * Earth_R*asinA;
		cout << "Distance From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << Distance <<" meter"<< endl;
		int time = 3;//拍照间隔 s
		double speed = Distance / time;
		cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;
	}

三、完整代码展示

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <cmath>

using namespace std;
#define M_PI       3.14159265358979323846   // pi
// Function to split a string by a delimiter
vector<string> split(const string &s, char delimiter) {
	vector<string> tokens;
	string token;
	istringstream tokenStream(s);
	while (getline(tokenStream, token, delimiter)) {
		tokens.push_back(token);
	}
	return tokens;
}

//  direction angle in degrees
//原理是 在平面上以正北方向为0°方向，顺时针为0-360°
double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
	// Convert degrees to radians
	double lon1_rad = lon1 * M_PI / 180.0;
	double lat1_rad = lat1 * M_PI / 180.0;
	double lon2_rad = lon2 * M_PI / 180.0;
	double lat2_rad = lat2 * M_PI / 180.0;

	// Calculate delta longitude and convert to radians
	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;

	// Calculate y and x components
	double y = sin(delta_lon_rad) * cos(lat2_rad);
	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);

	// Calculate direction angle in radians
	double direction_rad = atan2(y, x);

	// Convert direction angle to degrees
	double direction_deg = direction_rad * 180.0 / M_PI;

	// Ensure direction angle is within [0, 360) degrees
	if (direction_deg < 0) {
		direction_deg += 360.0;
	}

	return direction_deg;
}

int main() {
	ifstream file("LBH.txt");
	if (!file.is_open()) {
		cerr << "Could not open the file!" << endl;
		return 1;
	}

	string line;
	// Skip the header line
	getline(file, line);

	vector<vector<string>> extractedData;

	// Read each line from the file
	while (getline(file, line)) {
		vector<string> columns = split(line, '\t');
		if (columns.size() < 16) {
			cerr << "Invalid line format" << endl;
			continue;
		}

		// Extract the required columns: 0, 13, 14, 15
		vector<string> extractedColumns;
		extractedColumns.push_back(columns[0]);  // Image Name
		extractedColumns.push_back(columns[13]); // Longitude
		extractedColumns.push_back(columns[14]); // Latitude
		extractedColumns.push_back(columns[15]); // Altitude

		extractedData.push_back(extractedColumns);
	}

	file.close();
	
	// Calculate direction angles between adjacent image centers
	cout << "Direction angles between adjacent image centers:" << endl;
	for (size_t i = 1; i < extractedData.size(); ++i) {
		//三角函数计算用弧度制
		double lon1 = (stod(extractedData[i - 1][1]))* M_PI/180; // Longitude 
		double lat1 = (stod(extractedData[i - 1][2]))* M_PI / 180; // Latitude 
		double lon2 = (stod(extractedData[i][1]))* M_PI / 180;   // Longitude 
		double lat2 = (stod(extractedData[i][2]))* M_PI / 180;   // Latitude 
		//计算方向变化角也要用弧度制
		double direction_angle = calculateDirectionAngle(lon1, lat1, lon2, lat2);
		cout << "lon1=" << lon1 << endl << "lat1=" << lat1 << endl << "lon2=" << lon2 << endl << "lat2=" << lat2 << endl;
		// Output Direction
		cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << direction_angle << " degrees" << endl;

		double lon2_1 = lon2 - lon1;
		double lat2_1 = lat2 - lat1;
		double lon_ = lon2_1 / 2;//1/2的Δlon
		double lat_ = lat2_1 / 2; //1 / 2的Δlat
		double sin2lon_ = sin(lon_)*sin(lon_);//sin²（1/2Δlon）
		double sin2lat_ = sin(lat_)*sin(lat_); //sin²（1 / 2Δlat）
		double cos_lat1 = cos(lat1);
		double cos_lat2 = cos(lat2);
		double sqrtA = sqrt(sin2lat_+ cos_lat1* cos_lat2*sin2lon_);
		//cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "sqrtA =" << sqrtA << endl;
		double asinA = asin(sqrtA);
		//长半轴 短半轴  单位是m
		int a_r = 6378137.0;
		int b_r = 6356752;
		double Earth_R = (2 * a_r + b_r) / 3;
		double Distance = 2 * Earth_R*asinA;
		cout << "Distance From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << Distance <<" meter"<< endl;
		int time = 3;//拍照间隔 s
		double speed = Distance / time;
		cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;
	}
	//cin.get();
	
	return 0;
}