網上有很多關于pos機錯誤代碼對應表,無人機自主降落代碼解析的知識，也有很多人為大家解答關于pos機錯誤代碼對應表的問題，今天pos機之家(www.dsth100338.com)為大家整理了關于這方面的知識，讓我們一起來看下吧!
本文目錄一覽：
pos機錯誤代碼對應表
前言本主要講解promtheus仿真環(huán)境中靜態(tài)目標的自主降落, 涉及整體邏輯, 識別降落點, 坐標系變換. 不會涉及仿真環(huán)境搭建。本人之前的屬于純作計算機視覺工作的, 如果你和我一樣在此之前沒有接觸過機器人控制, 無人機相關的內容, 那這篇文章對于入門prometheus的目標檢測模塊很適合, 視覺方面簡單(opencv 寫好的接口), 控制方面簡單但全面。剛開始接觸這方面知識, 如有錯誤請指正。
launch地址:Simulator/gazebo_simulator/launch_detection/sitl_landing_static_target.launch
promtheus自主降落-靜態(tài)目標-仿真環(huán)境靜態(tài)目標自主降落的代碼有3個部分組成仿真環(huán)境, 降落點識別, 控制邏輯組成。重點關注在降落點識別模塊, 即prometheus_detection的landpad_det, 其次是邏輯控制prometheus_mission的autonomous_landing, 對于仿真環(huán)境部分為公有模塊暫時忽略。
旋轉矩陣, 坐標系變換不熟悉的強烈建議先看臺大機器人學之運動學——林沛群的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中心點在相機坐標系的坐標。目標數據發(fā)布:轉化為prometheus_msgs::DetectionInfo格式的數據發(fā)布。** 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)表示獲取的那個字典, 不同的字典的區(qū)別在于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在相機坐標系的姿態(tài)偏移向量: 用于表示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的優(yōu)先級為: 43 --> 1,2,3,4 --> 19; 理想情況下在遠處的無人機會最先發(fā)現最大的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;
直到看到 , "相機是朝向下方" 以及以下文字。"最后的坐標是要換算在機體坐標下的, 而不是在相機坐標系之下?！?
關于坐標系轉換的說明：識別算法發(fā)布的目標位置位于相機坐標系（從相機往前看，物體在相機右方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無人機當前狀態(tài) prometheus_msgs::DroneState主要輸出:
無人機控制數據無人機共有4種狀態(tài)enum EXEC_STATE{ WAITING_RESULT, TRACKING, LOST, LANDING,};
初始時為WATING_RESULT狀態(tài), 等待降落點識別模塊找到降落點, 找到降落點后進入TRACKING狀態(tài)。
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狀態(tài)下, 如果當前不再懸停指令下且沒有再找到降落點則轉為LOST狀態(tài)。
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狀態(tài)下, 如果機體離降落點的距離(歐式距離)小于閾值, 或則飛行高度過低, 進入LANDING狀態(tài)。
// 抵達上鎖點,進入LANDINGdistance_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狀態(tài)下, 如果未滿足進入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狀態(tài)下, 機體原地向上飛行, 嘗試找到降落點. 如果機體的高度在達到閾值高度仍然未找到目標, 則判定為定點降落失敗, 并進入LANDING。
2.1 坐標系變換從降落點識別模塊獲得降落點坐標是基于相機坐標系的, 需要處理轉換為機體坐標系, 慣性坐標系下的點。
相機坐標系 --> 機體坐標系: camera_offset是相機距離機體質心的偏移量. 對于機體來說機頭方向為x為正, 機體左邊為y為正, 機體上方z為正。
// 識別算法發(fā)布的目標位置位于相機坐標系（從相機往前看，物體在相機右方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];
機體系 -> 機體慣性系 (原點在機體的慣性系) (對無人機姿態(tài)進行解耦): R_Body_to_ENU, 機體坐標系到慣性坐標系的轉移矩陣, 有飛機當前姿態(tài)(歐拉角) --> 轉為旋轉矩陣。
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];
結尾最后作為這方面剛入門者, 總結下在閱讀這部分代碼時所踩的坑, 先簡單過一遍代碼, 忽略細節(jié)了解邏輯, 大概了解代碼那些可以當作黑盒使用, 那些是需要深入的。對于需要深入的且之前未曾接觸過的不要一來就直接看文章, 要先看相關視頻。遇到和自己看法不同的代碼, 先忽略往后面看代碼有些時候答案就藏在后面, 如果還是為解決就再仔細閱讀一遍代碼相關的文章介紹。
End -技術發(fā)展的日新月異，阿木實驗室將緊跟技術的腳步，不斷把機器人行業(yè)最新的技術和硬件推薦給大家。看到經過我們培訓的學員在技術上突飛猛進，是我們培訓最大的價值。如果你在機器人行業(yè)，就請關注我們的公眾號，我們將持續(xù)發(fā)布機器人行業(yè)最有價值的信息和技術。阿木實驗室致力于前沿IT科技的教育和智能裝備，讓機器人研發(fā)更高效！
以上就是關于pos機錯誤代碼對應表,無人機自主降落代碼解析的知識，后面我們會繼續(xù)為大家整理關于pos機錯誤代碼對應表的知識，希望能夠幫助到大家！
