无人机自主探索FUEL：代码阅读2--算法重要参数

1、 主要launch文件 exploration.launch

 <!-- 整体地图尺寸   size of map, change the size in x, y, z according to your application  50 50 10-->
  <arg name="map_size_x" value="50.0"/>
  <arg name="map_size_y" value="50.0"/>
  <arg name="map_size_z" value=" 10.0"/>
  <!-- 无人机初始位置 -->
  <arg name="init_x" value="0"/>
  <arg name="init_y" value="0"/>
  <arg name="init_z" value="1.0"/> 
  
    <!-- 边对界簇做AABB框的参数 改变探索范围 -10.-15，0，10，15，2 -->
    <arg name="box_min_x" value="-5.0"/>
    <arg name="box_min_y" value="-15.0"/>
    <arg name="box_min_z" value=" 0.0"/>
    <arg name="box_max_x" value="5.0"/>
    <arg name="box_max_y" value="15.0"/>
    <arg name="box_max_z" value=" 2.0"/>

  <!-- ODOM对应的topic -topic of your odometry such as VIO or LIO -->
  <arg name="odom_topic" value="/state_ukf/odom" />
 <!-- 相机内参  intrinsic params of the depth camera -->
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>

    <!-- 无人机速度、加速度上限  maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="2.0" />
    <arg name="max_acc" value="2.0" />

    <!-- 轨迹id，函数displayTrajWithColor中对traj_cmd_进行显示 -->
    <param name="traj_server/pub_traj_id" value="4" type="int"/>
    <param name="traj_server/init_x" value="$(arg init_x)" type="double"/>
    <param name="traj_server/init_y" value="$(arg init_y)" type="double"/>
    <param name="traj_server/init_z" value="$(arg init_z)" type="double"/>
    <!-- 传感器视角范围 80*60deg  0.69对应80deg-->
    <param name="perception_utils/top_angle" value="0.56125" type="double"/>
    <param name="perception_utils/left_angle" value="0.69222" type="double"/>
    <param name="perception_utils/right_angle" value="0.68901" type="double"/>
    <!-- 传感器最大范围 原4.5m 改动每变化？-->
    <param name="perception_utils/max_dist" value="4.5" type="double"/>
    <!-- FOV上下左右距离 原1.0  改小后，FOV对应红色边框变小-->
    <param name="perception_utils/vis_dist" value="1.0" type="double"/>
  

其中，在改变了地图尺寸时，会出现将扫描后的内容重复拼接出现的情况，例如将地图尺寸设为x=20, y=20

2. 算法主要参数文件 algorighm.xml

