超维空间S2无人机使用说明书——52、初级版——使用PID算法进行基于yolo的目标跟踪

引言：在实际工程项目中，为了提高系统的响应速度和稳定性，往往需要采用一定的控制算法进行目标跟踪。这里抛砖引玉，仅采用简单的PID算法进行目标的跟随控制，目标的识别依然采用yolo。对系统要求更高的，可以对算法进行改进，也欢迎读者与我们联系，合作开发。

步骤一：打开摄像头

注意：为了获取目标物的三维位置信息，我们采用了D435深度摄像头，仅供参考，可根据需要自行选择即可

roslaunch realsense2_camera rs_camera.launch

请添加图片描述

查看话题，需要/camera/color/image_raw和/camera/depth/image_rect_raw

请添加图片描述

步骤二：打开yolo识别节点，具体yolo版本可以根据需要选择

 roslaunch darknet_ros darknet_ros.launch

请添加图片描述

没有报错的情况下，会弹出识别效果图，如下：

请添加图片描述## 注：我这里训练的是自己打印的H型地标，具体可以根据需要选择合适的目标物

步骤三：打开三维坐标转换节点

该节点可以直接一话题的形式输出目标物的名称和真实的位置信息

roslaunch darknet_real_position darknet_real_position.launch

请添加图片描述

launch文件解析

此处的launch文件，以参数的方式指定了识别目标。比如landing，因此这个节点只会把指定的landing地标位置信息打印出来，其他的目标通通忽略

请添加图片描述

查看话题数据/object_position

请添加图片描述

请添加图片描述

从上述图片可以看出，系统非常准确的给出了目标物的名称和真实的位置信息，单位是米。需要指出的是，这里的位置是相对于D435摄像头的位置信息，X表示横向位置，Y表示纵向位置，Z表示实际的距离信息

步骤四：启动PID跟随节点。注意，可以先不要启动mavros，仅仅测试PID控制器发布出的速度是否正确。在确认了没问题后在启动mavros节点，无人机就可以进行正常的跟随运动了

roslaunch follow_pid follow_pid.launch

请添加图片描述

launch文件解析

这里仅仅进行偏航角度和距离的控制，如果需要对高度方向控制。可以直接复制代码进行简单的修改即可。参数linear_x_p和linear_x_d是距离的PID控制，同理yaw_rate_p和yaw_rate_d是角度的控制。参数target_x_angle是期望保持的角度，通常设置为0即可。最后参数target_distance是期望保持的距离，单位是毫米

请添加图片描述

