Обмен технологиями

Позиционирование БПЛА наземных движущихся целей --- определение направления и скорости движения цели.

2024-07-12

한어Русский языкEnglishFrançaisIndonesianSanskrit日本語DeutschPortuguêsΕλληνικάespañolItalianoSuomalainenLatina

Оглавление

1. Введение

Мы используем монокулярный дрон для обнаружения движущейся цели на земле, делая фотографии через равные промежутки времени. В настоящее время мы получили трехмерные координаты цели на каждой фотографии и знаем местоположение дрона, делающего фотографии во время полета. . Интервал времени, то с помощью определенных вычислений мы можем получить направление и скорость движения цели.

2. Объяснение кода

1. Импортированные данные представляют собой текстовый файл, состоящий из названия фотографии и трехмерных данных целевой точки, поэтому нам необходимо установить функцию сегментации строк для получения данных в текстовом файле.

2. Определите значение числа Пи

3. Определите функцию расчета направления.

4. Извлеките необходимую информацию из txt-файла.

5. Рассчитайте угол изменения направления цели между соседними фотографиями.

6. Рассчитайте расстояние и скорость перемещения цели между соседними фотографиями.

3. Полное отображение кода

4. Отображение результатов

Все коды в этой статье предоставлены пользователем CSDN CV-X.WANG. Любому лицу или группе не разрешается вести коммерческую и преподавательскую деятельность. Любое ценовое или частичное ценовое предложение должно получить разрешение.

1. Введение

        Мы используем монокулярный дрон для обнаружения движущейся цели на земле, делая фотографии через равные промежутки времени. В настоящее время мы получили трехмерные координаты цели на каждой фотографии и знаем местоположение дрона, делающего фотографии во время полета. . Интервал времени, то с помощью определенных вычислений мы можем получить направление и скорость движения цели.

2. Объяснение кода

1. Импортированные данные представляют собой текстовый файл, состоящий из названия фотографии и трехмерных данных целевой точки, поэтому нам необходимо установить функцию сегментации строк для получения данных в текстовом файле.

  1. //字符串分割
  2. vector<string> split(const string &s, char delimiter) {
  3. 	vector<string> tokens;
  4. 	string token;
  5. 	istringstream tokenStream(s);
  6. 	while (getline(tokenStream, token, delimiter)) {
  7. 		tokens.push_back(token);
  8. 	}
  9. 	return tokens;
  10. }

2. Определите значение числа Пи

#define M_PI       3.14159265358979323846   // pi

3. Определите функцию расчета направления.

Чтобы получить направление движения цели в направлении плоскости, в этой статье используется метод направления на 360 °, который распространен в военной области. То есть истинный север — это направление 0°, а направление по часовой стрелке — 0–360°. Например, истинное направление на восток: в нашей системе направлений это направление 90°.

некоторые из,

двойной lon1_rad = lon1 * M_PI / 180,0;
двойной lat1_rad = lat1 * M_PI / 180,0;
двойной lon2_rad = lon2 * M_PI / 180,0;
двойной lat2_rad = lat2 * M_PI / 180,0;

Это в радианах.

  1. //方向函数
  2. double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
  3. 	// Convert degrees to radians
  4. 	double lon1_rad = lon1 * M_PI / 180.0;
  5. 	double lat1_rad = lat1 * M_PI / 180.0;
  6. 	double lon2_rad = lon2 * M_PI / 180.0;
  7. 	double lat2_rad = lat2 * M_PI / 180.0;
  8.  
  9. 	// Calculate delta longitude and convert to radians
  10. 	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;
  11.  
  12. 	// Calculate y and x components
  13. 	double y = sin(delta_lon_rad) * cos(lat2_rad);
  14. 	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);
  15.  
  16. 	// Calculate direction angle in radians
  17. 	double direction_rad = atan2(y, x);
  18.  
  19. 	// Convert direction angle to degrees
  20. 	double direction_deg = direction_rad * 180.0 / M_PI;
  21.  
  22. 	// Ensure direction angle is within [0, 360) degrees
  23. 	if (direction_deg < 0) {
  24. 		direction_deg += 360.0;
  25. 	}
  26.  
  27. 	return direction_deg;
  28. }

