GAZEBO仿真+offboard模式无人机速度控制

已于 2024-05-27 17:16:04 修改
阅读量775 收藏 14
点赞数 3
文章标签： 无人机
于 2023-09-04 22:06:42 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/2301_76165902/article/details/132643286
版权

offboard代码部分

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>  
#include <mavros_msgs/CommandBool.h> 
#include <mavros_msgs/SetMode.h>     
#include <mavros_msgs/State.h>
#include <geometry_msgs/Twist.h>	//添加速度话题消息类型的头文件

typedef struct
{	
	float kp = 0.88;              //比例系数
	float ki = 0.0002;              //积分系数
	float kd = 1.68;              //微分系数
	
	float err_I_lim = 500;		//积分限幅值
	
	float errx_Now,errx_old_Last,errx_old_LLast;           //当前偏差,上一次偏差,上上次偏差
	float erry_Now,erry_old_Last,erry_old_LLast;
	float errz_Now,errz_old_Last,errz_old_LLast;
	
	float errx_p,errx_i,errx_d;
	float erry_p,erry_i,erry_d;
	float errz_p,errz_i,errz_d;		
	
	float CtrOutx,CtrOuty,CtrOutz;          //控制增量输出
 
        float OUTLIM = 0;		//输出限幅
	
}PID;
PID H;

geometry_msgs::Twist vector;
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}
// 订阅的无人机当前位置数据
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
	local_pos = *msg;
}

void vec_pid(float pose_x,float pose_y,float pose_z)
{
	H.errx_Now = pose_x - local_pos.pose.position.x;
	H.errx_p = H.errx_Now;
	H.errx_i = H.errx_Now + H.errx_i;
	H.errx_d = H.errx_Now - H.errx_old_Last + H.errx_old_Last - H.errx_old_LLast;
	H.erry_Now = pose_y - local_pos.pose.position.y;
	H.erry_p = H.erry_Now;
	H.erry_i = H.erry_Now + H.erry_i;
	H.erry_d = H.erry_Now - H.erry_old_Last + H.erry_old_Last - H.erry_old_LLast;
	H.errz_Now = pose_z - local_pos.pose.position.z;
	H.errz_p = H.errz_Now;
	H.errz_i = H.errz_Now + H.errz_i;
	H.errz_d = H.errz_Now - H.errz_old_Last + H.errz_old_Last - H.errz_old_LLast;

	H.errx_old_LLast = H.errx_old_Last;
	H.errx_old_Last = H.errx_Now;
	H.erry_old_LLast = H.erry_old_Last;
	H.erry_old_Last = H.erry_Now;	
	H.errz_old_LLast = H.errz_old_Last;
	H.errz_old_Last = H.errz_Now;	
	
	//积分限幅
	if(H.errx_i > H.err_I_lim)	H.errx_i = H.err_I_lim;		
	if(H.errx_i < -H.err_I_lim)	H.errx_i = -H.err_I_lim;
	if(H.erry_i > H.err_I_lim)	H.erry_i = H.err_I_lim;		
	if(H.erry_i < -H.err_I_lim)	H.erry_i = -H.err_I_lim;
	if(H.errz_i > H.err_I_lim)	H.errz_i = H.err_I_lim;		
	if(H.errz_i < -H.err_I_lim)	H.errz_i = -H.err_I_lim;

	H.CtrOutx = H.errx_p*H.kp + H.errx_i*H.ki + H.errx_d*H.kd;
	H.CtrOuty = H.erry_p*H.kp + H.erry_i*H.ki + H.erry_d*H.kd;
	H.CtrOutz = H.errz_p*H.kp + H.errz_i*H.ki + H.errz_d*H.kd;	

	/*
	H.OUTLIM = limt;
	if(H.OUTLIM != 0)
	{
		if(H.CtrOutx > H.OUTLIM)		H.CtrOutx = H.OUTLIM;
		if(H.CtrOutx < -H.OUTLIM)		H.CtrOutx = -H.OUTLIM;
		if(H.CtrOuty > H.OUTLIM)		H.CtrOuty = H.OUTLIM;
		if(H.CtrOuty < -H.OUTLIM)		H.CtrOuty = -H.OUTLIM;
	}	
	*/
 
	vector.linear.x = H.CtrOutx;
	vector.linear.y = H.CtrOuty;
	vector.linear.z = H.CtrOutz;
}

