无人机最大航向速度怎么设置？技术分享避坑指南-无人机自主降落代码解析

无人机最大航向速度怎么设置？技术分享避坑指南-无人机自主降落代码解析流程:输出:网址： https://www.bilibili.com/video/BV1v4411H7ez?p=11、降落点识别 Prometheus/Modules/object_detection/cpp_nodes/landpad_det.cpp输入:

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

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

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

重点关注在降落点识别模块 即 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字典选择以及旋转向量和评议向量初始化 Ptrcv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(10) //------------------调用ArUco Marker库对图像进行识别-------------- // markerids存储每个识别到二维码的编号 markerCorners每个二维码对应的四个角点的像素坐标 std::vector markerids markerids_deted; vector<vector> markerCorners markerCorners_deted rejectedCandidate; Ptrcv::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 提高降落位置精度。

if (markerids_deted.size() > 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (19 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } if (markerids.size() == 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (43 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } } if (markerids.size() == 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (1 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } } if (markerids.size() == 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (2 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } } if (markerids.size() == 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (3 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } } if (markerids.size() == 0) { for (int tt = 0; tt < markerids_deted.size(); tt ) { if (4 == markerids_deted[tt]) { markerids.push_back(markerids_deted[tt]); markerCorners.push_back(markerCorners_deted[tt]); } } } }

** 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时会远离飞行 而不会往中间飞。

上图红色为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];

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

• End -

技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把机器人行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在机器人行业，就请关注我们的公众号，我们将持续发布机器人行业最有价值的信息和技术。 阿木实验室致力于前沿IT科技的教育和智能装备，让机器人研发更高效！