超维空间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/280500.html

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

相关文章

使用SecoClient软件连接L2TP

使用SecoClient软件连接L2TP

secoclient软件是华为防火墙与友商设备进行微屁恩对接的一款软件,运行在windows下可以替代掉win系统自带的连接功能,因为win系统自带的连接功能总是不可用而且我照着网上查到的各种方法调试了很久都调不好,导致我一度怀疑是我的服务没搭建好,浪费了大把时间去研究其他搭建方案 …
阅读更多...
Kubernetes技术与架构-集群管理

Kubernetes技术与架构-集群管理

Kubernetes技术与架构提供支撑工具支持集群的规划、安装、创建以及管理。 数字证书 用户可以使用easyrsa、openssl、cfssl工具生成数字证书&#xff0c;在kubernetes集群的api server中部署数字证书用于访问鉴权 资源管理 如上所示&#xff0c;定义一个服务类service用于负…
阅读更多...
Flask笔记

Flask笔记

一&#xff1a;模板渲染 一般的话都序列化成字符串 二&#xff1a;项目拆分 2.1 项目拆分 app.py init.py views.py models.py 模型数据 2.2 蓝图 三&#xff1a;路由参数 3.1 String 重点 3.2 int 3.3 path 3.4 UUID 3.5 any 四&#xff1a;请求方式 五&#xff1a;Requ…
阅读更多...
将网页变身移动应用：网址封装成App的完全指南

将网页变身移动应用：网址封装成App的完全指南

什么是网址封装&#xff1f; 网址封装是一个将你的网站或网页直接嵌入到一个原生应用容器中的过程。用户可以通过下载你的App来访问网站&#xff0c;而无需通过浏览器。这种方式不仅提升了用户体验&#xff0c;还可利用移动设备的功能&#xff0c;如推送通知和硬件集成。 小猪…
阅读更多...
介绍一款PDF在线工具

介绍一款PDF在线工具

PDF是我们日常工作中的一种常见格式&#xff0c;其处理也是我们工作的重要基础性环节&#xff0c;一款可靠的处理工具显得十分重要。 完全免费、易于使用、丰富的PDF处理工具&#xff0c;包括&#xff1a;合并、拆分、压缩、转换、旋转和解锁PDF文件&#xff0c;以及给PDF文件…
阅读更多...
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、…
阅读更多...
SSM房屋租赁系统----计算机毕业设计

SSM房屋租赁系统----计算机毕业设计

项目介绍 房屋租赁系统&#xff0c;基于 Spring5.x 的实战项目&#xff0c;此项目非Maven项目。 前台系统主要功能包括房源列表展示、房源详细信息展示、根据房源特征进行搜索&#xff0c;包括&#xff1a;房型、小区名;以及房源的预订功能。 后台管理&#xff1a; 用户信息管…
阅读更多...
1.2.0 IGP高级特性之FRR

1.2.0 IGP高级特性之FRR

理论部分参考文档&#xff1a;Segment Routing TI-LFA FRR保护技术 - 华为 一、快速重路由技术 FRR(Fast Reroute)快速重路由 实现备份链路的快速切换&#xff0c;也可以与BFD联动实现对故障的快速感知。 随着网络的不断发展&#xff0c;VoIP和在线视频等业务对实时性的要求越…
阅读更多...
【AIGC-图片生成视频系列-3】AI视频随心而动：MotionCtrl的相机运动控制和物体运动控制

【AIGC-图片生成视频系列-3】AI视频随心而动：MotionCtrl的相机运动控制和物体运动控制

