5_实机绕圈测试-场地搭建

Mian 发布于

前言

在完成了起飞与降落实验后想要让无人机在场地内绕行一圈，但是在进行实验之前对于安全保障与场地搭建有一些问题

首先是安全保障。目前的实验都是将无人机栓在重物上以避免无人机突然飞走，如果要完成绕行则需要新的安全保障措施。其次是关于场地，目前的场地中有两面是白色的墙壁，另外两面是窗户，这对于vins十分不友好，需要更新场地。

综合上述需求，在进行下一步的实机实验之前，最好搭建一个更安全更对于vins友好的实验场地。且最好不要有破坏性措施。

  1. 窗帘或者屏风遮挡窗户，避免光线不平衡和玻璃反光
  2. 增加纹理，使用拼色泡沫垫来铺地板和组装障碍物
  3. 安全措施，用旧枕头或者瑜伽砖放在墙下方

关于架设保护网，的确会增加安全性，但是相对来说会复杂很多

仿真测试

仿真中使用里程计

为了提高仿真的拟真度，尝试在仿真环境中使用里程计

技巧 不显示加载信息
例如roslaunch px4 mavros_posix_sitl.launch > /dev/null 2>&1

发现使用脚本启动gazebo然后在后台运行，例如 roslaunch px4 mavros_posix_sitl.launch &会出现gazebo闪退的情况，检查后发现是gazebo如果检测到在后台运行会自动停止。于是要么用虚拟后台，要么单独在一个终端中启动gazebo

经过测试后发现，在仿真环境中使用里程计比在现实中还要复杂，并且此时电脑的负载太高，一般仅在需要证明算法在“高保真”的环境中能成功运行时才使用，通常直接使用仿真器提供的真值或者加上一定的噪声的数据。

故目前放弃在仿真环境中使用里程计的想法。

绕圈飞行

仿真实验

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
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <cmath>
#include <vector>

// 状态定义
enum FlightState { TAKEOFF, FLYING, STAYING, LANDING, FINISHED };

const char* status_to_str(FlightState s) {
    switch(s) {
        case TAKEOFF: return "TAKEOFF";
        case FLYING:  return "FLYING";
        case STAYING: return "STAYING";
        case LANDING: return "LANDING";
        case FINISHED: return "FINISHED";
        default:      return "UNKNOWN";
    }
}

// 全局变量
mavros_msgs::State current_state;           // 当前飞行状态
geometry_msgs::PoseStamped current_pose;    // 当前位姿
geometry_msgs::PoseStamped last_pose;
bool first_pose = true;
bool emergency_brake = false; // 紧急制动标志

void state_cb(const mavros_msgs::State::ConstPtr& msg) { current_state = *msg; }
void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) { 
    if(!first_pose){
        float delta_d = std::sqrt(std::pow(msg->pose.position.x - last_pose.pose.position.x, 2) + 
                                  std::pow(msg->pose.position.y - last_pose.pose.position.y, 2));
        if(delta_d > 0.5){
            ROS_WARN("\033[1;31mVINS JUMP DETECTED! Emergency Landing...\033[0m");
            emergency_brake = true;
        }
    }
    last_pose = *msg;
    current_pose = *msg; 
    first_pose = false;
}


