超维空间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.40

using 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站进行讲解。同时会提供相应的实机演示。链接会在后续给出。

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

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

【Redis】一文掌握Redis原理及常见问题

【Redis】一文掌握Redis原理及常见问题

Redis是基于内存数据库&#xff0c;操作效率高&#xff0c;提供丰富的数据结构&#xff08;Redis底层对数据结构还做了优化&#xff09;&#xff0c;可用作数据库&#xff0c;缓存&#xff0c;消息中间件等。如今广泛用于互联网大厂&#xff0c;面试必考点之一&#xff0c;本文…
阅读更多...
python3下载手机安卓版,python下载手机版最新

python3下载手机安卓版,python下载手机版最新

大家好&#xff0c;小编为大家解答python3下载手机安卓版的问题。很多人还不知道python下载手机版最新&#xff0c;现在让我们一起来看看吧&#xff01; 1、先去python官网下载python3的源码包&#xff0c;网址&#xff1a;https://www.python.org/ 1)进去之后点击导航栏的Down…
阅读更多...
CentOS系统环境搭建（二十六）——使用nginx在无域名情况下使用免费证书设置https

CentOS系统环境搭建（二十六）——使用nginx在无域名情况下使用免费证书设置https

centos系统环境搭建专栏&#x1f517;点击跳转 文章目录 使用nginx在无域名情况下使用免费证书设置https1.获取SSL证书1.1 生成SSL密钥1.2 生成SSL证书1.3 重命名密钥文件 2.nginx配置https2.1 放证书2.2 修改nginx.conf文件2.2.1 将80端口重定向到4432.2.2 端口443配置ssl证书…
阅读更多...
【三维生成】稀疏重建、Image-to-3D方法（汇总）

【三维生成】稀疏重建、Image-to-3D方法（汇总）

系列文章目录 总结一下近5年的三维生成算法&#xff0c;持续更新 文章目录 系列文章目录一、LRM&#xff1a;单图像的大模型重建&#xff08;2023&#xff09;摘要1.前言2.Method3.实验 二、SSDNeRF&#xff1a;单阶段Diffusion NeRF的三维生成和重建&#xff08;ICCV 2023&am…
阅读更多...
基于VUE3+Layui从头搭建通用后台管理系统（前端篇）十六：统计报表模块相关功能实现

基于VUE3+Layui从头搭建通用后台管理系统（前端篇）十六：统计报表模块相关功能实现

一、本章内容 本章使用Echarts及DataV实现常用图表、特殊图表、地图及综合图表等图表展示功能。 1. 详细课程地址： https://edu.csdn.net/course/detail/38183 2. 源码下载地址： 点击下载 二、界面预览 三、开发视频 3.1 B站视频地址： 基于VUE3+Layui从
阅读更多...
2023.12.25 关于 Redis 数据类型 Hash 常用命令、内部编码、应用场景

2023.12.25 关于 Redis 数据类型 Hash 常用命令、内部编码、应用场景

目录 Hash 数据类型 Hash 操作命令 HSET HGET HEXISTS HDEL HKEYS HVALS HGETALL HMGET HLEN HSETNX HINCRBY HINCRBYFLOAT HSTRLEN Hash 编码方式 理解什么是压缩 Hash 实际应用 Cache 缓存 Hash 数据类型 整体上来说 Redis 是键值对结构&#xff0c;其中 …
阅读更多...
【C++高阶(九)】C++类型转换以及IO流

【C++高阶(九)】C++类型转换以及IO流

&#x1f493;博主CSDN主页:杭电码农-NEO&#x1f493;   ⏩专栏分类:C从入门到精通⏪   &#x1f69a;代码仓库:NEO的学习日记&#x1f69a;   &#x1f339;关注我&#x1faf5;带你学习C   &#x1f51d;&#x1f51d; C高阶 1. 前言2. C语言类型转换的方式3. C的强制…
阅读更多...
5G阅信在教研行业应用场景有哪些？

5G阅信在教研行业应用场景有哪些？

1、新生入学 新生录用通知、录用信息查看、注意事项&#xff08;入学指引&#xff09; 校园简介 资讯1 秒速达&#xff0c;让信息传达更高效便捷 2、校务服务 可作为校园微服务的新入口 提供学校各专业课表课程查看、校园卡办理、一卡通应…
阅读更多...
怎样在win10命令行窗口跑起来mujava

怎样在win10命令行窗口跑起来mujava

MuJava简介 Java (muJava) 是 Java 程序的变异系统。 它自动生成用于传统突变测试和类级别突变测试的突变体。 Java 可以测试单个类和多个类的包。 用户以对封装在单独 JUnit 类的方法中的被测类的方法调用序列的形式提供测试。 官网地址&#xff1a;Java Home Page 需要下…
阅读更多...
正则表达式与bs4选择器筛选论文数准确率之比较

正则表达式与bs4选择器筛选论文数准确率之比较