最近&#xff0c;「单张图片生成视频」相关工作很多&#xff0c;但运动控制的准确性依旧是个挑战&#xff0c;包括相机运动的控制以及物体运动控制。 然&#xff0c;MotionCtrl 横空出世。 一. 项目简介 MotionCtrl——一个相机运动控制、物体运动控制的视频工具&#xff0c…
阅读更多...
[C#]opencvsharp进行图像拼接普通拼接stitch算法拼接

[C#]opencvsharp进行图像拼接普通拼接stitch算法拼接

介绍&#xff1a; opencvsharp进行图像拼一般有2种方式&#xff1a;一种是传统方法将2个图片上下或者左右拼接&#xff0c;还有一个方法就是融合拼接&#xff0c;stitch拼接就是一种非常好的算法。opencv里面已经有stitch拼接算法因此我们很容易进行拼接。 效果&#xff1a; …
阅读更多...
二分查找(非朴素)--在排序数组中查找元素的第一个和最后一个位置

二分查找(非朴素)--在排序数组中查找元素的第一个和最后一个位置

个人主页&#xff1a;Lei宝啊 愿所有美好如期而遇 目录 本题链接 输入描述 输出描述 算法分析 1.算法一&#xff1a;暴力求解 2.算法二&#xff1a;朴素二分算法 3.算法三&#xff1a;二分查找左右端点 3.1查找左端点 3.1.1细节一&#xff1a;循环条件 3.1.2细节二…
阅读更多...
【前端基础】——原型与原型链详解，看一篇即可【图文版】

【前端基础】——原型与原型链详解，看一篇即可【图文版】

前言 本文旨在通过图文的方式&#xff0c;一步步回顾原型链的整个流程是如何运作的&#xff0c;如果你刚好在电脑旁边&#xff0c;不妨跟着我的思路&#xff0c;一起走一遍敲一遍代码流程&#xff0c;你会发现原型链并没有你想的那么复杂。 new关键字 我们先看这一个代码&am…
阅读更多...
华锐三维云展平台 | VR在线展览云平台提供定制化虚拟展厅制作工具

华锐三维云展平台 | VR在线展览云平台提供定制化虚拟展厅制作工具

随着科技的飞速发展&#xff0c;互联网技术的不断革新&#xff0c;广州华锐互动顺应时代需求&#xff0c;开发了VR在线展览云平台&#xff0c;用户可以在平台上自主创建属于自己的3D展厅。VR在线展览云平台正改变着传统展览行业的模式&#xff0c;为参展者提供更高效、更便捷、…
阅读更多...
vue3中怎么巧妙的去运用jsx？

vue3中怎么巧妙的去运用jsx？

文章目录 概要JSX / TSX?安装配置封装JsxRender.vue使用JsxRender.vue怎么巧妙的去使用它?Demo下载 概要 我们都知道vue3是支持用jsx/tsx&#xff0c;但是好像从来都没有人告诉我们应该怎么运用到项目当中&#xff0c;下面是我觉得不错的一种使用方式&#xff0c;分享给大家…
阅读更多...
二级路由的配置以及注意项

二级路由的配置以及注意项

二级路由 比如说LayOut组件是父亲&#xff0c;LayOut和ArtComp是儿子&#xff0c;那我们怎么给儿子配路由呢&#xff1f; 1、首先在router下的index.js导入组件&#xff0c;配置规则&#xff0c;详细如下 // 导入路由相关组件 import LayOut from /views/LayOut import UserC…
阅读更多...
IntelliJ IDEA常用快捷键

IntelliJ IDEA常用快捷键

【1】创建内容&#xff08;新建&#xff09;&#xff1a;altinsert 【2】main方法&#xff1a;psvm 【3】输出语句&#xff1a;sout 【4】复制行&#xff1a;ctrld 【5】删除行&#xff1a;ctrly&#xff08;很多编辑器ctrly是前进操作&#xff0c;如果选择 Delete Line&…
阅读更多...
C++内联函数与引用（超详细）

C++内联函数与引用（超详细）

文章目录 前言一、内联函数1.为什么会存在内联函数2.什么是内联函数3.内联函数注意事项 二、引用1.什么是引用2.引用的特性3.常引用4.引用使用场景5.引用与指针 总结 前言 一、内联函数 1.为什么会存在内联函数 &#x1f9d0;&#x1f9d0;首先我们介绍内联函数之前&#xf…
阅读更多...
NVMe over Fabrics：概念、应用和实现

NVMe over Fabrics：概念、应用和实现

对于大部分人来说&#xff0c;NVMe over Fabrics&#xff08;简称NVMf&#xff09;还是个新东西&#xff0c;因为其第一个正式版本的协议在今年6月份才发布。但是这并不影响人们对NVMf的关注&#xff0c;因为这项依托于NVMe的技术很可能继续改变存储市场格局。 NVMf的贡献在于…
阅读更多...
USB -- STM32F103复合设备（HID+MassStorage）传输讲解（十）

USB -- STM32F103复合设备（HID+MassStorage）传输讲解（十）

目录 链接快速定位 前沿 1 描述符讲解 1.1 设备描述符 1.2 配置描述符 1.3 接口描述符 1.4 功能描述符 1.5 端点描述符 1.6 字符串描述符 1.7 报告描述符 2 运行演示 链接快速定位 USB -- 初识USB协议&#xff08;一&#xff09; 源码下载请参考链接&#xff1a;…
阅读更多...
vivado CDC约束-“设置总线倾斜”对话框

vivado CDC约束-“设置总线倾斜”对话框

“设置总线倾斜”对话框 在AMD Vivado™ IDE中&#xff0c;可以通过多种方式设置总线偏斜约束&#xff1a; •通过时间约束编辑器。选择窗口 → 时间限制 → 断言 → 设置总线倾斜。从“时序约束编辑器”中&#xff0c;可以添加、删除或修改总线扭曲约束。 注意&#…
阅读更多...
最新文章

Copyright @ 2022~2023