超维空间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.mzph.cn/news/579525.shtml

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

基于Java在线商城系统设计实现（源码+部署文档+讲解视频）

基于Java在线商城系统设计实现（源码+部署文档+讲解视频）

博主介绍&#xff1a; ✌至今服务客户已经1000、专注于Java技术领域、项目定制、技术答疑、开发工具、毕业项目实战 ✌&#x1f345; 文末获取源码联系 &#x1f345;&#x1f447;&#x1f3fb; 精彩专栏 推荐订阅 &#x1f447;&#x1f3fb; 不然下次找不到 Java项目精品实…
阅读更多...
使用YT Config Tools工具导出引脚配置清单至Excel文件

使用YT Config Tools工具导出引脚配置清单至Excel文件

使用YT Config Tool工具导出引脚配置清单至Excel文件 文章目录 使用YT Config Tool工具导出引脚配置清单至Excel文件IntroductionOperations在YTC中导入hello_world样例工程在Pinout Configuration标签页中配置引脚保存源码工程导出Excel文件 Conclusion Introduction YT Conf…
阅读更多...
GC6208国产5V摄像机镜头驱动IC ，可用于摄像机，机器人等产品中可替代AN41908

GC6208国产5V摄像机镜头驱动IC ，可用于摄像机，机器人等产品中可替代AN41908

GC6208是一个镜头电机驱动IC摄像机和安全摄像机。该设备集成了一个直流电机驱动器的Iris的PID控制系统&#xff0c;也有两个通道的STM电机驱动器的变焦和对焦控制。 芯片的特点: 内置用于Iris控制器的直流电机驱动器 内置2个STM驱动程序&#xff0c;用于缩放和…
阅读更多...
【WPF】使用Behavior以及ValidationRule实现表单校验

【WPF】使用Behavior以及ValidationRule实现表单校验

文章目录 使用ValidationRule实现检测用户输入EmptyValidationRule 非空校验TextBox设置非空校验TextBox设置非空校验并显示校验提示 结语 使用ValidationRule实现检测用户输入 EmptyValidationRule是TextBox内容是否为空校验&#xff0c;TextBox的Binding属性设置ValidationRu…
阅读更多...
华锐视点为广汽集团打造VR汽车在线展厅，打破地域限制，尽享购车乐趣

华锐视点为广汽集团打造VR汽车在线展厅，打破地域限制，尽享购车乐趣

随着科技的飞速发展&#xff0c;我们正在进入一个全新的时代——元宇宙时代。元宇宙是一个虚拟的世界&#xff0c;它不仅能够模拟现实世界&#xff0c;还能够创造出现实世界无法实现的事物。而汽车行业作为人类生活的重要组成部分&#xff0c;也在积极探索与元宇宙的融合&#…
阅读更多...
[数据结构]树与二叉树的性质

[数据结构]树与二叉树的性质

文章目录 0.二叉树的形态和基本性质1.完全二叉树的叶子节点个数2.树的叶子节点个数3.线索二叉树4.树和森林和二叉树5.平衡二叉树的最少结点数6.树/二叉树/森林的转换 0.二叉树的形态和基本性质 一棵二叉树具有5中基本形态n个结点可以构造的二叉树种数: C2n-n/n1 一棵树 n个结点…
阅读更多...
SpringBoot3 整合Kafka

SpringBoot3 整合Kafka

官网&#xff1a;https://kafka.apache.org/documentation/ 消息队列-场景 1. 异步 2. 解耦 3. 削峰 4. 缓冲 消息队列-Kafka 1. 消息模式 消息发布订阅模式&#xff0c;MessageQueue中的消息不删除&#xff0c;会记录消费者的偏移量 2. Kafka工作原理 同一个消费者组里的消…
阅读更多...
Volume Control 2

Volume Control 2

为游戏添加音乐和音效总是需要一些编码来设置一个系统来控制、显示和保存应用程序的音量设置。 音量控制的设计是为了立即为您设置这些内容，让您有更多时间专注于最重要的事情——制作出色的游戏！ 在版本2中，我们对系统进行了重新设计，使其更加模块化、灵活，甚至更易于使用…
阅读更多...
工具系列：TimeGPT_(1)获取token方式和初步使用

工具系列：TimeGPT_(1)获取token方式和初步使用

文章目录 介绍获取Token用法数据的重要要求使用DateTime索引推断频率。 介绍 Nixtla的TimeGPT是一种用于时间序列数据的生成式预训练预测模型。TimeGPT可以在没有训练的情况下&#xff0c;仅使用历史值作为输入&#xff0c;为新的时间序列生成准确的预测。TimeGPT可以用于各种…
阅读更多...
ubuntu磁盘管理常用命令

ubuntu磁盘管理常用命令

写的不全&#xff0c;后面随时修改。 Linux 磁盘管理常用三个命令为 df、du 和 fdisk。 df&#xff08;英文全称&#xff1a;disk free&#xff09;&#xff1a;列出文件系统的整体磁盘未使用量du&#xff08;英文全称&#xff1a;disk used&#xff09;&#xff1a;检查磁盘空…
阅读更多...
Bytebase：统一数据库 CI/CD 解决方案 | 开源日报 No.128

