基于ROS搭建简易软件框架实现ROV水下目标跟踪(十四完结)–目标跟踪模块

258
0
2020年11月26日 09时12分

 

模块十分简单,可以介绍的内容很少。包括两个部分:计算目标物中心距图片中心的偏差,对应cabin_vision/object_deviation;PID跟踪控制,对应cabin_behaviors/pid_tracking。

一、偏差计算

输入输出:

 

1

 

监听topic:/darknet_ros/bounding_boxes,格式darknet_ros_msgs::BoundingBoxes,目标框图顶点在图片中像素点的位置;

发布topic:/cabin_vision/deviation,格式geometry_msgs::PointStamped,目标中心像素点与图片中心像素点偏差。

直接看代码吧,object_deviation.cpp。这部分我写了个python版本的object_deviation.py,有兴趣的话可以看看。

首先是读取设定的摄像头分辨率。

 

//Load the width and heigth of the camera image
string properties_file;
nh.param("properties_file", properties_file, std::string(""));
if(properties_file.size() != 0){
    if(access(properties_file.c_str(),F_OK) == -1){
        std::cout<<""<<std::endl;
        std::cout<<RED<<"\""<<properties_file<<"\""<<" does not exist!!!!"<<std::endl;
        std::cout<<""<<std::endl;
        ros::shutdown();
    }
    else{
        YAML::Node properties;
        properties = YAML::LoadFile(properties_file);
        image_width = properties["image_width"].as<int>();
        image_height = properties["image_height"].as<int>();
    }
}

 

再者为收到一个目标数据后,计算偏差。

 

void ObjectDeviation::BoundingBoxSub(const darknet_ros_msgs::BoundingBoxes msg){
    deviation_msg.header.stamp = ros::Time::now();
    deviation_msg.point.x = (msg.bounding_boxes[0].xmin + msg.bounding_boxes[0].xmax) / 2.0 - image_width / 2.0;
    deviation_msg.point.y = (msg.bounding_boxes[0].ymin + msg.bounding_boxes[0].ymax) / 2.0 - image_height / 2.0;
    deviation_pub.publish(deviation_msg);
}

 

 

二、PID跟踪控制

输入输出:

 

2

 

监听topic:/cabin_vision/deviation,格式geometry_msgs::PointStamped,目标中心像素点与图片中心像素点偏差。

发布topic:/command/netLoad,格式cabin_msgs::netLoad,作用于机器人上的力及转矩,直接作为基础运动模块的输入;

直接看代码cabin_behaviors/pid_tracking.cpp。对于偏差的处理,如下图:

 

3

 

设定了一个区域,当机器人中心位于该区域时,即偏差量小于safe_area_x或safe_area_y时,PID控制器在相应方向输出的控制量为0。PID控制器随手写了一个简易的,对应代码:

 

double PIDTracking::PIDController(double err, double pid_p, double pid_i, double pid_d, double last_err, double *integral, double safe_area){
    double u = 0.0;
    //double t = *integral;
    if(abs(err) > safe_area){
        *integral += err;
        //t += err;
        //u = pid_p * err + pid_i * t + pid_d * (err - last_err);
        u = pid_p * err + pid_i * (*integral) + pid_d * (err - last_err);
        last_err = err;
        //*integral = t;
    }
    return u;
}

 

剩下的是ros下借助rqt动态调参工具的配置,包括pid参数,安全区域大小,跟踪模式开关:

 

//Callback for dynamic reconfigure
void PIDTracking::DynamicReconfigCallback(cabin_behaviors::PIDTrackingConfig &config, uint32_t levels){
    pid_p_mz = config.pid_p_mz;
    pid_i_mz = config.pid_i_mz;
    pid_d_mz = config.pid_d_mz;
    pid_p_z = config.pid_p_z;
    pid_i_z = config.pid_i_z;
    pid_d_z = config.pid_d_z;
    safe_area_x = config.safe_area_x;
    safe_area_y = config.safe_area_y;
 
    tracking_switch = config.tracking_switch;
}

 

 

实际在调试时没有太追求效果,只用了比例控制,即将积分i和微分d的参数都设为0。

 

自此,整个demo软件框架所有模块基本都介绍完毕。

发表评论

后才能评论