在exploration.launch中被调用
主要节点以及算法参数：

    <remap from ="/odom_world" to="$(arg odometry_topic)"/>
    <remap from ="/map_ros/pose"   to = "$(arg sensor_pose_topic)"/>      位置信息
    <remap from ="/map_ros/depth" to = "$(arg depth_topic)"/>             深度信息

 <!-- 障碍物膨胀系数 -->
    <param name="sdf_map/obstacles_inflation"     value="0.199" /> 
    <param name="sdf_map/local_bound_inflate"    value="0.5"/>
    <!-- 地图边缘 原 50-->
    <param name="sdf_map/local_map_margin" value="50"/>


	 <!-- Fsm fast_exploration_fsm.cpp 中使用-->
    <!-- 轨迹基本执行后重规划时间阈值 -->
    <param name="fsm/thresh_replan1" value="0.5" type="double"/>
    <!-- 边界被覆盖，重新规划时间阈值 -->
    <param name="fsm/thresh_replan2" value="0.5" type="double"/>
    <!-- 定时重规划时间 -->
    <param name="fsm/thresh_replan3" value="1.5" type="double"/>


		<!-- Exploration manager 对应使用在 fast_exploration_manager.cpp中-->
		 <param name="exploration/refine_local" value="true" type="bool"/>
    <!-- 进行优化的视点数量上限 -->
    <param name="exploration/refined_num" value="7" type="int"/>
     <!-- 全环境搜索的截断半径，应该与fast_exploration_manageer.cpp中的radius_far联系起来 -->
    <param name="exploration/refined_radius" value="5.0" type="double"/>
    
    <!-- max_decay,top_view_num在函数getNViewPoints中用于获得N个视点 -->
    <param name="exploration/max_decay" value="0.8" type="double"/>
    <param name="exploration/top_view_num" value="15" type="int"/>
    <!-- 计算视点代价函数的参数 -->
    <param name="exploration/vm" value="$(eval 1.0 * arg('max_vel'))" type="double"/>
    <param name="exploration/am" value="$(eval 1.0 * arg('max_acc'))" type="double"/>
    <param name="exploration/yd" value="$(eval 60 * 3.1415926 / 180.0)" type="double"/>
    <param name="exploration/ydd" value="$(eval 90 * 3.1415926 / 180.0)" type="double"/>
    <param name="exploration/w_dir" value="1.5" type="double"/>      没有用到
    <!-- 旅行商算法求解依赖路径 -->
    <param name="exploration/tsp_dir" value="$(find lkh_tsp_solver)/resource" type="string"/>
 <!-- 在planner_manager.cpp中使用 -->
  <!-- 簇聚类与检查相关系数 -->
    <param name="frontier/cluster_min" value="100" type="int"/>        
    <!-- 对簇主成分分析的参数 -->
    <param name="frontier/cluster_size_xy" value="2.0" type="double"/>
    <!-- 未查到使用 -->
    <param name="frontier/cluster_size_z" value="10.0" type="double"/>
    <!-- 检测视点覆盖率的参数 -->
    <param name="frontier/min_candidate_dist" value="0.75" type="double"/>
    <!-- //检测簇距离的参数 -->
    <param name="frontier/min_candidate_clearance" value="0.21" type="double"/>
    <!-- dphi,rnum,rmin,rmax:在sampleViewpoints函数中用于检测视点覆盖率 -->
    <param name="frontier/candidate_dphi" value="$(eval 15 * 3.1415926 / 180.0)" type="double"/>
    <param name="frontier/candidate_rnum" value="3" type="int"/>
    <param name="frontier/candidate_rmin" value="1.5" type="double"/>
    <param name="frontier/candidate_rmax" value="2.5" type="double"/>
    <!-- 簇降采样参数 -->
    <param name="frontier/down_sample" value="3" type="int"/>;
    <!-- 用于保留视点数量的参数 -->
    <param name="frontier/min_visib_num" value="15" type="int"/>;

（1）vm，am，yd，ydd，w_dir

在函数ViewNode::computeCost中被使用，用于计算视点之间的位置、速度、yaw角代价，对应原文VI.C部分，公式（1），最后得到将代价保存在FIS中，当簇过时代价一并删除
(但其中仅vm,yd,w_dir使用了)

double ViewNode::computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1, const double& y2,
                             const Vector3d& v1, const double& yd1, vector<Vector3d>& path) {
  // Cost of position change
  double pos_cost = ViewNode::searchPath(p1, p2, path) / vm_;

  // Consider velocity change
  if (v1.norm() > 1e-3) {
    Vector3d dir = (p2 - p1).normalized();
    Vector3d vdir = v1.normalized();
    double diff = acos(vdir.dot(dir));
    pos_cost += w_dir_ * diff;
  }

  // Cost of yaw change
  double diff = fabs(y2 - y1);
  diff = min(diff, 2 * M_PI - diff);
  double yaw_cost = diff / yd_;
  return max(pos_cost, yaw_cost);
}

！！！：

其中，在计算代价时使用了A* 来计算无碰撞路径，调用了Astar2.cpp，在簇不同时调用Astar2.cpp的次数不同，即Astar2.cpp仅用于计算簇的代价，是仅用2个视点进行规划。主体算法的规划使用kinodynamicAstar.cpp来计算。 可以在终端中看出规划的使用区别：