代码如下：

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>   
#include <string>#define MAX_ERROR 0.20
#define VEL_SET   0.10
#define ALTITUDE  0.40using namespace std;float target_x_angle = 0;
float target_distance = 2000;
float linear_x_p = 0.5;
float linear_x_d = 0.33;
float yaw_rate_p = 4.0;
float yaw_rate_d = 15;geometry_msgs::PointStamped object_pos; 
nav_msgs::Odometry local_pos;
mavros_msgs::State current_state;  
mavros_msgs::PositionTarget setpoint_raw;
//检测到的物体坐标值
string current_frame_id   = "no_object";
double current_position_x = 0;
double current_position_y = 0;
double current_distance   = 0;//1、订阅无人机状态话题
ros::Subscriber state_sub;//2、订阅无人机实时位置信息
ros::Subscriber local_pos_sub;//3、订阅实时位置信息
ros::Subscriber object_pos_sub;//4、发布无人机多维控制话题
ros::Publisher  mavros_setpoint_pos_pub;//5、请求无人机解锁服务        
ros::ServiceClient arming_client;//6、请求无人机设置飞行模式，本代码请求进入offboard
ros::ServiceClient set_mode_client;void pid_control()
{static float last_error_x_angle = 0;static float last_error_distance = 0;				float x_angle;float distance;if(current_position_x == 0 && current_position_y == 0 && current_distance == 0){x_angle  = target_x_angle;distance = target_distance;}else{x_angle = current_position_x / current_distance;distance = current_distance;}float error_x_angle = x_angle - target_x_angle;float error_distance = distance - target_distance;if(error_x_angle > -0.01 && error_x_angle < 0.01)  {error_x_angle = 0;}if(error_distance > -80 && error_distance < 80) {error_distance = 0;}setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;if(setpoint_raw.velocity.x < -0.3)  {setpoint_raw.velocity.x = -0.3;}else if(setpoint_raw.velocity.x > 0.3) {setpoint_raw.velocity.x = 0.3;	}setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;if(setpoint_raw.yaw_rate < -0.5)  {setpoint_raw.yaw_rate = -0.5;}else if(setpoint_raw.yaw_rate > 0.5) {setpoint_raw.yaw_rate = 0.5;}mavros_setpoint_pos_pub.publish(setpoint_raw);last_error_x_angle  = error_x_angle;last_error_distance = error_distance;
}void state_cb(const mavros_msgs::State::ConstPtr& msg)
{current_state = *msg;
}void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{local_pos = *msg;
}void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{object_pos = *msg;current_position_x = object_pos.point.x*(-1000);current_position_y = object_pos.point.y*(-1000);//此处将距离由单位米改称毫米，方便提高控制精度current_distance   = object_pos.point.z*1000;current_frame_id   = object_pos.header.frame_id; pid_control();	 //ROS_INFO("current_position_x = %f",current_position_x);//ROS_INFO("current_position_y = %f",current_position_y);//ROS_INFO("current_distance = %f"  ,current_distance);
}int main(int argc, char *argv[])
{ros::init(argc, argv, "follow_pid");ros::NodeHandle nh;state_sub     = nh.subscribe<mavros_msgs::State>("mavros/state", 100, state_cb);local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 100, local_pos_cb);object_pos_sub = nh.subscribe<geometry_msgs::PointStamped>("object_position", 100, object_pos_cb);mavros_setpoint_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);   arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");ros::Rate rate(20.0); ros::param::get("linear_x_p",linear_x_p);ros::param::get("linear_x_d",linear_x_d);ros::param::get("yaw_rate_p",yaw_rate_p);ros::param::get("yaw_rate_d",yaw_rate_d);ros::param::get("target_x_angle", target_x_angle);ros::param::get("target_distance",target_distance);//等待连接到PX4无人机/* while(ros::ok() && current_state.connected){ros::spinOnce();rate.sleep();}*/setpoint_raw.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 /*+ 1024 + 2048*/;setpoint_raw.coordinate_frame = 1;setpoint_raw.position.x = 0;setpoint_raw.position.y = 0;setpoint_raw.position.z = 0 + ALTITUDE;mavros_setpoint_pos_pub.publish(setpoint_raw);for(int i = 100; ros::ok() && i > 0; --i){mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}//请求offboard模式变量mavros_msgs::SetMode offb_set_mode;offb_set_mode.request.custom_mode = "OFFBOARD";//请求解锁变量mavros_msgs::CommandBool arm_cmd;arm_cmd.request.value = true;ros::Time last_request = ros::Time::now();//请求进入offboard模式并且解锁无人机，15秒后退出，防止重复请求       /*while(ros::ok()){//请求进入OFFBOARD模式if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ROS_INFO("Offboard enabled");}last_request = ros::Time::now();}else {//请求解锁if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){if( arming_client.call(arm_cmd) && arm_cmd.response.success){ROS_INFO("Vehicle armed");}last_request = ros::Time::now();}}if(ros::Time::now() - last_request > ros::Duration(15.0))break;mavros_setpoint_pos_pub.publish(setpoint_raw);ros::spinOnce();rate.sleep();}*/   while(ros::ok()){//ROS_INFO("11111");ros::spinOnce();rate.sleep();}}

步骤五：在上述基础上再打开mavros，即可开始跟随控制。代码后续会在B站进行讲解。同时会提供相应的实机演示。链接会在后续给出。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.xdnf.cn/news/824162.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章

Unity中裁剪空间推导（使用FOV来调节）

Unity中裁剪空间推导（使用FOV来调节）

文章目录 前言一、使用FOV代替之前使用的Size&#xff08;h&#xff09;1、我们可以把矩阵中使用到 h(高) 和 w(宽) 的部分使用比值替换掉。2、替换后 前言 在之前的文章中&#xff0c;我们控制透视相机使用的是SIze。但是&#xff0c;在透视相机中&#xff0c;我们使用的是FO…
阅读更多...
118基于matlab的二级倒立摆的数学建模

118基于matlab的二级倒立摆的数学建模

基于matlab的二级倒立摆的数学建模&#xff0c;引入二次型最优控制率&#xff0c;对系统进行仿真分析&#xff0c;得出摆杆及小车位置变化曲线。程序已调通&#xff0c;可直接运行。
阅读更多...
【MySQL】事务Transaction