Bytebase：统一数据库 CI/CD 解决方案 | 开源日报 No.128

bytebase/bytebase Stars: 7.9k License: NOASSERTION Bytebase 是一个数据库 CI/CD 解决方案&#xff0c;为开发人员和 DBA 提供统一的工具来管理不同数据库系统的开发生命周期。其主要功能包括标准化操作流程、SQL 代码审查、GitOps 集成以及数据访问控制等。关键特性和核心…
阅读更多...
QT 输入框输入限制 正则表达式限制 整理

QT 输入框输入限制 正则表达式限制 整理

在使用 输入数值时&#xff0c;经常遇到限制其范围的需要&#xff0c;比如角太阳高度角范围为[-90,90]&#xff0c;经度值范围[-180,180]&#xff0c;方位角范围[0,360]。Qt提供了QIntValidator和QDoubleValidator可以限定数值输入范围&#xff0c;如使用QIntValidator限制整数…
阅读更多...
TypeScript学习笔记归纳（先做基础梳理，后期不断丰满，持续更新ing）

TypeScript学习笔记归纳（先做基础梳理，后期不断丰满，持续更新ing）

文章目录 前言 二、TypeScript的优势体现在哪里&#xff1f; 1、执行时间上的区别 2、基础数据类型区别 3、TS优势 三、TypeScript的关键特性 四、TypeScript的类型系统 1、什么是类型注释&#xff1f; 2、类型系统核心 - 常用类型 1&#xff09; 基本类型&#xff0…
阅读更多...
计网03-数据的封装和解封装

计网03-数据的封装和解封装

数据封装和解封装的过程 实例&#xff1a;有两台电脑 PC&#xff11;和PC&#xff12;&#xff0c;PC1要给PC&#xff12;发送一个文本文件 1、数据的封装过程&#xff1a; 应用层&#xff1a;将原始数据转换成计算机能识别的二进制数传输层&#xff1a;在传输层是有固定的传…
阅读更多...
为什么说依赖抽象就变得更加灵活呢？举例

为什么说依赖抽象就变得更加灵活呢？举例

说依赖抽象变得更加灵活的主要原因在于它提供了更大的替换和扩展的空间。让我们通过一个简单的例子来说明&#xff1a; 考虑一个电商系统&#xff0c;其中有一个OrderProcessor类负责处理订单&#xff0c;它依赖于一个PaymentGateway用于处理支付。最初的设计可能如下所示&…
阅读更多...
LabVIEW进行激光斑点图像处理与分析

LabVIEW进行激光斑点图像处理与分析

LabVIEW进行激光斑点图像处理与分析 近年来&#xff0c;激光技术的应用日益繁荣。激光光斑的质量评估和分析技术决定了应用效果&#xff0c;对机器视觉、武器装备、光学测量和医疗设备产生深远影响。就具体用途和技术而言&#xff0c;激光光斑的采集和处理至关重要。即插即用的…
阅读更多...
渗透测试 | php的webshell绕过方法总结

渗透测试 | php的webshell绕过方法总结

目录 1.php的异或运算 2.通过获取注释去绕过 3.利用字符的运算符​​​​​​​ 4.通过end函数代替[] 5.通过常量去绕过 6.字符串拼接双美元符 7.通过函数定义绕过 8.通过类定义&#xff0c;然后传参分割 9.多传参方式绕过​​​​​​​ 10.通过get_defined_function…
阅读更多...
使用Maven Archetype插件制作项目脚手架(一)

使用Maven Archetype插件制作项目脚手架(一)

Archetype是一个Maven项目模板工具包。通过Archetype我们可以快速搭建Maven项目。比如我们在ide里面创建项目时&#xff0c;可以选择很多maven内置的Archetype&#xff0c;我们最常用的可能是maven-archetype-quickstart 当然maven提供了能力&#xff0c;让我们自定义项目结构&…
阅读更多...
帮我超越技术壁垒的“泰斗”服务--Amazon ElastiCache

帮我超越技术壁垒的“泰斗”服务--Amazon ElastiCache

前言 作为开发者&#xff0c;外加上云服务的使用者&#xff0c;对于借助云服务来提升日常开发中的效率是一个非常关键且重要的事情。在日常实际开发中&#xff0c;关于缓存服务领域想必作为开发者应该都不陌生&#xff0c;比如常用到的 Redis 就是缓存服务之一。对于互联网领域…
阅读更多...
使用ffmpeg实现视频旋转并保持清晰度不变

使用ffmpeg实现视频旋转并保持清晰度不变

1 原始视频信息 通过ffmpeg -i命令查看视频基本信息 ffmpeg -i source.mp4 ffmpeg version 6.1-essentials_build-www.gyan.dev Copyright (c) 2000-2023 the FFmpeg developersbuilt with gcc 12.2.0 (Rev10, Built by MSYS2 project)configuration: --enable-gpl --enable-…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023