Update README

Blimp-Autopilot-Ascend/README.md

@@ -245,7 +245,7 @@ body_T_cam1: !!opencv-matrix
 
 # 常见的可调节参数及其效果
 
-## VINS相关
+## VINS相关（视觉自主定位）
 
 ### 相机帧率
 
@@ -271,7 +271,7 @@ body_T_cam1: !!opencv-matrix
 - 当未开启多线程时，前端的耗时为特征提取与位姿解算两者之和；
 - 当开启多线程时， 前端的耗时为特征提取与位姿解算两者中较长的那个。
 
-## ego-planner相关
+## ego-planner相关（自主避障规划）
 
 ### 膨胀半径
 
@@ -298,6 +298,17 @@ src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/
 - 当球心高度比实际低，气球机器人可能碰到高处的障碍物（如天花板等）；
 - 当球心高度比实际高，气球机器人可能碰到低处的障碍物（如地面或气球机器人试图越过的障碍物等）。
 
+*若需要修改此参数，修改完需要重新编译ego-planner*
+
+### 虚拟天花板与地板
+
+src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/virtual_ceil为虚拟天花板，grid_map/virtual_ground为虚拟地板，这两个参数主要用于应对相机视野有限、向前拍摄时无法观察到真实的天花板与地板的问题。通过设置此虚拟天花板，在ego-planner建立的局部栅格占据地图中加入虚拟的天花板与地面，人为限制轨迹规划的可行空间，避免规划器向它看不到但实际有障碍的区域规划。
+
+以虚拟天花板为例：
+- 当天花板设置过高，比真实场景中的天花板更高，飞艇如果发现前方被障碍物占据，在看不到上方真实天花板的情况下，可能规划穿入天花板的轨迹来从上方越过障碍物，直到气球已经撞到天花板，由于其视野受限依然无法发现上方的天花板，会继续增大油门向上飞，可能发生危险；
+- 当天花板设置过低，会严重限制规划器的可行空间，导致飞艇前方明明有宽阔的可行空间，但在ego-planner看来，前方已经被障碍物完全遮挡而无法规划出可行轨迹。
+
+虚拟地面的情况与之类似。
 
 ### 速度、加速度、角速度限制
 
@@ -342,3 +353,23 @@ src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/
 * 保持`estimate_extrinsic`为1的情况下，VINS会持续对外参进行估计，但相当于在一个优化问题中引入了更多的自由变量，在不良工况下发散的可能性会更大一些
 * 机体转动过快时更容易发散，因为快速的转动比快速平移时画面中特征点的位移更大、画面的动态模糊更严重，此时光流追踪更容易失败
 * 当摄像头朝向缺乏纹理的白墙、容易反光的玻璃/镜子、具有大量重复纹理的墙纸/地砖或场景中存在大量运动物体时，VINS较容易发散
+
+## Ego-Planner自主运动中可能出现的问题
+
+### 无法规划可行轨迹
+
+* 启动ego-planner后，在可视化界面中发现前方几乎全是障碍物，将可行空间堵死而无法规划出可行轨迹
+1. 检查膨胀半径是否过大
+2. 检查虚拟天花板设置是否过低
+
+* 前方有可行空间，ego-planner一直在尝试求解但始终无法得到可行解
+1. 检查所设置的目标点是否位于障碍物区域内，如果目标点被障碍物（膨胀之后的）包裹，则无论如何也无法规划出不碰撞的可行轨迹
+
+### 飞行轨迹不合理
+
+* 左右侧有可行空间，但飞艇直接向上飞，甚至撞到天花板
+1. 检查虚拟天花板是否过高，无法起到限制作用
+
+* 飞艇在飞行过程中，轨迹超调严重，轨迹跟踪精度不足
+1. 检查速度、加速度上限是否设置过高；
+2. 检查飞艇重量，“飞艇自身+气囊升力+配重”全部抵消之后的净重力维持在10～20g为最佳状态，净重过大会严重影响控制性能，可以通过称重判断是否存在此问题，也可以通过飞艇原地悬停时螺旋桨转速来判断。

Blimp-Autopilot-Ascend/src/vins-fusion/realflight_modules/VINS-Fusion/loop_fusion/src/pose_graph_node.cpp

@@ -202,17 +202,6 @@ void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
     */
 }
 
-// void vins_fail_callback(const std_msgs::Bool::ConstPtr &vins_fail)
-// {
-//     if (vins_fail)
-//     {
-//         geometry_msgs::PoseStamped odometry_mavros_vision_pose;
-//         odometry_mavros_vision_pose.header = pose_msg->header;
-//         odometry_mavros_vision_pose.header.frame_id = "world";
-//         odometry_mavros_vision_pose.covariance
-//     }
-// }
-
 void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
 {
     //ROS_INFO("vio_callback!");
@@ -256,27 +245,17 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
     init_pos << init_position[0], init_position[1], init_position[2];
     Quaterniond q_pub = q_yaw * vio_q;
     pos_pub = q_yaw * vio_t + init_pos;
-    // Quaterniond q_pub = vio_q * q_yaw;
-    // pos_pub = vio_q * init_pos + vio_t;
 
     nav_msgs::Odometry odometry;
     odometry.header = pose_msg->header;
     odometry.header.frame_id = "world";
-    odometry.pose.pose.position.x = vio_t.x();
+    odometry.pose.pose.position.x = pos_pub.x();
-    odometry.pose.pose.position.y = vio_t.y();
+    odometry.pose.pose.position.y = pos_pub.y();
-    odometry.pose.pose.position.z = vio_t.z();
+    odometry.pose.pose.position.z = pos_pub.z();
-    odometry.pose.pose.orientation.x = vio_q.x();
+    odometry.pose.pose.orientation.x = q_pub.x();
-    odometry.pose.pose.orientation.y = vio_q.y();
+    odometry.pose.pose.orientation.y = q_pub.y();
-    odometry.pose.pose.orientation.z = vio_q.z();
+    odometry.pose.pose.orientation.z = q_pub.z();
-    odometry.pose.pose.orientation.w = vio_q.w();
+    odometry.pose.pose.orientation.w = q_pub.w();
-
-    // odometry.pose.pose.position.x = pos_pub.x();
-    // odometry.pose.pose.position.y = pos_pub.y();
-    // odometry.pose.pose.position.z = pos_pub.z();
-    // odometry.pose.pose.orientation.x = q_pub.x();
-    // odometry.pose.pose.orientation.y = q_pub.y();
-    // odometry.pose.pose.orientation.z = q_pub.z();
-    // odometry.pose.pose.orientation.w = q_pub.w();
     pub_odometry_rect.publish(odometry);
 
 #ifdef SEND_ODOMETRY_RECT_MAVROS
@@ -560,7 +539,6 @@ int main(int argc, char **argv)
     ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);
     ros::Subscriber sub_land_trigger = n.subscribe("/land_trigger", 2000, land_trigger_callback);
 
-    // ros::Subscriber sub_vins_failure = n.subscribe("/vins_fail", 2000, vins_fail_callback);
 
     pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
     pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);