【MySQL】事务Transaction

1. 事务的概念 事务是什么 在业务逻辑中使用sql&#xff0c;面对一些较复杂的场景&#xff0c;是需要多个sql语句组合起来实现的。如&#xff1a;银行的转账业务&#xff0c;若客户A要转账100元给客户B&#xff0c;就要两条sql&#xff1a;A余额减100&#xff0c;B余额加100&a…
阅读更多...
[Angular] 笔记 24：ngContainer vs. ngTemplate vs. ngContent

[Angular] 笔记 24：ngContainer vs. ngTemplate vs. ngContent

请说明 Angular 中 ngContainer&#xff0c; ngTemplate 和 ngContent 这三者之间的区别。 chatgpt 回答&#xff1a; 这三个在 Angular 中的概念是关于处理和组织视图的。 1. ngContainer&#xff1a; ngContainer 是一个虚拟的 HTML 容器&#xff0c;它本身不会在最终渲染…
阅读更多...
【Spark精讲】一文讲透SparkSQL聚合过程以及UDAF开发

【Spark精讲】一文讲透SparkSQL聚合过程以及UDAF开发

SparkSQL聚合过程 这里的 Partial 方式表示聚合函数的模式&#xff0c;能够支持预先局部聚合&#xff0c;这方面的内容会在下一节详细介绍。 对应实例中的聚合语句&#xff0c;因为 count 函数支持 Partial 方式&#xff0c;因此调用的是 planAggregateWithoutDistinct 方法&a…
阅读更多...
【AIGC科技展望】预测AIGC2025年的机会与挑战

【AIGC科技展望】预测AIGC2025年的机会与挑战

2025年&#xff0c;AIGC的机会与挑战 在未来的五年里&#xff0c;AIGC&#xff08;AI Generated Content&#xff09;将会成为一个越来越重要的领域。但是&#xff0c;伴随着机会而来的是挑战。在这篇文章中&#xff0c;我们将一起探讨AIGC的机会与挑战&#xff0c;并预测2025…
阅读更多...
SSH -L：安全、便捷、无边界的网络通行证

SSH -L：安全、便捷、无边界的网络通行证

欢迎来到我的博客&#xff0c;代码的世界里&#xff0c;每一行都是一个故事 SSH -L&#xff1a;安全、便捷、无边界的网络通行证 前言1. SSH -L基础概念SSH -L 的基本语法&#xff1a;端口转发的原理和作用&#xff1a; 2. SSH -L的基本用法远程访问本地示例&#xff1a;访问本…
阅读更多...
项目中使用Java中List.subList()的注意事项

项目中使用Java中List.subList()的注意事项

使用介绍 在Java中&#xff0c;subList是List接口的一个方法&#xff0c;用于获取原始列表的子列表 方法的声明如下 List<E> subList(int fromIndex, int toIndex);fromIndex&#xff1a;起始索引&#xff08;包括&#xff09;toIndex&#xff1a;结束索引&#xff08…
阅读更多...
深入浅出理解TensorFlow的padding填充算法

深入浅出理解TensorFlow的padding填充算法

一、参考资料 notes_on_padding_2 二、TensorFlow的padding算法 本文以TensorFlow v2.14.0版本为例&#xff0c;介绍TensorFlow的padding算法。 1. 引言 tf.nn.conv2d and tf.nn.max_pool2d 函数都有padding参数&#xff0c;在执行函数之前&#xff0c;都需要进行填充padd…
阅读更多...
系列六、Consul

系列六、Consul

一、Consul 1.1、概述 Consul是一套开源的分布式服务发现和配置管理系统&#xff0c;由HashiCorp公司用Go语言开发。他提供了微服务系统中的服务治理、配置中心、控制总线等功能。这些功能中的每一个功能都可以单独使用&#xff0c;也可以一起使用以构建全方位的服务网格&…
阅读更多...
AI模型训练【偏差/方差】与【欠拟合/过拟合】

AI模型训练【偏差/方差】与【欠拟合/过拟合】

在我们拿到一个数据集&#xff0c;高高兴兴准备训练一个模型时&#xff0c;会遇到欠拟合或过拟合的问题&#xff0c;业内也喜欢用偏差和方差这两指标去定义它们&#xff0c;那这些词什么意思呢&#xff1f;有什么方法能避免/解决 欠拟合和过拟合呢&#xff1f; 这其实是非常非常…
阅读更多...
将本地工作空间robot_ws上传到gitee仓库

