Compartilhamento de tecnologia

Posicionamento de UAV de alvos móveis no solo --- obtenção da direção e velocidade do movimento do alvo

2024-07-12

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

Índice

1. Introdução

Usamos um drone monocular para localizar um alvo em movimento no solo, tirando fotos em intervalos de tempo iguais. Atualmente, obtivemos as coordenadas tridimensionais do alvo em cada foto e sabemos a localização do drone tirando fotos durante o voo. . intervalo de tempo, então podemos obter a direção e velocidade do movimento do alvo por meio de certos cálculos.

2. Explicação do código

1. Os dados importados são um arquivo txt composto pelo nome da foto e os dados tridimensionais do ponto alvo, portanto, precisamos estabelecer uma função de segmentação de string para obter os dados no arquivo txt.

2. Defina o valor pi

3. Defina a função de cálculo de direção

4. Extraia as informações necessárias do arquivo txt

5. Calcule o ângulo de mudança de direção do alvo entre fotos adjacentes

6. Calcule a distância móvel e a velocidade do alvo entre fotos adjacentes

3. Exibição completa do código

4. Exibição de resultados

Todos os códigos deste artigo são fornecidos pelo usuário CSDN CV-X.WANG. Qualquer indivíduo ou grupo não está autorizado a realizar atividades comerciais e de ensino. Qualquer cotação ou cotação parcial deve obter autorização.

1. Introdução

        Usamos um drone monocular para localizar um alvo em movimento no solo, tirando fotos em intervalos de tempo iguais. Atualmente, obtivemos as coordenadas tridimensionais do alvo em cada foto e sabemos a localização do drone tirando fotos durante o voo. . intervalo de tempo, então podemos obter a direção e velocidade do movimento do alvo por meio de certos cálculos.

2. Explicação do código

1. Os dados importados são um arquivo txt composto pelo nome da foto e os dados tridimensionais do ponto alvo, portanto, precisamos estabelecer uma função de segmentação de string para obter os dados no arquivo txt.

  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. Defina o valor pi

#define M_PI       3.14159265358979323846   // pi

3. Defina a função de cálculo de direção

Para obter a direção do movimento do alvo na direção plana, este artigo adota o método da direção 360°, comum no campo militar. Ou seja, o norte verdadeiro é a direção de 0° e o sentido horário é de 0-360°. Por exemplo, a verdadeira direção leste: em nosso sistema de direção, é a direção de 90°.

alguns,

duplo lon1_rad = lon1 * M_PI / 180,0;
duplo lat1_rad = lat1 * M_PI / 180,0;
duplo lon2_rad = lon2 * M_PI / 180,0;
duplo lat2_rad = lat2 * M_PI / 180,0;

Está em radianos.

  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. Extraia as informações necessárias do arquivo 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. Calcule o ângulo de mudança de direção do alvo entre fotos adjacentes

  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. Calcule a distância móvel e a velocidade do alvo entre fotos adjacentes

Observação: a fórmula de cálculo para a distância que chegamos aqui é:

Esta é apenas a demonstração mais simples. Em situações reais, precisamos considerar uma série de condições, como o sistema de coordenadas, a localização da área de medição, etc., para obter uma distância mais precisa.

  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. Exibição completa do código

  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. Exibição de resultados

Todos os códigos neste artigo são fornecidos pelo usuário CSDN CV-X.WANG, não é permitida a realização de atividades comerciais e docentes a qualquer indivíduo ou grupo, devendo qualquer cotação ou cotação parcial obter autorização.

Perfil pessoal

Ele se dedica à pesquisa de tecnologia há mais de 30 anos e é proficiente em diversas linguagens como java, linux, javascript, php, css, etc. estação de documentação do desenvolvedor para compartilhar alguns problemas no desenvolvimento de tecnologia para referência futura.

minhas informações de contato

Correspondência