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

server/2024/10/11 13:27:56/

目录

一、引子

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

二、代码解释

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 radiansdouble 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 radiansdouble delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;// Calculate y and x componentsdouble 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 radiansdouble direction_rad = atan2(y, x);// Convert direction angle to degreesdouble direction_deg = direction_rad * 180.0 / M_PI;// Ensure direction angle is within [0, 360) degreesif (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 linegetline(file, line);vector<vector<string>> extractedData;// Read each line from the filewhile (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, 15vector<string> extractedColumns;extractedColumns.push_back(columns[0]);  // Image NameextractedColumns.push_back(columns[13]); // LongitudeextractedColumns.push_back(columns[14]); // LatitudeextractedColumns.push_back(columns[15]); // AltitudeextractedData.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 Directioncout << "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的Δlondouble lat_ = lat2_1 / 2; //1 / 2的Δlatdouble 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);//长半轴 短半轴  单位是mint 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;//拍照间隔 sdouble 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 radiansdouble 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 radiansdouble delta_lon_rad = (lon2 - lon1) * M_PI / 180.0;// Calculate y and x componentsdouble 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 radiansdouble direction_rad = atan2(y, x);// Convert direction angle to degreesdouble direction_deg = direction_rad * 180.0 / M_PI;// Ensure direction angle is within [0, 360) degreesif (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 linegetline(file, line);vector<vector<string>> extractedData;// Read each line from the filewhile (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, 15vector<string> extractedColumns;extractedColumns.push_back(columns[0]);  // Image NameextractedColumns.push_back(columns[13]); // LongitudeextractedColumns.push_back(columns[14]); // LatitudeextractedColumns.push_back(columns[15]); // AltitudeextractedData.push_back(extractedColumns);}file.close();// Calculate direction angles between adjacent image centerscout << "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 Directioncout << "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的Δlondouble lat_ = lat2_1 / 2; //1 / 2的Δlatdouble 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);//长半轴 短半轴  单位是mint 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;//拍照间隔 sdouble speed = Distance / time;cout << "Speed From " << extractedData[i - 1][0] << " to " << extractedData[i][0] << ": " << "=" << speed << " meter per second" << endl;}//cin.get();return 0;
}

四、结果展示

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


http://www.ppmy.cn/server/58372.html

相关文章

Java中的InetAddress类

Java中的InetAddress类

InetAddress类是Java标准库中的一个重要类&#xff0c;用于表示互联网协议&#xff08;IP&#xff09;地址。它可以表示IPv4和IPv6地址&#xff0c;提供了用于查找主机名和IP地址的静态方法&#xff0c;以及获取本地主机和远程主机的网络地址信息的功能。本文将深入探讨InetAdd…
阅读更多...
使用clion刷leetcode

使用clion刷leetcode

如何优雅的使用clion刷leetcode 安装插件&#xff1a;LeetCode Editor) 插件配置&#xff1a; 这样我们每打开一个项目&#xff0c;就会创建类似的文件 我们的项目结构&#xff1a; 我们在题解文件中导入头文件myHeader.h并将新建的文件添加到cmakelists.txt文件&#xff0c;…
阅读更多...
KBPC2504-ASEMI无人机专用整流桥KBPC2504

KBPC2504-ASEMI无人机专用整流桥KBPC2504

编辑&#xff1a;ll KBPC2504-ASEMI无人机专用整流桥KBPC2504 型号&#xff1a;KBPC2504 品牌&#xff1a;ASEMI 封装&#xff1a;KBPC-4 最大重复峰值反向电压&#xff1a;400V 最大正向平均整流电流(Vdss)&#xff1a;25A 功率(Pd)&#xff1a;中小功率 芯片个数&…
阅读更多...
枚举类 (enum)

枚举类 (enum)

目录 一、为什么要有枚举类&#xff1f; 二、枚举的简介 三、自定义枚举类 四、使用enum关键字 五、注意事项 一、为什么要有枚举类&#xff1f; 假如我们有这样的一个需求&#xff1a;设计季节类&#xff0c;并创建对象。 我们就需要以下操作&#xff0c;创建Season类&…
阅读更多...
CSS学习

CSS学习

CSS学习 CSS&#xff08;层叠样式表&#xff09;是网页设计和开发中的重要技术&#xff0c;用于描述HTML或XML文档的样式和布局。掌握CSS不仅能让网页看起来更加美观&#xff0c;还能提高网页的加载速度和可维护性。 一、CSS基础入门 1. CSS简介 CSS是一种用于描述HTML或XM…
阅读更多...
kafka 消费者

kafka 消费者

消费者 消费者。消费者连接到Kafka上并接收消息&#xff0c;进而进行相应的业务逻辑处理。 消费组 消费者负责订阅Kafka中的主题&#xff0c;并且从订阅的主题上拉取消息。 消费组&#xff1a;每个消费者都有一个对应的消费组&#xff0c;每一个分区只能被一个消费组中的一个…
阅读更多...
nginx优化

nginx优化

编辑nginx.conf server_tokens off 关闭版本号 修改用户与组 在配置文件中将用户注释掉 表示主进程master会有root创建&#xff0c;子进程由nginx用户来创建 设置页面的缓存时间&#xff0c;主要是针对动态页面&#xff0c;图片的缓存 cd html/ 先把图片托进来 配置文件做…
阅读更多...
//usr/lib/libgdal.so.20:对‘sqlite3_column_table_name’未定义的引用

//usr/lib/libgdal.so.20:对‘sqlite3_column_table_name’未定义的引用

//usr/lib/libgdal.so.20:对‘sqlite3_column_table_name’未定义的引用 编译安装sqlite3之后&#xff0c;会出现 “ //usr/lib/libgdal.so.20:对‘sqlite3_column_table_name’未定义的引用”的报错&#xff0c;主要是因为之前安装低版本sqlite3的时候改了系统的文件导致的 …
阅读更多...
最新文章

Copyright @ 2022~2023