Skip to content

自主降落

Jump to bottom
Ren Jin edited this page Mar 1, 2023 · 3 revisions

自主降落

本教程的目的：

  • 理解二维码识别程序背后的原理
  • 掌握降落板识别程序，并能够进行二次开发
  • 理解追踪的原理，运动目标的状态估计的原理
  • 掌握自主降落程序，并能够进行二次开发

识别算法介绍

landpad_det.cpp

  • 订阅：

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

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

  • 发布：

检测结果话题/prometheus/object_detection/landpad_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

注意修改参数landpad_det降落板边长(默认0.6m)，0.6m包括边缘白色部分，黑色部分边长为0.4m

  • 算法识别流程

降落板示例：

GykHsK.png

Aruco模块主要基于ArUco library，是一个二维码识别中被广泛使用的库。

一个ArUco标记外围都有一组黑色边框，同时内部有着确定该标记ID的二维矩阵组合而成。黑色的边框能加速标记在图像中的检测速度，内部的二维编码能唯一识别该标记，同时进行错误检测和错误修复。

  • 注意：

二维码的样图可以从SpirePan下载

自主降落算法介绍

autonomous_landing.cpp

  • 订阅降落板识别检测结果，即相对位置 ros::Subscriber landpad_det_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10, landpad_det_cb);

  • 执行追踪算法及追踪策略（请阅读源码进行学习）

  • 发布上层控制指令

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

  • 参数Thres_vision为视觉丢失阈值，参数distance_thres为判断是否直接上锁距离阈值，参数landing_pad_height为降落板高度，参数use_ukf表示是否使用UKF目标状态进行滤波估计，moving_target表示目标是静止或者移动，kpx_land/kpy_land/kpz_land为控制参数，start_point_x/start_point_y/start_point_z为起始点位置

关于坐标系转换的说明：

  • 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
  • 首先，从相机坐标系转换至机体坐标系（从机体往前看，物体在相机前方x为正，左方y为正，上方z为正）：由于此demo相机朝下安装，且xy方向无偏移量 pos_body_frame[0] = - Detection_raw.position[1]; pos_body_frame[1] = - Detection_raw.position[0]; pos_body_frame[2] = - Detection_raw.position[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;
  • 从与机体固连的ENU系转换至ENU系（原点位于起飞点，x轴指向yaw=0的方向，y轴指向yaw=90的方向，z轴指向上的坐标系） Detection_ENU.position[0] = drone_pos[0] + pos_body_enu_frame[0]; Detection_ENU.position[1] = drone_pos[1] + pos_body_enu_frame[1]; Detection_ENU.position[2] = drone_pos[2] + pos_body_enu_frame[2];

仿真流程

  • 运行启动脚本(静止目标) roslaunch prometheus_gazebo sitl_landing.launch

  • 运行启动脚本(移动目标)

    roslaunch prometheus_gazebo sitl_landing_moving_target.launch

  • 启动后，输入1启动自动降落任务，飞机将起飞至起始点 GD54l8.md.png

  • 对于移动目标，待飞机起飞完毕后，新开一个窗口运行以下指令控制小车移动 rosrun prometheus_gazebo move_landing_pad.py

  • 飞机将持续读取图像信息，结果可在终端查看，并可与真值进行比较。如丢失图像信息，飞机将会悬停在当前点 GD5hSf.md.png

  • 当飞机与降落板位置小于阈值时，飞机将自动上锁，完成任务 GD52FI.md.png

测试视频1 测试视频1

如何进行真机实验？

相机购买

相机标定

# 首先启动相机节点，如下命令启动相机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，例如参数如下：

  • 标定板样张如下

降落标识打印

我们使用的淘宝打印店铺（仅供参考），https://saikeyou.tmall.com/shop/view_shop.htm?spm=a230r.1.14.5.6b6e17e5jOudLm&user_number_id=1673324284

打印样张如下：

距离估计精度测试（地面实验）

在A4纸打印标识如下：

如图所示，降落版边长为0.188m。

执行指令：

## 请根据自己的相机节点选择
roslaunch prometheus_detection web_cam0.launch
## 运行检测节点
roslaunch prometheus_detection landpad_det.launch
## 查看检测效果（可视化）
rqt_image_view

距离估计精度实验结果如下：

可以看出，距离误差在为距离的百分之7左右。

降落版边长为0.188m时，最远检测距离为2.65m，最近检测距离小于0.01m。

实际试验测试视频

video1

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

Clone this wiki locally