// 计算两点间的欧氏距离 (2D)
float get_dist_2d(float x1, float y1, float x2, float y2) {
    return std::sqrt(std::pow(x1 - x2, 2) + std::pow(y1 - y2, 2));
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "square_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);

    // 控制参数
    const float target_alt = 0.6;      // 飞行高度
    const float takeoff_dist = 0.2;   // 起飞领先距离
    const float land_dist = 0.10;      // 降落落后距离
    const float waypoint_dist = 0.15;  // 路径点领先距离
    const float wp_threshold = 0.1;    // 到达判定阈值
    const float stay_duration = 4.0;   // 停留时间 (秒)
    const float print_interval = 4.0;  // 状态打印间隔 (秒)

    // 设定矩形顶点 (x, y) - 这里的单位是米
    std::vector<std::pair<float, float>> waypoints = {
        {1.5, 0.0},
        {1.5, 1.5},
        {0.0, 1.5},
        {0.0, 0.0}
    };

    // 运动状态变量
    int current_wp_idx = 0;                     // 当前路径点索引
    FlightState status = TAKEOFF;               // 初始状态为起飞
    ros::Time last_request = ros::Time::now();  // 上次请求时间
    ros::Time last_print = ros::Time::now();    // 上次状态打印时间
    ros::Time wp_reach_time;                    // 到达路径点的时间
    
    geometry_msgs::PoseStamped setpoint;
    setpoint.pose.position.x = 0;
    setpoint.pose.position.y = 0;
    setpoint.pose.position.z = 0;


    // 等待连接
    ROS_INFO("\033[1;32mConnecting to FCU...\033[0m");
    while (ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("\033[1;32mFCU Connected\033[0m");

    // 预发布设定点
    for (int i = 100; ros::ok() && i > 0; --i) {
        local_pos_pub.publish(setpoint);
        ros::spinOnce();
        rate.sleep();
    }

    // 进入主循环
    while (ros::ok()) {
        ros::spinOnce();
        if (emergency_brake && status != LANDING) {
            ROS_ERROR("\033[1;31mEmergency Brake Activated! Initiating Landing...\033[0m");
            status = LANDING;
        }


        if (ros::Time::now() - last_print > ros::Duration(4.0)) {
            ROS_INFO("Current Mode: %s, Armed: %d, Flight State: %s", 
                    current_state.mode.c_str(), current_state.armed, status_to_str(status));
             last_print = ros::Time::now();
        }

        if(status != TAKEOFF && current_state.connected && !current_state.armed){
            ROS_INFO("\033[1;32m------------------------------------------\033[0m");
            ROS_INFO("\033[1;32m[FINAL] Disarm detected! Mission accomplished.\033[0m");
            ROS_INFO("\033[1;32m[FINAL] Exiting Node now...\033[0m");
            ROS_INFO("\033[1;32m------------------------------------------\033[0m");
            
            // 给系统一点点时间把这条日志打印出来
            ros::Duration(1.0).sleep();
            return 0;
        }

        // 模式切换逻辑
        if (current_state.mode != "OFFBOARD" &&
            (status == TAKEOFF || status == FLYING) &&
            (ros::Time::now() - last_request > ros::Duration(print_interval))) {
            mavros_msgs::SetMode offb_set_mode;
            offb_set_mode.request.custom_mode = "OFFBOARD";
            if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) {
                ROS_INFO("\033[1;32mOffboard enabled\033[0m");
            }
            last_request = ros::Time::now();
        } else if (!current_state.armed &&
            status == TAKEOFF &&
            (ros::Time::now() - last_request > ros::Duration(5.0))) {
            mavros_msgs::CommandBool arm_cmd;
            arm_cmd.request.value = true;
            if (arming_client.call(arm_cmd) && arm_cmd.response.success) {
                ROS_INFO("\033[1;32mVehicle armed\033[0m");
            }
            last_request = ros::Time::now();
        }

        // 任务状态机
        if (current_state.mode == "OFFBOARD" && current_state.armed) {
            switch (status) {
                case TAKEOFF: {
                    float next_z = current_pose.pose.position.z + takeoff_dist;
                    if (next_z > target_alt) next_z = target_alt;

                    if (next_z < takeoff_dist) setpoint.pose.position.z = takeoff_dist; // 直接设定目标高度，避免过慢的起飞
                    else setpoint.pose.position.z = next_z;

                    if (current_pose.pose.position.z > target_alt - 0.1) {
                        status = STAYING;
                        wp_reach_time = ros::Time::now();
                        ROS_INFO("\033[1;32mTakeoff complete. Staying for %.1f seconds...\033[0m", stay_duration);
                    }
                    break;
                }
                case FLYING: {
                    float tx = waypoints[current_wp_idx].first;
                    float ty = waypoints[current_wp_idx].second;

                    float dx = tx - current_pose.pose.position.x;
                    float dy = ty - current_pose.pose.position.y;
                    float dist_to_wp = std::sqrt(dx * dx + dy * dy);

                    if (dist_to_wp > waypoint_dist) {
                        // 如果到目标点的距离大于领先距离，则设定一个领先点
                        setpoint.pose.position.x = current_pose.pose.position.x + (dx / dist_to_wp) * waypoint_dist;
                        setpoint.pose.position.y = current_pose.pose.position.y + (dy / dist_to_wp) * waypoint_dist;
                    } else {
                        // 否则直接设定目标点
                        setpoint.pose.position.x = tx;
                        setpoint.pose.position.y = ty;
                    }

                    setpoint.pose.position.z = target_alt;

                    if (dist_to_wp < wp_threshold) {
                        current_wp_idx++;
                        status = STAYING;
                        wp_reach_time = ros::Time::now();

                        if (current_wp_idx >= waypoints.size()) {
                            ROS_INFO("\033[1;32mPath finished. Staying for %.1f seconds\033[0m", stay_duration);
                        } else {
                            ROS_INFO("\033[1;32mReached Waypoint %d. Staying for %.1f seconds...\033[0m", 
                                current_wp_idx, stay_duration);
                        }               
                    }
                    break;
                }

                case STAYING: {
                    if(current_wp_idx < waypoints.size()){
                        if(current_wp_idx == 0) {
                            setpoint.pose.position.x = 0;
                            setpoint.pose.position.y = 0;
                        }
                        else {
                            setpoint.pose.position.x = waypoints[current_wp_idx - 1].first;
                            setpoint.pose.position.y = waypoints[current_wp_idx - 1].second;
                        }
                    }
                    setpoint.pose.position.z = target_alt;

                    if (ros::Time::now() - wp_reach_time > ros::Duration(stay_duration)) {
                        if (current_wp_idx >= waypoints.size()) {
                            status = LANDING;
                            ROS_INFO("\033[1;32mStaying finished. Initiating Landing...\033[0m");
                        } else {
                            status = FLYING;
                            ROS_INFO("\033[1;32mStaying finished. Moving to Waypoint %d...\033[0m", current_wp_idx + 1);
                        }
                    }
                    break;
                }

                case LANDING: {
                    float next_z = current_pose.pose.position.z - land_dist;
                    if (next_z < 0.0) next_z = -0.05; // 压向地面
                    setpoint.pose.position.z = next_z;

                    // 切换为自动降落模式以实现自动上锁
                    if (current_pose.pose.position.z < 0.15) {
                        mavros_msgs::SetMode land_mode;
                        land_mode.request.custom_mode = "AUTO.LAND";
                        set_mode_client.call(land_mode);
                    }
                    break;
                }
            }
        }
        if (current_state.mode == "AUTO.LAND"){
            // 停止发布设定点，等待自动降落完成
        }
        else {
            // 如果不在自动降落模式，持续发布设定点
        local_pos_pub.publish(setpoint);
        }
        rate.sleep();
    }
    return 0;
}

