技术分享 | 避坑指南-无人机自主降落代码解析

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

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

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

file

重点关注在降落点识别模块, 即
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 dictionary = cv::aruco::getPredefinedDictionary(10)
//------------------调用ArUco Marker库对图像进行识别--------------
// markerids存储每个识别到二维码的编号 markerCorners每个二维码对应的四个角点的像素坐标
std::vector markerids, markerids_deted;
vector> markerCorners, markerCorners_deted, rejectedCandidate;
Ptr 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 提高降落位置精度。

file

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

file

上图红色为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 < 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 < 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 < 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 < 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科技的教育和智能装备，让机器人研发更高效！