Condivisione della tecnologia

Posizionamento UAV di bersagli mobili a terra: acquisizione della direzione di movimento e della velocità del bersaglio

2024-07-12

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

Sommario

1. Introduzione

Utilizziamo un drone monoculare per localizzare un bersaglio in movimento sul terreno scattando foto a intervalli di tempo uguali. Attualmente, abbiamo ottenuto le coordinate tridimensionali del bersaglio in ciascuna foto e conosciamo la posizione del drone che scatta foto durante il volo. intervallo di tempo, quindi possiamo ottenere la direzione e la velocità del movimento del bersaglio attraverso determinati calcoli.

2. Spiegazione del codice

1. I dati importati sono un file txt costituito dal nome della foto e dai dati tridimensionali del punto target, quindi dobbiamo stabilire una funzione di segmentazione delle stringhe per ottenere i dati nel file txt.

2. Definire il valore pi greco

3. Definire la funzione di calcolo della direzione

4. Estrai le informazioni richieste dal file txt

5. Calcolare l'angolo di cambio di direzione del target tra foto adiacenti

6. Calcola la distanza in movimento e la velocità del bersaglio tra foto adiacenti

3. Visualizzazione completa del codice

4. Visualizzazione dei risultati

Tutti i codici in questo articolo sono forniti dall'utente CSDN CV-X.WANG. A qualsiasi individuo o gruppo non è consentito condurre attività commerciali e didattiche. Qualsiasi quotazione o quotazione parziale deve ottenere l'autorizzazione.

1. Introduzione

        Utilizziamo un drone monoculare per localizzare un bersaglio in movimento sul terreno scattando foto a intervalli di tempo uguali. Attualmente, abbiamo ottenuto le coordinate tridimensionali del bersaglio in ciascuna foto e conosciamo la posizione del drone che scatta foto durante il volo. intervallo di tempo, quindi possiamo ottenere la direzione e la velocità del movimento del bersaglio attraverso determinati calcoli.

2. Spiegazione del codice

1. I dati importati sono un file txt costituito dal nome della foto e dai dati tridimensionali del punto target, quindi dobbiamo stabilire una funzione di segmentazione delle stringhe per ottenere i dati nel file 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. Definire il valore pi greco

#define M_PI       3.14159265358979323846   // pi

3. Definire la funzione di calcolo della direzione

Per ottenere la direzione di movimento del bersaglio nella direzione del piano, questo articolo adotta il metodo della direzione a 360°, comune in campo militare. Cioè, il vero nord è la direzione di 0° e la direzione in senso orario è 0-360°. Ad esempio, la vera direzione est: nel nostro sistema di direzione, è la direzione 90°.

alcuni,

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

È in radianti.

  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. Estrai le informazioni richieste dal file 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. Calcolare l'angolo di cambio di direzione del target tra foto adiacenti

  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. Calcola la distanza in movimento e la velocità del bersaglio tra foto adiacenti

Nota: la formula di calcolo per la distanza che otteniamo qui è:

Questa è solo la dimostrazione più semplice. Nelle situazioni reali, dobbiamo considerare una serie di condizioni come il sistema di coordinate, la posizione dell'area di misurazione, ecc., per ottenere una distanza più 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. Visualizzazione completa del codice

  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. Visualizzazione dei risultati

Tutti i codici in questo articolo sono forniti dall'utente CSDN CV-X.WANG, a qualsiasi individuo o gruppo non è consentito svolgere attività commerciale e didattica e qualsiasi quotazione, anche parziale, dovrà essere preventivamente autorizzata.

Profilo personale

Si dedica alla ricerca tecnologica da più di 30 anni ed è esperto in vari linguaggi come Java, Linux, Javascript, php, css, ecc. Ha dato numerosi contributi nel campo dell'open source stazione di documentazione per gli sviluppatori per condividere alcuni problemi nello sviluppo della tecnologia per riferimento futuro

le mie informazioni di contatto

Posta