Skip to content

圆形穿越

Jump to bottom
新一 edited this page Sep 16, 2020 · 2 revisions

圆形穿越

本教程的目的：

  • 理解椭圆识别程序背后的原理
  • 掌握圆形识别程序，并能够进行二次开发
  • 掌握圆形穿越程序，并能够进行二次开发

识别算法介绍

ellipse_det.cpp

  • 订阅：

真实环境中订阅的相机话题为/prometheus/camera/rgb/image_raw

仿真环境中订阅的相机话题为/P300_Monocular_front/Monocular/image_raw

  • 发布：

检测结果话题/prometheus/object_detection/ellipse_det (话题格式请参考Prometheus/Modules/msgs/msg/DetectionInfo.msg)

  • 配置文件

真实环境中的配置文件为Prometheus/Modules/object_detection/config/camera_param.yaml

仿真环境中的配置文件为Prometheus/Modules/object_detection/config/camera_param_gazebo_monocular.yaml

注意修改ellipse_det_r（圆的半径）

  • 识别流程

GyiuQK.png

  1. 利用高斯滤波做预处理。
  2. 边界检测部分用到了自适应Canny检测。
  3. 将边界分为凹弧和凸弧，根据输入参数筛选弧段。
  4. 利用弧段来估计椭圆参数，交叉验算得出椭圆中心点，计算出椭圆得分。
  5. 利用圆心、长短轴和旋转角度聚类。

圆形穿越算法介绍

circle_crossing.cpp

  • 订阅圆形识别检测结果，即相对位置

      ros::Subscriber ellipse_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/ellipse_det", 10, ellipse_det_cb);

  • 发布上层控制指令

      ros::Publisher command_pub = nh.advertise<prometheus_msgs::ControlCommand>("/prometheus/control_command", 10);

  • 执行状态机，输入1开始任务，当无人机与目标距离小于1米时，由于视场限制，无法探测到目标，无人机按照当前位置直接向前飞行，完成穿越，并返回初始位置。

关于坐标系转换的说明：

  • 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）

  • 首先，从相机坐标系转换至机体坐标系（从机体往前看，物体在相机前方x为正，左方y为正，上方z为正）：camera_offset为相机安装偏移量，此处为前置摄像头，参看P300_Monocular_front.sdf可知，相机安装于机体质心前方0.2米，下方0.05米，因此，camera_offset[0] = 0.2，camera_offset[1] = 0.0，camera_offset[2] = -0.05 pos_body_frame[0] = Detection_info.position[2] + camera_offset[0]; pos_body_frame[1] = - Detection_info.position[0] + camera_offset[1]; pos_body_frame[2] = - Detection_info.position[1] + camera_offset[2];

  • 从机体坐标系转换至与机体固连的ENU系（原点位于质心，x轴指向yaw=0的方向，y轴指向yaw=90的方向，z轴指向上的坐标系）：直接乘上机体系到惯性系的旋转矩阵即可 R_Body_to_ENU = get_rotation_matrix(_DroneState.attitude[0], _DroneState.attitude[1], _DroneState.attitude[2]);

      pos_body_enu_frame = R_Body_to_ENU * pos_body_frame;

仿真流程

  • 运行启动脚本 roslaunch prometheus_gazebo sitl_circle_crossing.launch
  • 输入1，飞机将起飞至预设点，等待任务开始 GrHnlq.md.png
  • 再次输入1，飞机将执行穿越任务，穿越后将返回起始点，等待下一次任务 GrHmpn.md.png

运行截图 GOFrAf.gif

如何进行真机实验？

相机购买

相机标定

# 首先启动相机节点，如下命令启动相机ID=0
roslaunch prometheus_detection web_cam0.launch
# 然后利用ros自带的标定程序对相机进行标定
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.0245 image:=/prometheus/camera/rgb/image_raw

  • 其中：size为标点板尺寸，square为每个方格宽度(m)，image:=相机话题

  • 棋盘格标定板下载地址：Chessboard

  • 将得到的参数写入如下文件(有关目标尺度的预定义也在这个文件中)：Prometheus/Modules/object_detection/config/camera_param.yaml，例如参数如下：

  • 标定板样张如下

感谢使用Prometheus自主无人机软件平台！

Clone this wiki locally