自己给自己网站做推广,文章内容网站系统,某网站建设策划方案,网站建设寮步引言#xff1a;在实际工程项目中#xff0c;为了提高系统的响应速度和稳定性#xff0c;往往需要采用一定的控制算法进行目标跟踪。这里抛砖引玉#xff0c;仅采用简单的PID算法进行目标的跟随控制#xff0c;目标的识别依然采用yolo。对系统要求更高的#xff0c;可以对…引言在实际工程项目中为了提高系统的响应速度和稳定性往往需要采用一定的控制算法进行目标跟踪。这里抛砖引玉仅采用简单的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.launchlaunch文件解析
此处的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.subscribemavros_msgs::State(mavros/state, 100, state_cb);local_pos_sub nh.subscribenav_msgs::Odometry(/mavros/local_position/odom, 100, local_pos_cb);object_pos_sub nh.subscribegeometry_msgs::PointStamped(object_position, 100, object_pos_cb);mavros_setpoint_pos_pub nh.advertisemavros_msgs::PositionTarget(/mavros/setpoint_raw/local, 100); arming_client nh.serviceClientmavros_msgs::CommandBool(mavros/cmd/arming);set_mode_client nh.serviceClientmavros_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站进行讲解。同时会提供相应的实机演示。链接会在后续给出。
