新闻中心 新闻
技术分享 | 避坑指南-无人机自主降落代码解析
新闻 2021-11-08 14:08:02

前言

本主要讲解promtheus仿真环境中静态目标的自主降落, 涉及整体逻辑, 识别降落点, 坐标系变换. 不会涉及仿真环境搭建。本人之前的属于纯作计算机视觉工作的, 如果你和我一样在此之前没有接触过机器人控制, 无人机相关的内容, 那这篇文章对于入门prometheus的目标检测模块很适合, 视觉方面简单(opencv 写好的接口), 控制方面简单但全面。刚开始接触这方面知识, 如有错误请指正。

launch地址:
Simulator/gazebo_simulator/launch_detection/sitl_landing_static_target.launch


promtheus自主降落-静态目标-仿真环境

静态目标自主降落的代码有3个部分组成仿真环境, 降落点识别, 控制逻辑组成。

7.png


重点关注在降落点识别模块, 即

prometheus_detection的landpad_det, 
其次是逻辑控制
prometheus_mission
autonomous_landing, 
对于仿真环境部分为公有模块暂时忽略。

旋转矩阵, 坐标系变换不熟悉的强烈建议先看台大机器人学之运动学——林沛群P2-P16部分。

网址:
https://www.bilibili.com/video/BV1v4411H7ez?p=1

 1、降落点识别

Prometheus/Modules/object_detection/cpp_nodes/landpad_det.cpp

输入:

  • 图像数据: 用于识别降落点。

  • 开关: 用于控制是否进行识别(暂时定主无人机)。


输出:

  • 图像数据: 将检测结果画在原始图片上。

  • 位置数据: 降落点在相机坐标系下的位置等信息。

  • Debug信息。


流程:

  • 获取数据。

  • 调用ArUco Marker库识别对象,获得识别到Marker(二维码)的四个角位置Marker ID

  • 筛选一个最好的Marker。

  • 计算降落点: 计算, Marker对于相机坐标系的旋转矩阵, 以及Marker中心点在相机坐标系的坐标。

  • 目标数据发布:

    转化为prometheus_msgs::DetectionInfo格式的数据发布。

1.1 ArUco Marker

官方: OpenCV: Detection of ArUco Markers

网址:

https://docs.opencv.org/4.5.3/d5/dae/tutorial_aruco_detection.html


1.1.1 获取Marker的id, 坐标


// ArUco Marker字典选择以及旋转向量和评议向量初始化
Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(10)
//------------------调用ArUco Marker库对图像进行识别--------------
// markerids存储每个识别到二维码的编号 markerCorners每个二维码对应的四个角点的像素坐标
std::vector<int> markerids, markerids_deted;
vector<vector<Point2f>> markerCorners, markerCorners_deted, rejectedCandidate;
Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate);


  • cv::aruco::getPredefinedDictionary(10) 获取一个 Marker_id --> Marker 字典. 参数(10)表示获取的那个字典, 不同的字典的区别在于Marker的大小不同。

  • cv::aruco::DetectorParameters::create() 获取默认的识别器识别参数, 比如图像二值化阈值等。

  • cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate)。

a)img 要识别的图像。

b) dictionary 和 parameters 上面定义的。

c)markerCorners_deted 保存Marker识别结果四个点的图片坐标系的坐。

d)markerids_deted 与 markerCorners_deted 一一对应的id。

e)rejectedCandidate 形状和Marker相似但不是Marker, 结构和markerCorners_deted一样。



1.1.2 计算旋转向量, 转移向量

旋转向量: 用于表示Marker在相机坐标系的姿态
偏移向量: 用于表示Marker在相机坐标系的位置
aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len * 0.133334, camera_matrix, distortion_coefficients, rvecs, tvecs);
复制
  • 第一个参数(MarkerCornersONE): Marker 四个角的坐标(图片坐标系为基)。

  • 第二个参数(landpad_det_len * ....): Marker的实际大小。

  • 第三, 四个参数

    (camera_matrix, distortion_coefficients)为相机的参数, 相机畸变参数。

  • 最后两个参数为输出, 旋转向量, 偏移向量(以相机坐标系为基)。


1.2 Marker 筛选 

降落板,以及每个Marker对应的id, 程序每次只处理一个Marker, 如果同时检测到多个Marker则各个Marker的优先级为: 43 --> 1,2,3,4 --> 19; 理想情况下在远处的无人机会最先发现最大的Marker 19, 然后检测到1,2,3,4 Marker调整位置, 最后检测到最小的Marker 43 提高降落位置精度。

2.png

微信图片_20211108140522.png

微信图片_20211108140652.png

 1.3 计算降落点