int main(int argc, char **argv)
{
setlocale(LC_ALL, "");

ros::init(argc, argv, "offb_node"); //ros初始化，最后一个参数为节点名称
ros::NodeHandle nh;


//订阅。<>里面为模板参数，传入的是订阅的消息体类型，（）里面传入三个参数，分别是该消息体的位置、缓存大小（通常为10）、回调函数
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, local_pos_cb);//订阅位置信息
//ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);//控制话题,用于发布位置信息
ros::Publisher vec_pub = nh.advertise<geometry_msgs::Twist>("mavros/setpoint_velocity/cmd_vel_unstamped", 10);//速度控制话题,用于发布速度信息

ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");

ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
ros::Rate rate(20.0);
 
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}
/*
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
*/

	vector.linear.x = 0.0;
	vector.linear.y = 0.0;
	vector.linear.z = 0.0;

for(int i = 100; ros::ok() && i > 0; --i){
       vec_pub.publish(vector);
       ros::spinOnce();
       rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";

//设定无人机保护模式 POSTION
mavros_msgs::SetMode offb_setPS_mode;
offb_setPS_mode.request.custom_mode = "POSCTL";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
 
ros::Time last_request = ros::Time::now();

ROS_INFO("UAV 启动!");
int step = 0;
int sametimes = 0;

while(ros::ok())//进入大循环
{
  if (current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
	{
		if (set_mode_client.call(offb_setPS_mode) && offb_setPS_mode.response.mode_sent)
		{
			ROS_INFO("POSTION PROTECTED");
		}
		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("UAV is arming");//解锁后打印信息
            }
            last_request = ros::Time::now();
        }
	else
	{
		switch(step)
		{
			case 0:
				//pose.pose.position.x = 0;
				//pose.pose.position.y = 0;
				//pose.pose.position.z = 2;
				vec_pid(0,0,2);
				if (local_pos.pose.position.z > 1.9 && local_pos.pose.position.z < 2.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 1;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 1:
				//pose.pose.position.x = 5;
				//pose.pose.position.y = 0;
				//pose.pose.position.z = 2;
				vec_pid(20,0,2);
				if (local_pos.pose.position.x > 19.9 && local_pos.pose.position.x < 20.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 2;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 2:
				//pose.pose.position.x = 5;
				//pose.pose.position.y = 5;
				//pose.pose.position.z = 2;
				vec_pid(20,20,2);
				if (local_pos.pose.position.y > 19.9 && local_pos.pose.position.y < 20.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 3;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 3:
				//pose.pose.position.x = 0;
				//pose.pose.position.y = 5;
				//pose.pose.position.z = 2;
				vec_pid(0,20,2);
				if (local_pos.pose.position.x > -0.1 && local_pos.pose.position.x < 0.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 4;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 4:
				//pose.pose.position.x = 0;
				//pose.pose.position.y = 0;
				//pose.pose.position.z = 2;
				vec_pid(0,0,2);
				if (local_pos.pose.position.x > -0.1 && local_pos.pose.position.x < 0.1 && local_pos.pose.position.y > -0.1 && local_pos.pose.position.y < 0.1)
                  		{
     		  	                if (sametimes > 20)
              				{
         		                   step = 5;
                        		}
                        		else
                           		sametimes++;
                    			}
                    			else sametimes = 0;
	 	                break;
			case 5:
				offb_set_mode.request.custom_mode = "AUTO.LAND";
				if (current_state.mode != "AUTO.LAND" && (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("AUTO.LAND enabled");
					}
					last_request = ros::Time::now();
					}
				break;
				default:
					break;
		}
	}
    } 
    vec_pub.publish(vector);

    ros::spinOnce();
    rate.sleep();
}
 
return 0;

}

 编写CMakeList.txt文件

add_executable(offboard_node2 src/offboard_node2.cpp)
target_link_libraries(offboard_node2
  ${catkin_LIBRARIES}
)

编译运行

b5b3a6cd0ba64ef4a77af0776ccd20a0.png

 rostopic echo mavros/local_position/pose 可以查看飞行器的位置信息

 rostopic echo mavros/local_position/veclocity_local 可以查看飞行器的速度信息(该速度为ENU东北天坐标系下的速度,即x轴为东西方向,x正为东,y轴为南北方向,y正为北)

83b11bf10717426ca7f919c3c626083c.png

 添加部分

1.将位置控制改为了速度控制,使用mavros/setpoint_velocity/cmd_vel_unstamped话题

ff3b0c5f06e64988a8fb255606c977dd.png

 该话题的变量有两种,一个是角速度一个是线速度.我们使用线速度.

添加订阅者vec_pub和头文件

2.添加了PID计算速度

添加了结构体变量,添加了位置式PID算法

void vec_pid(float pose_x,float pose_y,float pose_z)
{
	H.errx_Now = pose_x - local_pos.pose.position.x;
	H.errx_p = H.errx_Now;
	H.errx_i = H.errx_Now + H.errx_i;
	H.errx_d = H.errx_Now - H.errx_old_Last + H.errx_old_Last - H.errx_old_LLast;
	H.erry_Now = pose_y - local_pos.pose.position.y;
	H.erry_p = H.erry_Now;
	H.erry_i = H.erry_Now + H.erry_i;
	H.erry_d = H.erry_Now - H.erry_old_Last + H.erry_old_Last - H.erry_old_LLast;
	H.errz_Now = pose_z - local_pos.pose.position.z;
	H.errz_p = H.errz_Now;
	H.errz_i = H.errz_Now + H.errz_i;
	H.errz_d = H.errz_Now - H.errz_old_Last + H.errz_old_Last - H.errz_old_LLast;

	H.errx_old_LLast = H.errx_old_Last;
	H.errx_old_Last = H.errx_Now;
	H.erry_old_LLast = H.erry_old_Last;
	H.erry_old_Last = H.erry_Now;	
	H.errz_old_LLast = H.errz_old_Last;
	H.errz_old_Last = H.errz_Now;	
	
	//积分限幅
	if(H.errx_i > H.err_I_lim)	H.errx_i = H.err_I_lim;		
	if(H.errx_i < -H.err_I_lim)	H.errx_i = -H.err_I_lim;
	if(H.erry_i > H.err_I_lim)	H.erry_i = H.err_I_lim;		
	if(H.erry_i < -H.err_I_lim)	H.erry_i = -H.err_I_lim;
	if(H.errz_i > H.err_I_lim)	H.errz_i = H.err_I_lim;		
	if(H.errz_i < -H.err_I_lim)	H.errz_i = -H.err_I_lim;

	H.CtrOutx = H.errx_p*H.kp + H.errx_i*H.ki + H.errx_d*H.kd;
	H.CtrOuty = H.erry_p*H.kp + H.erry_i*H.ki + H.erry_d*H.kd;
	H.CtrOutz = H.errz_p*H.kp + H.errz_i*H.ki + H.errz_d*H.kd;	

	/*
	H.OUTLIM = limt;
	if(H.OUTLIM != 0)
	{
		if(H.CtrOutx > H.OUTLIM)		H.CtrOutx = H.OUTLIM;
		if(H.CtrOutx < -H.OUTLIM)		H.CtrOutx = -H.OUTLIM;
		if(H.CtrOuty > H.OUTLIM)		H.CtrOuty = H.OUTLIM;
		if(H.CtrOuty < -H.OUTLIM)		H.CtrOuty = -H.OUTLIM;
	}	
	*/
 
	vector.linear.x = H.CtrOutx;
	vector.linear.y = H.CtrOuty;
	vector.linear.z = H.CtrOutz;
}

最基本的计算速度方法.将每个点位放入PID计算速度

优点

用速度控制可以限制速度大小,现在由pid计算的速度可以达到8m/s,如果加上最大限制可以达到限速的效果,

H.OUTLIM = limt;
	if(H.OUTLIM != 0)
	{
		if(H.CtrOutx > H.OUTLIM)		H.CtrOutx = H.OUTLIM;
		if(H.CtrOutx < -H.OUTLIM)		H.CtrOutx = -H.OUTLIM;
		if(H.CtrOuty > H.OUTLIM)		H.CtrOuty = H.OUTLIM;
		if(H.CtrOuty < -H.OUTLIM)		H.CtrOuty = -H.OUTLIM;
	}

这样给予的最大速度就为limt变量值.

 

&黄昏的乐师
关注
gazebo仿真
qq_44089890的博客
05-11 632
可以使用 Gazebo 提供的传感器模型和 ROS 提供的消息类型，例如。需要注意的是，ROS 和 Gazebo 之间的交互是异步的，即 ROS 发布的消息和 Gazebo 接收的消息之间可能存在延迟。在 Gazebo 中接收控制指令：通过在 Gazebo 中加载 ROS 插件，接收 ROS 中发布的控制指令，并将其转换为 Gazebo 中的模型控制指令。在 ROS 中加载 Gazebo 插件：使用 ROS 提供的插件系统，加载 Gazebo 插件，将 ROS 和 Gazebo 进行连接。
无人机 PX4 飞控 | Offboard 模式详解与gazebo仿真测试
月照银海似蛟龙的博客
07-30 1657
在该模式下，飞行器根据飞行控制栈外部（如机载计算机）提供的设定值控制位置、速度、加速度、姿态以及推力/力矩。设定值可以通过MAVLink协议提供给飞控PX4要求外部控制器提供2Hz连续的“有效存在”信号，该信号可由任意支持的 MAVLink 设置点消息提供。PX4只有在收到该种信号超过1秒后才有效，如果该种信号停止飞行控制栈将重新获得控制权（脱离Offboard模式）。此模式需要位置或位姿信息的提供 - 例如GPS、光流、视觉惯性里程计、mocap等。
Gazebo仿真
weixin_30307921的博客
03-14 743
1、建议在本地Ubuntu 16.04下运行仿真程序。目前Gazebo模拟器的兼容性是一大问题，在虚拟机或配置较低的电脑上可能无法运行。如果你的显卡是N卡，建议安装Ubuntu下的显卡驱动。 2、运行Gazebo仿真程序前，请将Gazebo升级到7.x版本以上（推荐7.9版本）。 查看Gazebo版本方法 gazebo -v #确认7.0以上，推荐7.9 升级方法 $ s...
介绍Gazebo，ROS2兼容的模拟仿真机器人-AirSim
最新发布
远方雪的专栏
09-20 622
Gazebo 是一个著名的开源机器人仿真软件，广泛用于机器人算法开发、物理交互模拟、传感器数据的模拟以及视觉仿真等。它提供了一个丰富的环境，其中包括高度详细的模型、逼真的物理引擎和高级的图形渲染。Gazebo 能够模拟复杂的环境以及多种物理现象，如光照、阴影、地形纹理等。
GAZEBO仿真
XUTWOTWO的博客
10-22 1449
首先建立一个功能包mrobot_gazebo (gazebo_plugins gazebo_ros gazebo_ros_control roscpp rospy) $ cd ~/catkin_ws/src $ catkin_create_pkg mrobot_gazebo gazebo_plugins gazebo_ros gazebo_ros_control roscpp rospy 另起一...
Gazebo 仿真
m0_54091074的博客
08-23 372
Prometheus/Simulator/gazebo_simulator/(可以新建文件夹，也可以保存在detection_words)目前想法：一和二都不需要动，只需要修改传入的world参数；新建的world文件保存在。
基于PX4、ROS和Gazebo的无人机通用仿真平台
03-07
首先，PX4是一款开源的飞行控制系统，它主要用于无人驾驶航空器，包括固定翼、多旋翼以及混合动力无人机。PX4采用模块化设计，包含了飞行控制算法、传感器融合、导航和自主飞行功能。它的优势在于灵活性高，可以适应...
Simulink程序：使用ROS2控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化
02-22
文章【PX4&Simulink&Gazebo联合仿真】在Simulink中使用ROS2控制无人机进入Offboard模式起飞悬停并在Gazebo中可视化对应的源程序。 https://blog.csdn.net/weixin_45031928/article/details/136213665 microRTPS桥接...
Gazebo仿真+单次指点飞行
weixin_43914923的博客
05-26 838
1、开启QGC地面站与gazebo仿真 cd PX4-Autopilot/ make px4_sitl gazebo_iris 2、在catkin_ws下src目录创建offboard_pkg catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs 3、新建offboard_node.cpp /** * @file offb_node.cpp * @brief Offboard control
px4的gazebo仿真教程
11-01
【PX4的Gazebo仿真教程】 在无人机和机器人领域，仿真环境的使用至关重要，它允许开发者在不涉及实际硬件的情况下测试和优化控制系统。在PX4开源飞控系统中，Gazebo是一个强大的仿真工具，提供了丰富的场景和多种...
Ardupilot多架无人机于gazebo内仿真
06-02
ardupilot多家无人机于gazebo内仿真文档
迷你无人车 Gazebo 仿真
Solititude的博客
09-06 519
阿克曼转向小车 Gazebo 仿真与操控
# gazebo 仿真
m0_54027545的博客
05-07 2282
#gazebo 仿真
Gazebo仿真 【ROS: noetic】
weixin_46034116的博客
10-18 1900
了解如何使用URDF文件创建一个机器人模型，然后使用xacro文件优化该模型，并且放置到rviz+ArbotiX或Gazebo仿真环境中，以实现丰富的ROS功能。
机器人仿真（一）——Asctec 四轴飞行器 Gazebo 仿真
XiaoZhuang
11-26 6845
本文代码来源，ETH-ASl 实验室 如果没有条件花费昂贵的价钱采购几只asctec的四周飞行器（基准价七万人人民币左右），又想过一把机器人的干瘾，有或者希望用仿真平台验证自己的算法，建立一个仿真的机器人系统是十分有必要的。 闲话不说，本文平台基于ubuntu 14.04 + ROS,并且假设你已经配置好了相应的ROS环境(ros配置还请参考ros教程) 具体代码参考：https://gith
Gazebo机器人仿真
重复的出发点的博客
04-16 5028
在ROS中利用gazebo和rviz对小车进行仿真 安装其他依赖 二、Gazebo的使用和world创建 获取gazebo模型库： 自制实验场景 Edit -> Building Editor保存模型：File -> Save -> 保存路径保存环境模型：File -> Save World As -> 保存路径这里创建了一个maze.world文件。 创建完成后可将环境模型置于功能包的world文件夹下。 可使用gazebo_wor
Gazebo仿真--Gazebo插件与ROS
努力加油，冲~
12-10 524
插件为URDF模型提供了更大的功能，并且可以将ROS消息和服务呼叫联系在一起，以实现传感器输出和指令输入。 插件类型 URDF文件引用几种类型： ModelPlugins，提供对Physical :: Model API的访问 SensorPlugins，提供对传感器:: Sensor API的访问 VisualPlugins，提供对render :: Visual API的访问 添加一个 ModelPlugin 简而言之，将ModelPlugin插入元素内的URDF中。在之中，以指示传递给gazebo
XTDrone仿真教程：ROS+PX4+Gazebo构建顶级无人机平台
该教程聚焦于利用ROS（Robot Operating System）、PX4飞行控制系统、Gazebo仿真环境以及mavros套件，构建国内顶尖的无人机仿真平台XTDrone。XTDrone的源代码可以在Gitee上找到，其使用文档则托管在Yuque上，提供详细...
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值