4. Извлеките необходимую информацию из txt-файла.

  1.  
  2. 	ifstream file("LBH.txt");
  3. 	if (!file.is_open()) {
  4. 		cerr << "Could not open the file!" << endl;
  5. 		return 1;
  6. 	}
  7.  
  8. 	string line;
  9. 	// Skip the header line
  10. 	getline(file, line);
  11.  
  12. 	vector<vector<string>> extractedData;
  13.  
  14. 	// Read each line from the file
  15. 	while (getline(file, line)) {
  16. 		vector<string> columns = split(line, 't');
  17. 		if (columns.size() < 16) {
  18. 			cerr << "Invalid line format" << endl;
  19. 			continue;
  20. 		}
  21.  
  22. 		// Extract the required columns: 0, 13, 14, 15
  23. 		vector<string> extractedColumns;
  24. 		extractedColumns.push_back(columns[0]);  // Image Name
  25. 		extractedColumns.push_back(columns[13]); // Longitude
  26. 		extractedColumns.push_back(columns[14]); // Latitude
  27. 		extractedColumns.push_back(columns[15]); // Altitude
  28.  
  29. 		extractedData.push_back(extractedColumns);
  30. 	}
  31.  
  32. 	file.close();

5. Рассчитайте угол изменения направления цели между соседними фотографиями.

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

6. Рассчитайте расстояние и скорость перемещения цели между соседними фотографиями.

Обратите внимание: формула расчета расстояния, которое мы получаем, выглядит следующим образом:

Это всего лишь самая простая демонстрация. В реальных ситуациях нам необходимо учитывать ряд условий, таких как система координат, местоположение области измерения и т. д., чтобы получить более точное расстояние.

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

