EGO_Plannerv2代码学习(一):栅格建图之V1 V2的区别

本文记录了作者尝试将EGOPlannerV2部署到无人机的过程,重点分析了V2与V1在建图部分的差异,特别是MINCO轨迹参数化带来的提升和地图膨胀问题的解决。讨论了深度滤波在处理深度信息中的作用。

EGO_Plannerv2代码学习(一)


最近在尝试将高飞老师的集群规划器 ego_plannerV2部署到真实无人机上跑一下,开这个新帖记录下学习过程中的问题和想法。

EGO-Plannerv2

首先,V2是在EGO-planner基础上的进阶版,主要的改进是将轨迹参数化方式由均匀b样条改成了汪博的MINCO,提升了规划器的时空规划能力,代码整体结构上跟V1类似。

前天已经试着把v2的规划器移植到原来v1的硬件平台上,飞行时遇到的问题是由于地图膨胀,地面被抬高了三十厘米,导致轨迹规划失败。下面就读一下v1 v2中建图部分的代码,看下差别在哪。

建图部分的代码存放在src/planner/plan_env文件夹中,其中v2有两份建图源码,grid_map.cppgrid_map_bigmap.cpp改cmakelist可以选择编译哪份代码。
首先我们看一下V1中的建图代码流程:

grid_map.cpp
->	建图模块函数入口initMap( nh )
	1->	传入ros节点管理器node_ = nh;	
	2->	从参数服务器载入建图模块必要的参数
		//这里列出值得关注的变量,这些参数都是非常重要的,要在读代码的过程中不断理解参数的意义
		-----------------------------------------------------------------
		参数				变量名
		-----------------------------------------------------------------
		地图分辨率			mp_.resolution_
		地图大小				mp_.map_size_
		地图原点				mp_.map_origin_
		地图边界				mp_.map_min_boundary_
		地图边界				mp_.map_max_boundary_
		地图栅格个数			mp_.map_voxel_num_
		局部更新队列			mp_.local_update_range_
		障碍物膨胀系数		mp_.obstacles_inflation_
		深度命中值			mp_.
