超维空间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/586696.shtml

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

相关文章

项目中使用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…
阅读更多...
Activemq存储KahaDb详解

Activemq存储KahaDb详解

引言 ActiveMQ在不提供持久化的情况下&#xff0c;数据保存在内存中&#xff0c;一旦应用崩溃或者重启之后&#xff0c;数据都将会丢失&#xff0c;这显然在大部分情况下是我们所不希望的。对此ActiveMQ提供了两种持久化方式以供选择。 kahaDB kahaDB是一个基于文件&#xf…
阅读更多...
WPF+Halcon 培训项目实战(10):HS组件绘制图案

WPF+Halcon 培训项目实战(10):HS组件绘制图案

文章目录 前言相关链接项目专栏运行环境匹配图片模板匹配加载模板文件运行结果 绘制十字标 WPF HS组件绘制图像绘制和生成的区别 前言 为了更好地去学习WPFHalcon&#xff0c;我决定去报个班学一下。原因无非是想换个工作。相关的教学视频来源于下方的Up主的提供的教程。这里只…
阅读更多...
元旦特辑：Note6---选择排序

元旦特辑：Note6---选择排序

目录 前言❌ 1. 基本思想⚠️ 2. 直接选择排序&#x1f7e2; 2.1 思路分析✳️ 2.2 代码实现❎ 2.2.1 sort.h 2.2.2 sort.c 2.2.3 test.c 2.3 问题解决❇️ 2.3.1 sort.c修改 2.4 特性总结✅ 3. 堆排序&#x1f535; 3.1 代码实现&#x1f3e7; 3.2 特性总结&…
阅读更多...
神经元科技发布AI agent—“萨蔓莎”

神经元科技发布AI agent—“萨蔓莎”

今天神经元科技发布AI agent—“萨蔓莎“&#xff08;Samantha &#xff09;&#xff01; 取名“萨蔓莎”&#xff0c;是来自于一部讲述AI的电影《HER》。 电影讲述的是电影讲述男子西奥多汤布里&#xff08;Theodore Twombly&#xff0c;饰&#xff09;与拟人化萨曼莎&#…
阅读更多...
Vue常见面试问答

Vue常见面试问答

vue响应式数据 vue2 Vue2 的对象数据是通过 Object.defineProperty 对每个属性进行监听&#xff0c;当对属性进行读取的时候&#xff0c;就会触发 getter&#xff0c;对属性进行设置的时候&#xff0c;就会触发 setter。 /** * 这里的函数 defineReactive 用来对 Object.def…
阅读更多...
如何正确使用docker搭建redis服务器，安装gcc和make以及出现错误时的解决办法

如何正确使用docker搭建redis服务器，安装gcc和make以及出现错误时的解决办法

搭建redis服务器 目录 搭建redis服务器 &#xff08;1&#xff09;开启docker&#xff0c;并查看是否开启成功 &#xff08;2&#xff09;启动上面创建的ssrf容器&#xff0c;并进入ssrf容器 &#xff08;3&#xff09;进入opt&#xff0c;然后下载redis-5.0.5.tar.gz &a…
阅读更多...
Apache SSI 远程命令执行漏洞

Apache SSI 远程命令执行漏洞

一、环境搭建 二、访问upload.php 三、写shell <!--#exec cmd"id" --> 四、访问 如图所示&#xff0c;即getshell成功&#xff01;​
阅读更多...
Zookeeper-Zookeeper应用场景实战

Zookeeper-Zookeeper应用场景实战

1. Zookeeper Java客户端实战 ZooKeeper应用的开发主要通过Java客户端API去连接和操作ZooKeeper集群。 可供选择的Java客户端API有&#xff1a; ZooKeeper官方的Java客户端API。 第三方的Java客户端API&#xff0c;比如Curator。 ZooKeeper官方的客户端API提供了基本的操作…
阅读更多...
宝塔部署flask添加ssl即https

宝塔部署flask添加ssl即https

