无人机自主探索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.-15010152 -->
    <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<param name="fsm/replan_time" value="0.200" type="double"/>重规划时间

To be continue …

  • 5
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值