[FSM]: from PLAN_TRAJ to EXEC_TRAJ
[ INFO] [1689835929.587823117]: [FSM]: state: EXEC_TRAJ
[FSM]: from EXEC_TRAJ to PLAN_TRAJ
[ WARN] [1689835930.017565323]: Replan: cluster covered=====================================
[ WARN] [1689835930.027529959]: min_id 0.000000 0.000000 0.000000: 
[ WARN] [1689835930.027559013]: max_id 0.000000 0.000000 0.000000: 
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------

[ WARN] [1689835930.033240569]: Cost mat: 0.001881, TSP: 0.000355
[Dijkstra Search] Node: 17, edge: 30
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
[Kino replan]: start: start_pt = -1.03064  8.31489  1.16124, start_vel = -0.382292  0.385339 -0.232095, start_acc =  -1.54011 -0.202278 -0.704203
[Kino replan]:  goal: end_pt = -2.92632  7.96307  1.18743, end_vel = 0 0 0
--------------KinodynamicAstar begin to search ----------------------
vel:-1.4467, acc:2.02864
near end
[non uniform B-spline] A size	120
[planner manager] B-spline时间间隔: dt_yaw: 0.0769338, start yaw:   3.01219  0.361323 -0.590218, end_yaw: -2.26126
[FSM]: from PLAN_TRAJ to EXEC_TRAJ
[FSM]: from EXEC_TRAJ to PLAN_TRAJ
[ WARN] [1689835930.469860361]: Replan: traj fully executed=================================
[ WARN] [1689835930.477673706]: min_id 0.000000 0.000000 0.000000: 
[ WARN] [1689835930.477725216]: max_id 0.000000 0.000000 0.000000: 
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------

[ WARN] [1689835930.481764293]: Cost mat: 0.001246, TSP: 0.000382
[Dijkstra Search] Node: 17, edge: 30
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
------------------- Astar2 begin to search ---------------------
[Kino replan]: start: start_pt = -1.27208  8.47133  1.06096, start_vel = -0.793755  0.295399 -0.209913, start_acc =  -1.53363 -0.305818 0.0625115
[Kino replan]:  goal: end_pt = -3.78867  5.34186  1.14388, end_vel = 0 0 0
--------------KinodynamicAstar begin to search ----------------------
open set empty, no path!

（2）“sdf_map/local_map_margin”

在map_ros.cpp中被用到，具体的效果影响不是很确定。

  Eigen::Vector3i min_cut = map_->md_->local_bound_min_ - Eigen::Vector3i(map_->mp_->local_map_margin_,
                                                                          map_->mp_->local_map_margin_,
                                                                          map_->mp_->local_map_margin_);
  Eigen::Vector3i max_cut = map_->md_->local_bound_max_ + Eigen::Vector3i(map_->mp_->local_map_margin_,
                                                                          map_->mp_->local_map_margin_,
                                                                          map_->mp_->local_map_margin_);

（3）与exploration.launch重复的参数（修改时需要注意修改哪个文件）

    <param name="perception_utils/top_angle" value="0.56125" type="double"/>;      传感器向上角度60度
    <param name="perception_utils/left_angle" value="0.69222" type="double"/>;      传感器左右角度80度
    <param name="perception_utils/right_angle" value="0.68901" type="double"/>;
    <param name="perception_utils/max_dist" value="4.5" type="double"/>;         仿真实验中调整后区别不大，不知道是否表示传感器向前方探测的距离(m)
    <param name="perception_utils/vis_dist" value="1.0" type="double"/>;     传感器FOV尺寸（红色三角）

！！！：
其中，在heading_planner.cpp中，写死了相机FOV的参数，可能影响了在algorithm.xml中调参的效果

// camera FoV params
        far_ = 4.5;
// normals of hyperplanes
        const double top_ang = 0.56125;

        const double left_ang = 0.69222;
        const double right_ang = 0.68901;

3. 影响时间的参数

所在文件	参数名称	具体含义
algorithm.xml		重规划时间

To be continue …