将本地工作空间robot_ws上传到gitee仓库

git config --global user.name "geniusChinaHN" git config --global user.email "12705243geniuschinahnuser.noreply.gitee.com" cd ~/robot_ws #git init#创建原始仓库时候用 git add . git commit -m "上传文件内容描述" #git remote add r…
阅读更多...
day9--java高级编程：多线程

day9--java高级编程：多线程

1 Day16–多线程01 1.1 程序概念 程序(program)&#xff1a;是为完成特定任务、用某种语言编写的一组指令的集合。即指一段静态的代码&#xff0c;静态对象。 1.2 进程 1.2.1 概念 进程(process)&#xff1a;是程序的一次执行过程&#xff0c;或是正在运行的一个程序。是一…
阅读更多...
『番外篇七』SwiftUI 获取视图全局位置在 NavigationStack 中失效的解决方法

『番外篇七』SwiftUI 获取视图全局位置在 NavigationStack 中失效的解决方法

概览 在 番外篇六』SwiftUI 取得任意视图全局位置的三种方法 这篇博文里，我们详细讨论了在 SwiftUI 中获取任意视图全局坐标的几种方法。 不过，我们也从中提到了某些方法无法适用于 NavigationStack 视图，本篇博文由此应运而生。 在本篇博文种，您将学到如下内容： 概览1.…
阅读更多...
git是什么，git入门常用基本命令

git是什么，git入门常用基本命令

文章目录 git是什么1 .git init--初始化2.git status--检测当前文件夹下面文件状态3. git add--要管理的文件4.git add . --管理当前文件夹下的所有文件5.git commit -m--生成第一个版本6.git log--查看版本的记录 git是什么 分布式&#xff0c;版本控制&#xff0c;软件 版本…
阅读更多...
通信原理课设(gec6818) 007:语音识别

通信原理课设(gec6818) 007:语音识别

目录 1、去科大讯飞官网下载对应的sdk 2、科大讯飞文件夹的意思 3、配置ARM的录音环境 4、编程实现语音识别 我们的需求是将一个语音文件从客户端传到服务器&#xff0c;因此我们最好是选用tcp 现在市面上面常用的语音识别解决方案为&#xff1a;科大讯飞c和百度c 离…
阅读更多...
zookeeper之集群搭建

zookeeper之集群搭建

1. 集群角色 zookeeper集群下&#xff0c;有3种角色&#xff0c;分别是领导者(Leader)、跟随着(Follower)、观察者(Observer)。接下来我们分别看一下这三种角色的作用。 领导者(Leader)&#xff1a; 事务请求&#xff08;写操作&#xff09;的唯一调度者和处理者&#xff0c;保…
阅读更多...
【C语言】一篇文章深入解析联合体和枚举且和结构体的区别

【C语言】一篇文章深入解析联合体和枚举且和结构体的区别

文章目录 &#x1f4dd;前言&#x1f320; 联合体类型的声明&#x1f309;联合体的特点 &#x1f320;相同成员的结构体和联合体对⽐&#x1f309;联合体⼤⼩的计算 &#x1f320;联合体应用&#x1f309;枚举类型的声明 &#x1f320;枚举类型的优点&#x1f309; 枚举类型的使…
阅读更多...
MacBook查看本机IP

MacBook查看本机IP

嘚吧嘚 其实这也不是什么困难的问题&#xff0c;但是今年刚刚入坑Mac&#xff0c;外加用的频率不是很高&#xff0c;每次使用的时候都查&#xff0c;用完就忘&#xff0c;下次用的时候再查&#x1f92e;。真的把自己恶心坏了&#x1f648;。 所以写篇文章记录一下&#x1f92…
阅读更多...
软件设计师——软件工程（三）

软件设计师——软件工程（三）

&#x1f4d1;前言 本文主要是【软件工程】——软件设计师——软件工程的文章&#xff0c;如果有什么需要改进的地方还请大佬指出⛺️ &#x1f3ac;作者简介&#xff1a;大家好&#xff0c;我是听风与他&#x1f947; ☁️博客首页&#xff1a;CSDN主页听风与他 &#x1f304…
阅读更多...
最新文章

Copyright @ 2022~2023