一、正则爬取论文网首页论文标题的示例 import requests import re from bs4 import BeautifulSoupheaders {User-Agent: Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/83.0.4103.116 Safari/537.36}def get_html(url):try:res…
阅读更多...
C# 通过SharpCompress.Archives.Rar解压RaR文件

C# 通过SharpCompress.Archives.Rar解压RaR文件

/// <summary>/// 解压一个Rar文件/// </summary>/// <param name"RarFile">需要解压的Rar文件&#xff08;绝对路径&#xff09;</param>/// <param name"TargetDirectory">解压到的目录</param>/// <param name&…
阅读更多...
如何将图片（matlab、python）无损放入word论文

如何将图片（matlab、python）无损放入word论文

许多论文对插图有要求&#xff0c;直接插入png、jpg一般是不行的&#xff0c;这是一篇顶刊文章&#xff08;pdf&#xff09;的插图&#xff0c;放大2400%后依旧清晰&#xff0c;搜罗了网上的方法&#xff0c;总结了一下如何将图片无损放入论文中。 这里主要讨论的是数据生成的图…
阅读更多...
渗透测试（Lab4.2）

渗透测试（Lab4.2）

配置WebDeveloper的时候遇到一个错误 导入失败&#xff0c;因为 E:…ovf 未通过 OVF 规范一致性或虚拟硬件合规性检查。 请单击“重试”放松 OVF 规范与虚拟硬件合规性检查&#xff0c;并重新尝试导入&#xff1b; 或单击“取消”以取消导入。如果重新尝试导入&#xff0c;可能…
阅读更多...
[Angular] 笔记 10：服务与依赖注入

[Angular] 笔记 10：服务与依赖注入

什么是 Services & Dependency Injection? chatgpt 回答&#xff1a; 在 Angular 中&#xff0c;Services 是用来提供特定功能或执行特定任务的可重用代码块。它们可以用于处理数据、执行 HTTP 请求、管理应用程序状态等。Dependency Injection&#xff08;依赖注入&#…
阅读更多...
w16php系列之基础数组

w16php系列之基础数组

一、索引数组 概念 索引数组 是指键名为整数的数组。默认情况下&#xff0c;索引数组的键名是从0开始&#xff0c;并依次递增。它主要适用于利用位置&#xff08;0、1、2……&#xff09;来标识数组元素的情况。另外&#xff0c;索引数组的键名也可以自己指定 示例代码 <…
阅读更多...
VScode跑通Remix.js官方的contact程序开发过程

VScode跑通Remix.js官方的contact程序开发过程

目录 1 引言 2 安装并跑起来 3 设置根路由 4 用links来添加风格资源 ​5 联系人路由的UI 6 添加联系人的UI组件 7 嵌套路由和出口 8 类型推理 9 Loader里的URL参数 10 验证参数并抛出响应 书接上回&#xff0c;我们已经跑通了remix的quick start项目&#xff0c;接下…
阅读更多...
开源verilog模拟 iverilog verilator +gtkwave仿真及一点区别

开源verilog模拟 iverilog verilator +gtkwave仿真及一点区别

开源的 iverilog verilator 和商业软件动不动几G几十G相比&#xff0c;体积小的几乎可以忽略不计。 两个都比较好用&#xff0c;各有优势。 iverilog兼容性好。 verilator速度快。 配上gtkwave 看波形&#xff0c;仿真工具基本就齐了。 说下基本用法 计数器 counter.v module…
阅读更多...
HarmonyOS4.0开发该怎么系统学习，适合哪些人？

HarmonyOS4.0开发该怎么系统学习，适合哪些人？

对于想要系统学习HarmonyOS 4.0开发的人来说&#xff0c;以下是一些建议&#xff1a; 1.了解HarmonyOS基础&#xff1a; 首先&#xff0c;你需要对HarmonyOS有一个基本的了解&#xff0c;包括它的核心概念、系统架构、分布式技术等。可以通过官方文档、教程和在线课程来深入了…
阅读更多...
学习笔记15——前端和http协议

学习笔记15——前端和http协议

学习笔记系列开头惯例发布一些寻亲消息&#xff0c;感谢关注&#xff01; 链接&#xff1a;https://baobeihuijia.com/bbhj/ 关系 客户端&#xff1a;对连接访问到的前端代码进行解析和渲染&#xff0c;就是浏览器的内核服务器端&#xff1a;按照规则编写前端界面代码 解析标准…
阅读更多...
园艺伴侣应用程序Plant-it

园艺伴侣应用程序Plant-it

什么是 Plant-it &#xff1f; Plant-it 是一款园艺伴侣应用程序&#xff0c;可帮助您照顾植物。它不会建议您采取哪些操作&#xff0c;而是旨在记录您正在执行的活动。这是故意的&#xff0c;软件作者坚信&#xff0c;唯一负责知道何时给植物浇水、何时施肥等的人是你&#xf…
阅读更多...
最新文章

Copyright @ 2022~2023