技术分享避坑指南-⽆⼈机⾃主降落代码解析
前⾔ 本主要讲解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部分。
各地名小吃
1、降落点识别 Prometheus/Modules/object_detection/cpp_nodes/landpad_det.cpp
输⼊:
图像数据: ⽤于识别降落点。
开关: ⽤于控制是否进⾏识别(暂时定主⽆⼈机)。
输出:
图像数据: 将检测结果画在原始图⽚上。
位置数据: 降落点在相机坐标系下的位置等信息。
Debug信息。
形容比赛的成语流程:
获取数据。
调⽤ArUco Marker库识别对象，获得识别到Marker(⼆维码)的四个⾓位置, Marker ID对
筛选⼀个最好的Marker。
计算降落点: 计算, Marker对于相机坐标系的旋转矩阵, 以及Marker中⼼点在相机坐标系的坐标。
⽬标数据发布: 转化为prometheus_msgs::DetectionInfo格式的数据发布。
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; 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::estimatePoSingleMarkers(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);
6个Maker下, 计算旋转矩阵 --> 降落点(相机坐标系为基)
if (19 == markerids[tt] || 43 == markerids[tt]) { id_to8_t[0] = 0.; id_to8_t[1] = 0.; id_to8_t[2] = 0.; } el 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.; } el 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.; } el 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.; } el 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_vertTo(id_to8_t_mat, CV_32FC1);
vertTo(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号的右下⾓去了,⽽不会在左上⾓的中⼼了,
el 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];