旋转向量 --> 旋转矩阵 --> 旋转四元数

    cv::Mat rotation_matrix;
    cv::Rodrigues(rvecs[0], rotation_matrix);
    Eigen::Matrix3d rotation_matrix_eigen;
    cv::cv2eigen(rotation_matrix, rotation_matrix_eigen);
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_matrix_eigen);
    q.normalize();



    6个Maker下, 计算旋转矩阵 --> 降落点(相机坐标系为基)



    if (19 == markerids[tt] || 43 == markerids[tt])
    {
            id_to8_t[0] = 0.;
            id_to8_t[1] = 0.;
            id_to8_t[2] = 0.;
    }
    else if (1 == markerids[tt])
    {
            id_to8_t[0] = -(landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[1] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[2] = 0.;
    }
    else if (2 == markerids[tt])
    {
            id_to8_t[0] = -(landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[1] = -(landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[2] = 0.;
    }
    else if (3 == markerids[tt])
    {
            id_to8_t[0] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[1] = -(landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[2] = 0.;
    }
    else if (4 == markerids[tt])
    {
            id_to8_t[0] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[1] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[2] = 0.;
    }

    cv::Mat id_to8_t_mat{id_to8_t};
    id_to8_t_mat.convertTo(id_to8_t_mat, CV_32FC1);

    rotation_matrix.convertTo(rotation_matrix, CV_32FC1);
    // cv::invert(rotation_matrix, rotation_matrix); 旋转向量 --> 旋转矩阵 + 偏移向量
    // id_to8_mat 定位中心转换到纸面中心
    // rotation_matrix * id_to8_t_mat Marker为基的坐标系下的坐标乘上,旋转向量等于在以相机坐标系下为基的坐标
    cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;
    // cv::Mat id_8_t = vec_t_mat;


    最开始, 我一没有想明白 cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat id_to8_t_mat为什么前面没有负号, 如果没有负号, 无人机在看到1,2,3,4时会远离飞行, 而不会往中间飞。3.png


    上图红色为x轴, 绿色为y轴皆指向正方向. 以右下角4号Marker为例子, id_to8_t_mat为正时, 计算得到的id_8_t不应该在4号的右下角去了, 而不会在左上角的中心了,


    else if (4 == markerids[tt])
    {
            id_to8_t[0] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[1] = (landpad_det_len * 0.666667 + landpad_det_len * 0.133334) / 2.;
            id_to8_t[2] = 0.;
    }
    cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;



    直到看到 , "相机是朝向下方" 以及以下文字。"最后的坐标是要换算在机体坐标下的, 而不是在相机坐标系之下。”  


    关于坐标系转换的说明:识别算法发布的目标位置位于相机坐标系(从相机往前看,物体在相机右方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];



    2、控制逻辑

    主要输入:

    • 键盘控制指令

    • 降落点坐标(相机坐标轴下): prometheus_msgs::DetectionInfo

    • 无人机当前状态 prometheus_msgs::DroneState


    主要输出:

    • 无人机控制数据

    无人机共有4种状态


    enum EXEC_STATE
    {
        WAITING_RESULT,
        TRACKING,
        LOST,
        LANDING,
    };


    初始时为WATING_RESULT状态, 等待降落点识别模块找到降落点, 找到降落点后进入TRACKING状态。



    if(landpad_det.is_detected)
    {
        exec_state = TRACKING;
        message = "Get the detection result.";
        cout << message <<endl;
        pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
        break;
    }


    在TRACKING状态下, 如果当前不再悬停指令下且没有再找到降落点则转为LOST状态。


    if(!landpad_det.is_detected && !hold_mode)
    {
            exec_state = LOST;
            message = "Lost the Landing Pad.";
            cout << message <<endl;
            pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
            break;
    }   


    在TRACKING状态下, 如果机体离降落点的距离(欧式距离)小于阈值, 或则飞行高度过低, 进入LANDING状态。



    // 抵达上锁点,进入LANDING
    distance_to_pad = landpad_det.pos_body_enu_frame.norm();
    // 达到降落距离,上锁降落
    if(distance_to_pad < arm_distance_to_pad)
    {
            exec_state = LANDING;
            message = "Catched the Landing Pad.";
            cout << message <<endl;
            pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
            break;
    }
    // 达到最低高度,上锁降落
    else if(abs(landpad_det.pos_body_enu_frame[2]) < arm_height_to_ground)
    {
            exec_state = LANDING;
            message = "Reach the lowest height.";
            cout << message <<endl;
            pub_message(message_pub, prometheus_msgs::Message::WARN, NODE_NAME, message);
            break;
    }


    TRACKING状态下, 如果未满足进入LANDING的条件, 则以机体距离降落点的距离设置的一定比例设置飞机的数据, 即机体离目标越远速度越快, 越近降落点速度越慢, 机体惯性坐标系下

    Command_Now.header.stamp = ros::Time::now();
    Command_Now.Command_ID   = Command_Now.Command_ID + 1;
    Command_Now.source = NODE_NAME;
    Command_Now.Mode = prometheus_msgs::ControlCommand::Move;
    Command_Now.Reference_State.Move_frame = prometheus_msgs::PositionReference::ENU_FRAME;
    Command_Now.Reference_State.Move_mode = prometheus_msgs::PositionReference::XYZ_VEL;   //xy velocity z position

    for (int i=0; i<3; i++)
    {
            Command_Now.Reference_State.velocity_ref[i] = kp_land[i] * landpad_det.pos_body_enu_frame[i];
    }

    // 如果目标也在移动, 则加上目标的速度
    if(moving_target)
    {
            Command_Now.Reference_State.velocity_ref[0] += target_vel_xy[0];
            Command_Now.Reference_State.velocity_ref[1] += target_vel_xy[1];
    }
    复制

    LOST状态下, 机体原地向上飞行, 尝试找到降落点. 如果机体的高度在达到阈值高度仍然未找到目标, 则判定为定点降落失败, 并进入LANDING

    2.1 坐标系变换

    从降落点识别模块获得降落点坐标是基于相机坐标系的, 需要处理转换为机体坐标系, 惯性坐标系下的点.
    相机坐标系 --> 机体坐标系: camera_offset是相机距离机体质心的偏移量. 对于机体来说机头方向为x为正, 机体左边为y为正, 机体上方z为正.

    // 识别算法发布的目标位置位于相机坐标系(从相机往前看,物体在相机右方x为正,下方y为正,前方z为正)
    // 相机安装误差 在mission_utils.h中设置
    landpad_det.pos_body_frame[0] = -landpad_det.Detection_info.position[1] + camera_offset[0];
    landpad_det.pos_body_frame[1] = -landpad_det.Detection_info.position[0] + camera_offset[1];
    landpad_det.pos_body_frame[2] = -landpad_det.Detection_info.position[2] + camera_offset[2];
    复制

    机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦): R_Body_to_ENU, 机体坐标系到惯性坐标系的转移矩阵, 有飞机当前姿态(欧拉角) --> 转为旋转矩阵

    landpad_det.pos_body_enu_frame = R_Body_to_ENU * landpad_det.pos_body_frame;

    Eigen::Matrix3f get_rotation_matrix(float phi, float theta, float psi)
    {
        Eigen::Matrix3f R_Body_to_ENU;

        float r11 = cos(theta)*cos(psi);
        float r12 = - cos(phi)*sin(psi) + sin(phi)*sin(theta)*cos(psi);
        float r13 = sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi);
        float r21 = cos(theta)*sin(psi);
        float r22 = cos(phi)*cos(psi) + sin(phi)*sin(theta)*sin(psi);
        float r23 = - sin(phi)*cos(psi) + cos(phi)*sin(theta)*sin(psi);
        float r31 = - sin(theta);
        float r32 = sin(phi)*cos(theta);
        float r33 = cos(phi)*cos(theta);
        R_Body_to_ENU << r11,r12,r13,r21,r22,r23,r31,r32,r33;
        return R_Body_to_ENU;
    }
    复制

    机体惯性系 --> 惯性系: 机体质心到惯性坐标系原点的偏移量

    landpad_det.pos_enu_frame[0] = _DroneState.position[0] + landpad_det.pos_body_enu_frame[0];
    landpad_det.pos_enu_frame[1] = _DroneState.position[1] + landpad_det.pos_body_enu_frame[1];
    landpad_det.pos_enu_frame[2] = _DroneState.position[2] + landpad_det.pos_body_enu_frame[2];
    复制

    结尾

    最后作为这方面刚入门者, 总结下在阅读这部分代码时所踩的坑, 先简单过一遍代码, 忽略细节了解逻辑, 大概了解代码那些可以当作黑盒使用, 那些是需要深入的. 对于需要深入的且之前未曾接触过的不要一来就直接看文章, 要先看相关视频. 遇到和自己看法不同的代码, 先忽略往后面看代码有些时候答案就藏在后面, 如果还是为解决就再仔细阅读一遍代码相关的文章介绍.


    阿木实验室致力于前沿IT科技的教育和智能装备,让机器人研发更高效!


    公众号:阿木实验室 ( 领取免费资料包)

    官方淘宝店:阿木实验室(可购买硬件配件)

    硬件咨询:yanyue199506(欢迎洽谈合作)

    课程咨询:jiayue199506 (免费领取机器人工程师学习计划)


    - End -

    技术发展的日新月异,阿木实验室将紧跟技术的脚步,不断把机器人行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进,是我们培训最大的价值。如果你在机器人行业,就请关注我们的公众号,我们将持续发布机器人行业最有价值的信息和技术。

    您的浏览器版本过低,建议下载更换以下浏览器:

    官方店铺

     阿木实验室

    联系电话

    028-87872048

    扫一扫,快速加入

    硬件评测

    群号652692981

    课程学习

    群号652692981