Berbagi teknologi

Penentuan posisi UAV pada target pergerakan darat --- mendapatkan arah pergerakan dan kecepatan target

2024-07-12

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

Daftar isi

1. Perkenalan

Kami menggunakan drone bermata untuk menemukan target bergerak di darat dengan mengambil foto pada interval waktu yang sama. Saat ini, kami telah memperoleh koordinat tiga dimensi target di setiap foto, dan mengetahui lokasi drone yang mengambil foto selama penerbangan. selang waktu, maka kita dapat memperoleh arah dan kecepatan pergerakan target melalui perhitungan tertentu.

2. Penjelasan kode

1. Data yang diimpor berupa file txt yang terdiri dari nama foto dan data tiga dimensi titik target, sehingga kita perlu membuat fungsi segmentasi string untuk mendapatkan data pada file txt.

2. Tentukan nilai pi

3. Menentukan fungsi perhitungan arah

4. Ekstrak informasi yang diperlukan dari file txt

5. Hitung perubahan arah sudut target antar foto yang berdekatan

6. Hitung jarak gerak dan kecepatan target antar foto yang berdekatan

3. Tampilan kode lengkap

4. Tampilan hasil

Semua kode dalam artikel ini disediakan oleh pengguna CSDN CV-X.WANG. Setiap individu atau kelompok tidak diperbolehkan melakukan aktivitas komersial dan pengajaran. Kutipan atau kutipan sebagian harus mendapatkan izin.

1. Perkenalan

        Kami menggunakan drone bermata untuk menemukan target bergerak di darat dengan mengambil foto pada interval waktu yang sama. Saat ini, kami telah memperoleh koordinat tiga dimensi target di setiap foto, dan mengetahui lokasi drone yang mengambil foto selama penerbangan. selang waktu, maka kita dapat memperoleh arah dan kecepatan pergerakan target melalui perhitungan tertentu.

2. Penjelasan kode

1. Data yang diimpor berupa file txt yang terdiri dari nama foto dan data tiga dimensi titik target, sehingga kita perlu membuat fungsi segmentasi string untuk mendapatkan data pada 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. Tentukan nilai pi

#define M_PI       3.14159265358979323846   // pi

3. Menentukan fungsi perhitungan arah

Untuk mendapatkan arah pergerakan sasaran searah bidang, artikel ini mengadopsi metode arah 360° yang umum digunakan di bidang militer. Artinya, arah utara sebenarnya adalah arah 0°, dan arah searah jarum jam adalah 0-360°. Misalnya, arah timur sebenarnya: dalam sistem arah kita, arahnya adalah 90°.

beberapa,

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

Itu dalam radian.

  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. Ekstrak informasi yang diperlukan dari 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. Hitung perubahan arah sudut target antar foto yang berdekatan

  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. Hitung jarak gerak dan kecepatan target antar foto yang berdekatan

Mohon diperhatikan: Rumus perhitungan jarak yang kita peroleh disini adalah:

Ini hanyalah demonstrasi paling sederhana. Dalam situasi sebenarnya, kita perlu mempertimbangkan serangkaian kondisi seperti sistem koordinat, lokasi area pengukuran, dll., untuk mendapatkan Jarak yang lebih akurat.

  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. Tampilan kode lengkap

  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. Tampilan hasil

Semua kode dalam artikel ini disediakan oleh pengguna CSDN CV-X.WANG, setiap individu atau kelompok tidak diperbolehkan melakukan kegiatan komersial dan pengajaran, dan kutipan atau kutipan sebagian harus mendapat izin.

Profil pribadi

Ia telah mengabdikan dirinya untuk meneliti teknologi selama lebih dari 30 tahun, dan mahir dalam berbagai bahasa seperti java, linux, javascript, php, css, dll. Ia telah memberikan banyak kontribusi di bidang open source stasiun dokumentasi pengembang untuk berbagi beberapa masalah dalam pengembangan teknologi untuk referensi di masa mendatang. Semua orang memeriksanya

informasi kontak saya

Surat