在宝塔部署flask的步骤我已经写了一篇博客:宝塔部署flask项目-CSDN博客 之前说如果出现找不到application错误: spawned uWSGI http 1 (pid: 3116) --- no python application found, check your startup logs for errors --- [pid: 3114|app: -1|req: -1/1] 127.0.0.1 () {6…
阅读更多...
sklearn学习的一个例子用pycharm jupyter

sklearn学习的一个例子用pycharm jupyter

环境 运行在jupyter 进行开发。即一个WEB端的开发工具。能适时显示开发的输出。后缀用的是ipynb.pycharm也可以支持。但也要提示按装jupyter. 或直接用andcoda 这里我们用pycharm进行项目创建 pip install -i https://pypi.tuna.tsinghua.edu.cn/simple jupyterlab pip ins…
阅读更多...
QT 利用开源7z 实现解压各种压缩包,包括进度条和文件名的显示(zip,7z,rar,iso等50多种格式)

QT 利用开源7z 实现解压各种压缩包,包括进度条和文件名的显示(zip,7z,rar,iso等50多种格式)

想做一个winRAR一样的解压软件吗？很简单，利用开源的7z库就能实现。我看网上其他人说的方法不敢苟同，误人子弟。以前自己在项目中使用过7z，这次又有需要，就想记录下来。如果你研究过如何用7z的话，一定知道7z的每一个GUID都代表了一种格式，50多种GUID也就有50多个格式，最…
阅读更多...
uniapp打包Android、Ios、微信小程序

uniapp打包Android、Ios、微信小程序

首先我们需要在我们的代码中&#xff0c;把我们所要用到的配置信息配置好&#xff0c;在检查一下我们测试的内容是否有打开&#xff08;取消注释&#xff09;&#xff0c;在检查一下我们的版本信息是否正确&#xff0c;查看一下接口ip是否是正式线 这里的配置信息一定要配置好…
阅读更多...
Screenshot-to-code开源项目mac上实践

Screenshot-to-code开源项目mac上实践

github上的开源项目&#xff0c;看介绍可以将设计ui图片转换为 HTML 和 CSS 源码地址&#xff1a; GitCode - 开发者的代码家园 我的mac安装了2.7和3.11&#xff0c;就用3吧直接上代码 安装 pip3 install keras tensorflow pillow h5py jupyter 报错 ERROR: Could not in…
阅读更多...
linux实用技巧：ubuntu18.04安装samba服务器实现局域网文件共享

linux实用技巧：ubuntu18.04安装samba服务器实现局域网文件共享

Ubuntu安装配置Samba服务与Win10共享文件 Chapter1 Ubuntu18.04安装配置Samba服务与Win10共享文件一、什么是Samba二、安装Samba1、查看是否有安装samba2、安装samba 三、配置Samba服务1、创建共享目录&#xff08;以samba_workspaces为例&#xff09;2、为samba设置登录用户3、…
阅读更多...
独立站的个性化定制：提升用户体验的关键

独立站的个性化定制：提升用户体验的关键

随着电子商务的竞争加剧&#xff0c;用户体验成为了企业赢得市场的关键因素之一。独立站作为企业品牌形象和产品展示的重要平台&#xff0c;其个性化定制的程度直接影响着用户体验。本文将探讨独立站的个性化定制如何提升用户体验&#xff0c;并通过代码示例说明实现个性化定制…
阅读更多...
学习动态规划不同路径、最小路径和、打家劫舍、打家劫舍iii

学习动态规划不同路径、最小路径和、打家劫舍、打家劫舍iii

学习动态规划|不同路径、最小路径和、打家劫舍、打家劫舍iii 62 不同路径 动态规划&#xff0c;dp[i][j]表示从左上角到(i,j)的路径数量dp[i][j] dp[i-1][j] dp[i][j-1] import java.util.Arrays;/*** 路径数量* 动态规划&#xff0c;dp[i][j]表示从左上角到(i,j)的路径数量…
阅读更多...
计算机网络-动态路由

计算机网络-动态路由

网络层协议&#xff1a;ip&#xff0c;ospf&#xff0c;rip&#xff0c;icmp共同组成网络层体系 ospf用于自治系统内部。 一个路由器或者网关需要能够支持多个不同的路由协议&#xff0c;以适应不同的网络环境。特别是在连接不同自治系统的边缘路由器或边界网关的情况下&#…
阅读更多...
数据库原理与应用快速复习（期末急救）

数据库原理与应用快速复习（期末急救）

文章目录 第一章数据库系统概述数据、数据库、数据库管理系统、数据定义、数据组织、存储和管理、数据操纵功能、数据库系统的构成数据管理功能、数据库管理的3个阶段以及特点数据库的特点、共享、独立、DBMS数据控制功能数据库的特点 数据模型两类数据模型、逻辑模型主要包括什…
阅读更多...
2023 IoTDB Summit：天谋科技 CTO 乔嘉林《IoTDB 企业版 V1.3： 时序数据管理一站式解决方案》...

2023 IoTDB Summit：天谋科技 CTO 乔嘉林《IoTDB 企业版 V1.3： 时序数据管理一站式解决方案》...

12 月 3 日&#xff0c;2023 IoTDB 用户大会在北京成功举行&#xff0c;收获强烈反响。本次峰会汇集了超 20 位大咖嘉宾带来工业互联网行业、技术、应用方向的精彩议题&#xff0c;多位学术泰斗、企业代表、开发者&#xff0c;深度分享了工业物联网时序数据库 IoTDB 的技术创新…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023