代码分析

整体结构上，这仍然是一个单文件节点，故存在全局变量及其回调函数；而main函数可以分为ros内容、超参数、连接FCU、主循环（模式切换、解锁、状态机）。
其中状态机部分也曾在ego-planner中见到过，可以视为一个通用的架构。

实验细节

1. 紧急状态
与上一次相比，本次加入了一个紧急状态，当无人机的运动速度过大时将会切换为自动降落模式

2. 降落代码
首先，当无人机处于自动降落模式时将停止发布设定点，否则需要等待很长时间才会自动上锁；其次，仅当无人机处于起飞和飞行模式时才尝试切换模式与解锁；最后，由于仅在offboard与解锁状态下才进行状态机，故退出程序的检测写在状态机之外

3. gazebo使用
第一，直接ctrl+c无法完全关闭gazebo，然后导致下次启动仿真与节点时无人机可能无法正常起飞，需要pkill -9才能完全关闭。
第二，使用脚本后台运行gazebo会发生闪退，疑似是gazebo输入台检测到处于后台时会自动关闭，最终只能单独运行gazebo

4.日志降噪
在roslaunch启动命令后加上> /dev/null将正常输出丢弃，仅显示警告与报错；如果是> /dev/null 2>&1则丢弃所有输出

实机实验

第一次实验

同样先检查里程计，无人机静态下里程计读数不发散且读数正常，抱住无人机简单绕圈后放回原处，发现xy轴读数偏差在毫米级，但是z轴读数为-10cm左右。

不连接遥控器，直接先尝试运行offb node，发现运行正常，但是降落太慢了，可以将降落距离从5改为10。然后尝试运行square node，目测在起飞至0.6m左右后，无人机平稳转向朝左，然后继续升高且伴随后退，最后由于被绳子拉住，可能受力不平衡最终撞向地面。

存在问题

  1. 为什么offb node实机没问题，且square node仿真也没问题？为什么会不断升高？为什么会存在一个转向？
    假设无人机是在飞行到指定高度后，在进入到前进指令时才发生的旋转，那么推测原因为vins的方向和前进的方向之间存在夹角，为了对齐方向而发生旋转，之后因为该次旋转vins读数发生了漂移，于是不断向上和向后飞行。
    但是还是无法解释为什么offb node可以正常运行。一种说法是square node中存在复杂的switch结构，影响程序的频率。
    但是可以确定的是，问题根源出在里程计上，因为仿真没有问题；其次出在前进的代码上
    测试里程计后发现前进平移旋转的读数都正常，但是之前的目标点话题没有设置目标姿态，导致四元数为0 0 0 0，非法数据（四元数要保证和为一）可能在前进时出bug。于是将姿态固定为初始姿态（不过此处固定姿态的方法仅为暂时方法）。

  2. 安全措施还是太少了，缺乏紧急停止、防撞设施、安全区域
    首先是遥控器接管，先用offb node进行测试，切换为自稳/定高模式
    其次是防撞措施，一是地面上需要更多垫子，第二是是否要在墙根放沙袋，第三是要注意安全区域，尽量远离无人机飞行半径

  3. 后续测试。首先将square node矩形边长改小一点，先测试0.5的边长。第二，测试遥控器接管

Mian

Mian

每天成长一点

25
2
30
目录
  1. 1. 前言
  2. 2. 仿真测试
    1. 2.1. 仿真中使用里程计
    2. 2.2. 绕圈飞行
      1. 2.2.1. 仿真实验
      2. 2.2.2. 代码分析
      3. 2.2.3. 实验细节
    3. 2.3. 实机实验
      1. 2.3.1. 第一次实验
CATEGORIES
TAGS
AprilTag CPP FlightMare Gazebo IntelRealsenseD435 MAVROS NUC13 Python ROS1 Ubuntu20-04 Unity astar catkin cmake docker drone octomap px4 rosbag vins vitfly 仿真 动力学 参数服务器 实机 建图 模块化 视觉定位 路径规划 飞控