lixing@lixing:~/1/Fast-LIO2$ bash ./run_ego_planner.sh [START] Ego_Planner_Swarm_V1 Drone 0 autonomous run in max_vel 1.0 max_acc:=1.0 ... logging to /home/lixing/.ros/log/073dfb40-8314-11f0-8ffb-0145177117b2/roslaunch-lixing-12978.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt WARNING: disk usage in log directory [/home/lixing/.ros/log] is over 1GB. It's recommended that you use the 'rosclean' command. started roslaunch server http://lixing:40901/ SUMMARY ======== PARAMETERS * /drone_0_ego_planner_node/bspline/limit_acc: 1.0 * /drone_0_ego_planner_node/bspline/limit_ratio: 1.1 * /drone_0_ego_planner_node/bspline/limit_vel: 1.0 * /drone_0_ego_planner_node/fsm/emergency_time: 1.0 * /drone_0_ego_planner_node/fsm/fail_safe: True * /drone_0_ego_planner_node/fsm/flight_type: 1 * /drone_0_ego_planner_node/fsm/planning_horizen_time: 3.0 * /drone_0_ego_planner_node/fsm/planning_horizon: 7.5 * /drone_0_ego_planner_node/fsm/realworld_experiment: False * /drone_0_ego_planner_node/fsm/thresh_no_replan_meter: 1.0 * /drone_0_ego_planner_node/fsm/thresh_replan_time: 1.0 * /drone_0_ego_planner_node/fsm/waypoint0_x: 1.0 * /drone_0_ego_planner_node/fsm/waypoint0_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint0_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint1_x: 2.0 * /drone_0_ego_planner_node/fsm/waypoint1_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint1_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint2_x: 1.0 * /drone_0_ego_planner_node/fsm/waypoint2_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint2_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint3_x: 0.0 * /drone_0_ego_planner_node/fsm/waypoint3_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint3_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint4_x: 0.0 * /drone_0_ego_planner_node/fsm/waypoint4_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint4_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint5_x: 0.0 * /drone_0_ego_planner_node/fsm/waypoint5_y: 0.0 * /drone_0_ego_planner_node/fsm/waypoint5_z: 1.0 * /drone_0_ego_planner_node/fsm/waypoint_num: 5 * /drone_0_ego_planner_node/grid_map/cx: 321.04638671875 * /drone_0_ego_planner_node/grid_map/cy: 243.44969177246094 * /drone_0_ego_planner_node/grid_map/depth_filter_margin: 2 * /drone_0_ego_planner_node/grid_map/depth_filter_maxdist: 5.0 * /drone_0_ego_planner_node/grid_map/depth_filter_mindist: 0.2 * /drone_0_ego_planner_node/grid_map/depth_filter_tolerance: 0.15 * /drone_0_ego_planner_node/grid_map/frame_id: world * /drone_0_ego_planner_node/grid_map/fx: 387.229248046875 * /drone_0_ego_planner_node/grid_map/fy: 387.229248046875 * /drone_0_ego_planner_node/grid_map/ground_height: -0.01 * /drone_0_ego_planner_node/grid_map/k_depth_scaling_factor: 1000.0 * /drone_0_ego_planner_node/grid_map/local_map_margin: 10 * /drone_0_ego_planner_node/grid_map/local_update_range_x: 5.5 * /drone_0_ego_planner_node/grid_map/local_update_range_y: 5.5 * /drone_0_ego_planner_node/grid_map/local_update_range_z: 4.5 * /drone_0_ego_planner_node/grid_map/map_size_x: 50.0 * /drone_0_ego_planner_node/grid_map/map_size_y: 30.0 * /drone_0_ego_planner_node/grid_map/map_size_z: 5.0 * /drone_0_ego_planner_node/grid_map/max_ray_length: 5 * /drone_0_ego_planner_node/grid_map/min_ray_length: 0.2 * /drone_0_ego_planner_node/grid_map/obstacles_inflation: 0.199 * /drone_0_ego_planner_node/grid_map/p_hit: 0.65 * /drone_0_ego_planner_node/grid_map/p_max: 0.9 * /drone_0_ego_planner_node/grid_map/p_min: 0.12 * /drone_0_ego_planner_node/grid_map/p_miss: 0.35 * /drone_0_ego_planner_node/grid_map/p_occ: 0.8 * /drone_0_ego_planner_node/grid_map/pose_type: 2 * /drone_0_ego_planner_node/grid_map/resolution: 0.1 * /drone_0_ego_planner_node/grid_map/show_occ_time: False * /drone_0_ego_planner_node/grid_map/skip_pixel: 2 * /drone_0_ego_planner_node/grid_map/use_depth_filter: True * /drone_0_ego_planner_node/grid_map/virtual_ceil_height: 2.0 * /drone_0_ego_planner_node/grid_map/visualization_truncate_height: 1.8 * /drone_0_ego_planner_node/manager/control_points_distance: 0.4 * /drone_0_ego_planner_node/manager/drone_id: 0 * /drone_0_ego_planner_node/manager/feasibility_tolerance: 0.05 * /drone_0_ego_planner_node/manager/max_acc: 1.0 * /drone_0_ego_planner_node/manager/max_jerk: 4.0 * /drone_0_ego_planner_node/manager/max_vel: 1.0 * /drone_0_ego_planner_node/manager/planning_horizon: 7.5 * /drone_0_ego_planner_node/manager/use_distinctive_trajs: True * /drone_0_ego_planner_node/optimization/dist0: 0.5 * /drone_0_ego_planner_node/optimization/lambda_collision: 0.5 * /drone_0_ego_planner_node/optimization/lambda_feasibility: 0.1 * /drone_0_ego_planner_node/optimization/lambda_fitness: 1.0 * /drone_0_ego_planner_node/optimization/lambda_smooth: 1.0 * /drone_0_ego_planner_node/optimization/max_acc: 1.0 * /drone_0_ego_planner_node/optimization/max_vel: 1.0 * /drone_0_ego_planner_node/optimization/swarm_clearance: 0.5 * /drone_0_ego_planner_node/prediction/lambda: 1.0 * /drone_0_ego_planner_node/prediction/obj_num: 10 * /drone_0_ego_planner_node/prediction/predict_rate: 1.0 * /drone_0_odom_visualization/color/a: 1.0 * /drone_0_odom_visualization/color/b: 0.0 * /drone_0_odom_visualization/color/g: 0.0 * /drone_0_odom_visualization/color/r: 0.0 * /drone_0_odom_visualization/covariance_scale: 100.0 * /drone_0_odom_visualization/drone_id: drone_id * /drone_0_odom_visualization/robot_scale: 1.0 * /drone_0_odom_visualization/tf45: False * /drone_0_traj_server/traj_server/time_forward: 1.0 * /rosdistro: noetic * /rosversion: 1.17.4 NODES / drone_0_ego_planner_node (ego_planner/ego_planner_node) drone_0_odom_visualization (odom_visualization/odom_visualization) drone_0_traj_server (ego_planner/traj_server) ROS_MASTER_URI=http://localhost:11311 process[drone_0_ego_planner_node-1]: started with pid [13003] process[drone_0_traj_server-2]: started with pid [13004] process[drone_0_odom_visualization-3]: started with pid [13009] [INFO] [1756279902.190574508]: init grid_map Get intrinsic 387.229 0 321.046 0 387.229 243.45 0 0 1 hit: 0.619039 miss: -0.619039 min log: -1.99243 max: 2.19722 thresh log: 1.38629 [INFO] [1756279902.279829887]: before Subscriber [INFO] [1756279902.293179321]: grid_map init complete [WARN] [1756279903.921423393, 1076.624000000]: [Traj server]: ready. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. [FSM]: state: INIT no odom. wait for goal or trigger. Triggered! [FSM]: state: INIT no odom. Triggered! [FSM]: state: INIT no odom. [FSM]: state: INIT no odom. [FSM]: state: INIT no odom. 解释
08-28
评论 11
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值