3. Полное отображение кода

  1. #include <iostream>
  2. #include <fstream>
  3. #include <sstream>
  4. #include <vector>
  5. #include <cmath>
  6.  
  7. using namespace std;
  8. #define M_PI       3.14159265358979323846   // pi
  9. // Function to split a string by a delimiter
  10. vector<string> split(const string &s, char delimiter) {
  11. 	vector<string> tokens;
  12. 	string token;
  13. 	istringstream tokenStream(s);
  14. 	while (getline(tokenStream, token, delimiter)) {
  15. 		tokens.push_back(token);
  16. 	}
  17. 	return tokens;
  18. }
  19.  
  20. //  direction angle in degrees
  21. //原理是 在平面上以正北方向为0°方向，顺时针为0-360°
  22. double calculateDirectionAngle(double lon1, double lat1, double lon2, double lat2) {
  23. 	// Convert degrees to radians
  24. 	double lon1_rad = lon1 * M_PI / 180.0;
  25. 	double lat1_rad = lat1 * M_PI / 180.0;
  26. 	double lon2_rad = lon2 * M_PI / 180.0;
  27. 	double lat2_rad = lat2 * M_PI / 180.0;
  28.  
  29. 	// Calculate delta longitude and convert to radians
  30. 	double delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;
  31.  
  32. 	// Calculate y and x components
  33. 	double y = sin(delta_lon_rad) * cos(lat2_rad);
  34. 	double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(delta_lon_rad);
  35.  
  36. 	// Calculate direction angle in radians
  37. 	double direction_rad = atan2(y, x);
  38.  
  39. 	// Convert direction angle to degrees
  40. 	double direction_deg = direction_rad * 180.0 / M_PI;
  41.  
  42. 	// Ensure direction angle is within [0, 360) degrees
  43. 	if (direction_deg < 0) {
  44. 		direction_deg += 360.0;
  45. 	}
  46.  
  47. 	return direction_deg;
  48. }
  49.  
  50. int main() {
  51. 	ifstream file("LBH.txt");
  52. 	if (!file.is_open()) {
  53. 		cerr << "Could not open the file!" << endl;
  54. 		return 1;
  55. 	}
  56.  
  57. 	string line;
  58. 	// Skip the header line
  59. 	getline(file, line);
  60.  
  61. 	vector<vector<string>> extractedData;
  62.  
  63. 	// Read each line from the file
  64. 	while (getline(file, line)) {
  65. 		vector<string> columns = split(line, 't');
  66. 		if (columns.size() < 16) {
  67. 			cerr << "Invalid line format" << endl;
  68. 			continue;
  69. 		}
  70.  
  71. 		// Extract the required columns: 0, 13, 14, 15
  72. 		vector<string> extractedColumns;
  73. 		extractedColumns.push_back(columns[0]);  // Image Name
  74. 		extractedColumns.push_back(columns[13]); // Longitude
  75. 		extractedColumns.push_back(columns[14]); // Latitude
  76. 		extractedColumns.push_back(columns[15]); // Altitude
  77.  
  78. 		extractedData.push_back(extractedColumns);
  79. 	}
  80.  
  81. 	file.close();
  82. 	
  83. 	// Calculate direction angles between adjacent image centers
  84. 	cout << "Direction angles between adjacent image centers:" << endl;
  85. 	for (size_t i = 1; i < extractedData.size(); ++i) {
  86. 		//三角函数计算用弧度制
  87. 		double lon1 = (stod(extractedData[i - 1][1]))* M_PI/180; // Longitude 
  88. 		double lat1 = (stod(extractedData[i - 1][2]))* M_PI / 180; // Latitude 
  89. 		double lon2 = (stod(extractedData[i][1]))* M_PI / 180;   // Longitude 
  90. 		double lat2 = (stod(extractedData[i][2]))* M_PI / 180;   // Latitude 
  91. 		//计算方向变化角也要用弧度制
  92. 		double direction_angle = calculateDirectionAngle(lon1, lat1, lon2, lat2);
  93. 		cout << "lon1=" << lon1 << endl << "lat1=" << lat1 << endl << "lon2=" << lon2 << endl << "lat2=" << lat2 << endl;
  94. 		// Output Direction
  95. 		cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << direction_angle << " degrees" << endl;
  96.  
  97. 		double lon2_1 = lon2 - lon1;
  98. 		double lat2_1 = lat2 - lat1;
  99. 		double lon_ = lon2_1 / 2;//1/2的Δlon
  100. 		double lat_ = lat2_1 / 2; //1 / 2的Δlat
  101. 		double sin2lon_ = sin(lon_)*sin(lon_);//sin²（1/2Δlon）
  102. 		double sin2lat_ = sin(lat_)*sin(lat_); //sin²（1 / 2Δlat）
  103. 		double cos_lat1 = cos(lat1);
  104. 		double cos_lat2 = cos(lat2);
  105. 		double sqrtA = sqrt(sin2lat_+ cos_lat1* cos_lat2*sin2lon_);
  106. 		//cout << "Direction from " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "sqrtA =" << sqrtA << endl;
  107. 		double asinA = asin(sqrtA);
  108. 		//长半轴 短半轴  单位是m
  109. 		int a_r = 6378137.0;
  110. 		int b_r = 6356752;
  111. 		double Earth_R = (2 * a_r + b_r) / 3;
  112. 		double Distance = 2 * Earth_R*asinA;
  113. 		cout << "Distance From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << Distance <<" meter"<< endl;
  114. 		int time = 3;//拍照间隔 s
  115. 		double speed = Distance / time;
  116. 		cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;
  117. 	}
  118. 	//cin.get();
  119. 	
  120. 	return 0;
  121. }

4. Отображение результатов

Все коды в этой статье предоставлены пользователем CSDN CV-X.WANG., любому лицу или группе не разрешается осуществлять коммерческую и преподавательскую деятельность, и любое предложение или частичное предложение должно быть одобрено.

Личный профиль

Он посвятил себя исследованию технологий более 30 лет и владеет различными языками, такими как Java, Linux, Javascript, php, css и т. д. Он внес большой вклад в область открытого исходного кода. Станция документации для разработчиков, где можно поделиться некоторыми проблемами в разработке технологий для дальнейшего использования. Все ознакомьтесь.

моя контактная информация

Почта