延续之前的完整回路测试实验,将NUC与摄像头粘贴到无人机上,并安装桨叶尝试运行offb文件
查漏补缺
首先对于里程计配置进行补充和修改,在相机外参中可以设置output文件夹,其中可以导出vins自动修正过的外参。即一开始相机的外参只需要大致正确,然后启动里程计,拿着无人机在无人机场地中缓慢地绕圈最后放回原处,然后将vins自动修正的外参粘贴至相机设置中。如此反复,直至当无人机放回原处时里程计的偏差在可以接受的范围内。
此外为了保证安全,需要通过QGC对飞控参数进行修改,对于VEL相关参数,设置最大速度为0.5m/s
最后,之前的offb文件也需要进行修改。因为通过设定位置来控制无人机的话,一开始无人机检测到距离目标点的差距过大,可能使用最大加速度来冲向目标点。改为使用渐进的方式来起飞和降落。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
|
#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h>
mavros_msgs::State current_state; geometry_msgs::PoseStamped current_local_pose;
void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; }
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){ current_local_pose = *msg; }
int main(int argc, char **argv) { ros::init(argc, argv, "offb_node"); ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb); ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("mavros/local_position/pose", 10, pose_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10); ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
ros::Rate rate(20.0);
ROS_INFO("\033[1;34mConnecting to FCU...\033[0m"); while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } ROS_INFO("\033[1;32mFCU Connected\033[0m");
geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 0;
for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); }
const float target_z = 1.0; const float takeoff_dist = 0.2; const float land_dist = 0.05; bool is_landing = false; bool timer_started = false; ros::Time mission_start_time; ros::Time last_request = ros::Time::now(); ros::Time last_print = ros::Time::now();
mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true;
ROS_INFO("\033[1;32mStarting Main Loop\033[0m");
while(ros::ok()){ if (ros::Time::now() - last_print > ros::Duration(4.0)) { ROS_INFO("Current Mode: %s, Armed: %d, Actual_Z: %.2f", current_state.mode.c_str(), current_state.armed, current_local_pose.pose.position.z); last_print = ros::Time::now(); }
if(timer_started && !current_state.armed){ ROS_INFO("\033[1;32mDisarm Detected. Mission Accomplished! Exiting...\033[0m"); return 0; }
if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if(!is_landing){ ROS_INFO("\033[1;34mAttempting to switch to OFFBOARD mode...\033[0m"); if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("\033[1;32mOffboard enabled\033[0m"); } else { ROS_ERROR("\033[1;31mFailed to set OFFBOARD mode!\033[0m"); } last_request = ros::Time::now(); } } else { if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ ROS_INFO("\033[1;34mAttempting to ARM...\033[0m"); if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("\033[1;32mVehicle armed\033[0m"); } else { ROS_ERROR("\033[1;31mFailed to ARM!\033[0m"); } last_request = ros::Time::now(); } }
if(current_state.mode == "OFFBOARD" && current_state.armed){ if(!timer_started && current_local_pose.pose.position.z > (target_z - 0.05)){ mission_start_time = ros::Time::now(); timer_started = true; ROS_INFO("\033[1;32mReached target altitude. Hovering for 10 seconds...\033[0m"); }
if(timer_started && !is_landing && (ros::Time::now() - mission_start_time > ros::Duration(10.0))){ is_landing = true; ROS_INFO("\033[1;32mHovering finished. Initiating slow landing...\033[1;32m"); }
if(!is_landing){ float next_z = current_local_pose.pose.position.z + takeoff_dist; if(next_z > target_z) next_z = target_z; pose.pose.position.z = next_z; } else { float next_z = current_local_pose.pose.position.z - land_dist; if(next_z < 0.05) next_z = 0; pose.pose.position.z = next_z;
if(current_local_pose.pose.position.z < 0.20 && current_state.mode != "AUTO.LAND"){ if(ros::Time::now() - last_request > ros::Duration(2.0)){ mavros_msgs::SetMode land_set_mode; land_set_mode.request.custom_mode = "AUTO.LAND"; if(set_mode_client.call(land_set_mode) && land_set_mode.response.mode_sent){ ROS_WARN("\033[1;34mSwitching to AUTO.LAND...\033[0m"); } last_request = ros::Time::now(); } } } } else { pose.pose.position.z = 0; }
local_pos_pub.publish(pose);
ros::spinOnce(); rate.sleep(); }
return 0; }
|
然后最好先在仿真环境中运行节点。但要注意,仿真环境中能运行是最低要求,要放到真机运行可能还需要调整参数。
测试过程
阶段一
直接尝试运行起飞节点,发现电机声音很大,直接使用遥控器紧急断电。推测原因一是测试空间在狭窄室内,回声较大;第二是之前没有修改起飞逻辑,电机可能直接输出最大功率
于是在修改了起飞文件后,尝试先用遥控器控制无人机起飞,观察需要多大的推力,电机的声音有多大。但是即使最大输出,无人机也无法起飞,推测是电脑含有外壳过重。拆卸外壳后尝试重新起飞,但是仍然无法起飞,发现可能桨叶受损。
检查后发现是桨叶装配方向与电机转向相反,所以推力越大反而越不能起飞。修正后尝试用遥控器起飞,发现无人机起飞十分不稳定,会左右颠簸,于是尝试调整飞控。
参考高飞自主无人机教程 https://github.com/ZJU-FAST-Lab/Fast-Drone-250/tree/master 修改部分飞控参数。此外注意到一点,碳纤维板导电,NUC拆掉外壳后不能直接接触,最好是阻隔,故重新将外壳安装。
阶段二
再次尝试用遥控器起飞无人机,结果发现起飞后无人机会直给自己“摔”一圈,怀疑是电机推力不平衡。结果检查后发现一件之前没有注意过的事:在QGC中设置传感器旋转方向为180°并不能将飞行方向调整180°;即事实上,无人机的飞行方向由电机的连线决定(物理方向),当前结构下,1号电机一定在无人机飞行方向的右前方;而传感器中所设置的方向是飞控安装角度(飞控方向)与物理方向的差值;而最终摄像头的角度只能遵守物理方向。故当该角度设定有问题时,例如无人机一旦想要增大右侧电机推力,但接受的是左侧电机的数据,就会一直增大,导致瞬时翻滚。
发现px4飞控与电调之间有一个可以调整电机序号的接口,尝试更改序号后修改电机转向。结果使用dshot修改电机转向存在问题,无法保存。而电机转向是根据与电调的连线(焊接)决定的,不好修改。因此只能在既有的电机转向上修改电机的序号。
阶段三
起飞节点尝试切换无人机飞行模型,但是一直是ALTCTL,且查看/mavros/local_position/pose发现其中xy恒等于0,然后z轴发散,但是里程计读数正常。于是可知问题出现在飞控的EKF2数据融合。
使用QGC检查EKF2相关参数,发现是EKF2_AID_MASK(使用什么数据进行融合)值为1,选择的是GPS而不是vision。修改EKF2_AID_MASK=28(使用视觉、yaw、imu),EKF2_HDG_MODE(初始位置信息源)=vision(不使用气压计)。
再次运行,发现里程计正常,且local position的xyz轴都有正常读数,但是z轴初始在负值(-1.7)。需检查HDG_MODE是否还是气压计。
更改为vision后再次运行,发现z轴读数只有厘米级误差,显示正常。但是当多次用一个launch文件启动所有节点时,发现读数有时会发散。查阅后得知,launch文件不保证启动顺序,于是vins可能在摄像头之前启动,导致读数异常。改为用bash脚本启动多个launch文件,每个launch文件最好只负责一个功能,且内部没有先后顺序。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57
| #!/bin/bash
echo -e "\e[1;34m[INFO] Activating Mavros...\033[0m" roslaunch mavros px4.launch gcs_url:=udp://@192.168.8.50 &
until rostopic echo /mavros/state -n 1 2>/dev/null | grep -q "connected: True"; do echo -e "\e[1;34m[INFO] Waiting for FCU connection...\e[0m" sleep 1 done echo -e "\e[1;32m[INFO] FCU connected!\e[0m"
echo -e "\e[1;34m[INFO] Starting camera...\e[0m" roslaunch vins d435_run.launch &
until rostopic echo /camera/infra1/image_rect_raw -n 1 > /dev/null 2>&1;do echo -e "\e[1;34m[INFO] Waiting for camera stream...\e[0m" sleep 1 done echo -e "\e[1;32m[INFO] Camera stream detected!\e[0m"
echo -e "\e[1;34m[INFO] Starting VINS-Fusion...\e[0m" roslaunch vins vins_run.launch &
until rostopic echo /vins_node/odometry -n 1 > /dev/null 2>&1;do echo -e "\e[1;34m[INFO] Waiting for VINS to initialize...\e[0m" sleep 1 done echo -e "\e[1;32m[INFO] VINS initialized!Odometry is ready\e[0m"
echo -e "\e[1;34m[INFO] Starting transfer node...\e[0m" rosrun offb vins2mavros.py &
until rostopic echo /mavros/vision_pose/pose -n 1 >/dev/null 2>&1;do echo -e "\e[1;34m[INFO] Waiting for transfer node to publish data...\e[0m" sleep 1 done
echo -e "\e[1;34m[INFO] Setting origin node\e[0m" rosrun offb set_origin.py &
echo -e "\e[1;34m[INFO] Resetting PX4 EKF2 estimator...\e[0m" rosservice call /mavros/cmd/command "{command: 241, param1: 1, param2: 0, param3: 0, param4: 0, param5: 0, param6: 0, param7: 0}"
echo -e "\e[1;32m[INFO] All system started,ready to fly!\e[0m"
wait
|
抱着无人机进行绕圈检查里程计误差。发现里程计数据误差小,但是local position误差大,于是在QGC中减小EKF2_EVP_NOISE EKF2_EVV_NOISE EKF2_EVA_NOISE,让飞控更相信视觉信息。同样将EKF2_EV_DELAY也缩小,因为是有线连接,所以可以在20~50ms。
此外,修改EKF2_MAG_TYPE=0,SYS_HAS_MAG=0,不在室内环境启用磁力计
最后尝试启动offb_node节点,观察到无人机成功切换模式并解锁,但是解锁后没有起飞。发现当目标位置太低(当前高度+0.15)时在起飞之前无人机就会上锁,修改到+0.2后成功起飞并降落。
观察到无人机的水平方向上基本没有位移,且起飞降落非常稳定;而降落时目标点为-0.05稍微有些慢,但是不影响。但是最后切换到AUTO.LAND后