網(wǎng)上有很多關(guān)于pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表,無(wú)人機(jī)自主降落代碼解析的知識(shí)，也有很多人為大家解答關(guān)于pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表的問(wèn)題，今天pos機(jī)之家(www.mxllmx.com)為大家整理了關(guān)于這方面的知識(shí)，讓我們一起來(lái)看下吧!
本文目錄一覽：
1、pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表
pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表
前言本主要講解promtheus仿真環(huán)境中靜態(tài)目標(biāo)的自主降落, 涉及整體邏輯, 識(shí)別降落點(diǎn), 坐標(biāo)系變換. 不會(huì)涉及仿真環(huán)境搭建。本人之前的屬于純作計(jì)算機(jī)視覺(jué)工作的, 如果你和我一樣在此之前沒(méi)有接觸過(guò)機(jī)器人控制, 無(wú)人機(jī)相關(guān)的內(nèi)容, 那這篇文章對(duì)于入門prometheus的目標(biāo)檢測(cè)模塊很適合, 視覺(jué)方面簡(jiǎn)單(opencv 寫好的接口), 控制方面簡(jiǎn)單但全面。剛開(kāi)始接觸這方面知識(shí), 如有錯(cuò)誤請(qǐng)指正。
launch地址:Simulator/gazebo_simulator/launch_detection/sitl_landing_static_target.launch
promtheus自主降落-靜態(tài)目標(biāo)-仿真環(huán)境靜態(tài)目標(biāo)自主降落的代碼有3個(gè)部分組成仿真環(huán)境, 降落點(diǎn)識(shí)別, 控制邏輯組成。重點(diǎn)關(guān)注在降落點(diǎn)識(shí)別模塊, 即prometheus_detection的landpad_det, 其次是邏輯控制prometheus_mission的autonomous_landing, 對(duì)于仿真環(huán)境部分為公有模塊暫時(shí)忽略。
旋轉(zhuǎn)矩陣, 坐標(biāo)系變換不熟悉的強(qiáng)烈建議先看臺(tái)大機(jī)器人學(xué)之運(yùn)動(dòng)學(xué)——林沛群的P2-P16部分。
網(wǎng)址：https://www.bilibili.com/video/BV1v4411H7ez?p=1
1、降落點(diǎn)識(shí)別Prometheus/Modules/object_detection/cpp_nodes/landpad_det.cpp
輸入:
圖像數(shù)據(jù): 用于識(shí)別降落點(diǎn)。開(kāi)關(guān): 用于控制是否進(jìn)行識(shí)別(暫時(shí)定主無(wú)人機(jī))。輸出:
圖像數(shù)據(jù): 將檢測(cè)結(jié)果畫在原始圖片上。位置數(shù)據(jù): 降落點(diǎn)在相機(jī)坐標(biāo)系下的位置等信息。Debug信息。流程:
獲取數(shù)據(jù)。調(diào)用ArUco Marker庫(kù)識(shí)別對(duì)象,獲得識(shí)別到Marker(二維碼)的四個(gè)角位置, Marker ID對(duì)篩選一個(gè)最好的Marker。計(jì)算降落點(diǎn): 計(jì)算, Marker對(duì)于相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣, 以及Marker中心點(diǎn)在相機(jī)坐標(biāo)系的坐標(biāo)。目標(biāo)數(shù)據(jù)發(fā)布:轉(zhuǎn)化為prometheus_msgs::DetectionInfo格式的數(shù)據(jù)發(fā)布。** 1.1 ArUco Marker**官方: OpenCV: Detection of ArUco Markers網(wǎng)址：https://docs.opencv.org/4.5.3/d5/dae/tutorial_aruco_detection.html
1.1.1 獲取Marker的id, 坐標(biāo)// ArUco Marker字典選擇以及旋轉(zhuǎn)向量和評(píng)議向量初始化Ptrcv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(10)//------------------調(diào)用ArUco Marker庫(kù)對(duì)圖像進(jìn)行識(shí)別--------------// markerids存儲(chǔ)每個(gè)識(shí)別到二維碼的編號(hào) markerCorners每個(gè)二維碼對(duì)應(yīng)的四個(gè)角點(diǎn)的像素坐標(biāo)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) 獲取一個(gè) Marker_id --> Marker 字典. 參數(shù)(10)表示獲取的那個(gè)字典, 不同的字典的區(qū)別在于Marker的大小不同。cv::aruco::DetectorParameters::create() 獲取默認(rèn)的識(shí)別器識(shí)別參數(shù), 比如圖像二值化閾值等。cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate)。a）img 要識(shí)別的圖像。b) dictionary 和 parameters 上面定義的。c)markerCorners_deted 保存Marker識(shí)別結(jié)果四個(gè)點(diǎn)的圖片坐標(biāo)系的坐。d)markerids_deted 與 markerCorners_deted 一一對(duì)應(yīng)的id。e)rejectedCandidate 形狀和Marker相似但不是Marker, 結(jié)構(gòu)和markerCorners_deted一樣。1.1.2 計(jì)算旋轉(zhuǎn)向量, 轉(zhuǎn)移向量旋轉(zhuǎn)向量: 用于表示Marker在相機(jī)坐標(biāo)系的姿態(tài)偏移向量: 用于表示Marker在相機(jī)坐標(biāo)系的位置aruco::estimatePoseSingleMarkers(markerCornersONE, landpad_det_len * 0.133334, camera_matrix, distortion_coefficients, rvecs, tvecs);
第一個(gè)參數(shù)(MarkerCornersONE): Marker 四個(gè)角的坐標(biāo)(圖片坐標(biāo)系為基)。第二個(gè)參數(shù)(landpad_det_len * ....): Marker的實(shí)際大小。第三, 四個(gè)參數(shù)(camera_matrix, distortion_coefficients)為相機(jī)的參數(shù), 相機(jī)畸變參數(shù)。最后兩個(gè)參數(shù)為輸出, 旋轉(zhuǎn)向量, 偏移向量(以相機(jī)坐標(biāo)系為基)。*1.2 Marker 篩選 *降落板,以及每個(gè)Marker對(duì)應(yīng)的id, 程序每次只處理一個(gè)Marker, 如果同時(shí)檢測(cè)到多個(gè)Marker則各個(gè)Marker的優(yōu)先級(jí)為: 43 --> 1,2,3,4 --> 19; 理想情況下在遠(yuǎn)處的無(wú)人機(jī)會(huì)最先發(fā)現(xiàn)最大的Marker 19, 然后檢測(cè)到1,2,3,4 Marker調(diào)整位置, 最后檢測(cè)到最小的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 計(jì)算降落點(diǎn)**旋轉(zhuǎn)向量 --> 旋轉(zhuǎn)矩陣 --> 旋轉(zhuǎn)四元數(shù)
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個(gè)Maker下, 計(jì)算旋轉(zhuǎn)矩陣 --> 降落點(diǎn)(相機(jī)坐標(biāo)系為基)
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); 旋轉(zhuǎn)向量 --> 旋轉(zhuǎn)矩陣 + 偏移向量// id_to8_mat 定位中心轉(zhuǎn)換到紙面中心// rotation_matrix * id_to8_t_mat 在Marker為基的坐標(biāo)系下的坐標(biāo)乘上,旋轉(zhuǎn)向量等于在以相機(jī)坐標(biāo)系下為基的坐標(biāo)cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat;// cv::Mat id_8_t = vec_t_mat;
最開(kāi)始, 我一沒(méi)有想明白 cv::Mat id_8_t = rotation_matrix * id_to8_t_mat + vec_t_mat id_to8_t_mat為什么前面沒(méi)有負(fù)號(hào), 如果沒(méi)有負(fù)號(hào), 無(wú)人機(jī)在看到1,2,3,4時(shí)會(huì)遠(yuǎn)離飛行, 而不會(huì)往中間飛。
上圖紅色為x軸, 綠色為y軸皆指向正方向. 以右下角4號(hào)Marker為例子, id_to8_t_mat為正時(shí), 計(jì)算得到的id_8_t不應(yīng)該在4號(hào)的右下角去了, 而不會(huì)在左上角的中心了,
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;
直到看到 , "相機(jī)是朝向下方" 以及以下文字。"最后的坐標(biāo)是要換算在機(jī)體坐標(biāo)下的, 而不是在相機(jī)坐標(biāo)系之下。”
關(guān)于坐標(biāo)系轉(zhuǎn)換的說(shuō)明：識(shí)別算法發(fā)布的目標(biāo)位置位于相機(jī)坐標(biāo)系（從相機(jī)往前看，物體在相機(jī)右方x為正，下方y(tǒng)為正，前方z為正） 首先，從相機(jī)坐標(biāo)系轉(zhuǎn)換至機(jī)體坐標(biāo)系（從機(jī)體往前看，物體在相機(jī)前方x為正，左方y(tǒng)為正，上方z為正）：由于此demo相機(jī)朝下安裝，且xy方向無(wú)偏移量。
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、控制邏輯主要輸入:
鍵盤控制指令降落點(diǎn)坐標(biāo)(相機(jī)坐標(biāo)軸下): prometheus_msgs::DetectionInfo無(wú)人機(jī)當(dāng)前狀態(tài) prometheus_msgs::DroneState主要輸出:
無(wú)人機(jī)控制數(shù)據(jù)無(wú)人機(jī)共有4種狀態(tài)enum EXEC_STATE{ WAITING_RESULT, TRACKING, LOST, LANDING,};
初始時(shí)為WATING_RESULT狀態(tài), 等待降落點(diǎn)識(shí)別模塊找到降落點(diǎn), 找到降落點(diǎn)后進(jìn)入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)下, 如果當(dāng)前不再懸停指令下且沒(méi)有再找到降落點(diǎn)則轉(zhuǎn)為L(zhǎng)OST狀態(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)下, 如果機(jī)體離降落點(diǎn)的距離(歐式距離)小于閾值, 或則飛行高度過(guò)低, 進(jìn)入LANDING狀態(tài)。
// 抵達(dá)上鎖點(diǎn),進(jìn)入LANDINGdistance_to_pad = landpad_det.pos_body_enu_frame.norm();//達(dá)到降落距離，上鎖降落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;}//達(dá)到最低高度，上鎖降落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)下, 如果未滿足進(jìn)入LANDING的條件, 則以機(jī)體距離降落點(diǎn)的距離設(shè)置的一定比例設(shè)置飛機(jī)的數(shù)據(jù), 即機(jī)體離目標(biāo)越遠(yuǎn)速度越快, 越近降落點(diǎn)速度越慢（機(jī)體慣性坐標(biāo)系下）
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];}
// 如果目標(biāo)也在移動(dòng), 則加上目標(biāo)的速度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)下, 機(jī)體原地向上飛行, 嘗試找到降落點(diǎn). 如果機(jī)體的高度在達(dá)到閾值高度仍然未找到目標(biāo), 則判定為定點(diǎn)降落失敗, 并進(jìn)入LANDING。
2.1 坐標(biāo)系變換從降落點(diǎn)識(shí)別模塊獲得降落點(diǎn)坐標(biāo)是基于相機(jī)坐標(biāo)系的, 需要處理轉(zhuǎn)換為機(jī)體坐標(biāo)系, 慣性坐標(biāo)系下的點(diǎn)。
相機(jī)坐標(biāo)系 --> 機(jī)體坐標(biāo)系: camera_offset是相機(jī)距離機(jī)體質(zhì)心的偏移量. 對(duì)于機(jī)體來(lái)說(shuō)機(jī)頭方向?yàn)閤為正, 機(jī)體左邊為y為正, 機(jī)體上方z為正。
// 識(shí)別算法發(fā)布的目標(biāo)位置位于相機(jī)坐標(biāo)系（從相機(jī)往前看，物體在相機(jī)右方x為正，下方y(tǒng)為正，前方z為正）// 相機(jī)安裝誤差 在mission_utils.h中設(shè)置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];
機(jī)體系 -> 機(jī)體慣性系 (原點(diǎn)在機(jī)體的慣性系) (對(duì)無(wú)人機(jī)姿態(tài)進(jìn)行解耦): R_Body_to_ENU, 機(jī)體坐標(biāo)系到慣性坐標(biāo)系的轉(zhuǎn)移矩陣, 有飛機(jī)當(dāng)前姿態(tài)(歐拉角) --> 轉(zhuǎn)為旋轉(zhuǎn)矩陣。
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;
}
機(jī)體慣性系 --> 慣性系: 機(jī)體質(zhì)心到慣性坐標(biāo)系原點(diǎn)的偏移量。
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é)尾最后作為這方面剛?cè)腴T者, 總結(jié)下在閱讀這部分代碼時(shí)所踩的坑, 先簡(jiǎn)單過(guò)一遍代碼, 忽略細(xì)節(jié)了解邏輯, 大概了解代碼那些可以當(dāng)作黑盒使用, 那些是需要深入的。對(duì)于需要深入的且之前未曾接觸過(guò)的不要一來(lái)就直接看文章, 要先看相關(guān)視頻。遇到和自己看法不同的代碼, 先忽略往后面看代碼有些時(shí)候答案就藏在后面, 如果還是為解決就再仔細(xì)閱讀一遍代碼相關(guān)的文章介紹。
End -技術(shù)發(fā)展的日新月異，阿木實(shí)驗(yàn)室將緊跟技術(shù)的腳步，不斷把機(jī)器人行業(yè)最新的技術(shù)和硬件推薦給大家?？吹浇?jīng)過(guò)我們培訓(xùn)的學(xué)員在技術(shù)上突飛猛進(jìn)，是我們培訓(xùn)最大的價(jià)值。如果你在機(jī)器人行業(yè)，就請(qǐng)關(guān)注我們的公眾號(hào)，我們將持續(xù)發(fā)布機(jī)器人行業(yè)最有價(jià)值的信息和技術(shù)。阿木實(shí)驗(yàn)室致力于前沿IT科技的教育和智能裝備，讓機(jī)器人研發(fā)更高效！
以上就是關(guān)于pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表,無(wú)人機(jī)自主降落代碼解析的知識(shí)，后面我們會(huì)繼續(xù)為大家整理關(guān)于pos機(jī)錯(cuò)誤代碼對(duì)應(yīng)表的知識(shí)，希望能夠幫助到大家！
