1.背景

基于costmap地图，使用navigation导航时，会出现由于激光雷达/图像测距的局限性， 会导致costmap上有行人走过，代价地图中的障碍物，无法被及时的清除，留下影子一样的轨迹。

2.目标

针对障碍物无法及时清除，主要从两方面下手，一方面是对障碍物数据的预处理，例如给障碍物语义信息，对动态障碍物通过i卡尔曼继续跟踪，或者设置相关的参数，。另一方面是对障碍物清除机制进行优化，目前障碍物是根据光追法对障碍物进行清除，有其有时和弊端。

3. 障碍物信息添加方式

发送数据的数据结构与接收数据的数据结构

目前障碍物层主要接收种类型的障碍物信息，分别是LaserScan和PointCloud2两种，msg位于/opt/ros/humble/share/sensor_msgs/msg中,相关的数据结构如下所示:
LaserScan.msg

# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

std_msgs/Header header # timestamp in the header is the acquisition time of
                             # the first ray in the scan.
                             #
                             # in frame frame_id, angles are measured around
                             # the positive Z axis (counterclockwise, if Z is up)
                             # with zero angle being forward along the x axis

float32 angle_min            # start angle of the scan [rad]
float32 angle_max            # end angle of the scan [rad]
float32 angle_increment      # angular distance between measurements [rad]

float32 time_increment       # time between measurements [seconds] - if your scanner
                             # is moving, this will be used in interpolating position
                             # of 3d points
float32 scan_time            # time between scans [seconds]

float32 range_min            # minimum range value [m]
float32 range_max            # maximum range value [m]

float32[] ranges             # range data [m]
                             # (Note: values < range_min or > range_max should be discarded)
float32[] intensities        # intensity data [device-specific units].  If your
                             # device does not provide intensities, please leave
                             # the array empty.

PointCloud2.msg

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.
#
# The point cloud data may be organized 2d (image-like) or 1d (unordered).
# Point clouds organized as 2d images may be produced by camera depth sensors
# such as stereo or time-of-flight.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d points).
std_msgs/Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

其中关于PointField.msg数据类型如下:

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

# Common PointField names are x, y, z, intensity, rgb, rgba
string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

订阅相关话题之后，转成的障碍物类型为Observation，其主要的成员参数为

  geometry_msgs::msg::Point origin_;
  sensor_msgs::msg::PointCloud2 * cloud_;
  double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_;

其中最主要的是cloud_和origin_两个变量，origin_类型为Point类型，该位置为/opt/ros/humble/share/geometry_msgs/msg，其数据结构为x,y,z
如下所示

# This contains the position of a point in free space
float64 x
float64 y
float64 z

• 总的来说，Observation类型最重要的信息为点云和位置。
• 使用LaserScan数据类型更符合相机有视场角的信息。
• 使用PointCloud2可以加入障碍物的rgb信息。可以用intensity将障碍物的语义信息传到障碍物层进行处理。

4. 障碍物清理机制

前提: 基于costmap对障碍物的清除，主要是在接收端对障碍物进行处理，系统原本的是基于光追法对障碍物清理。
问题:

• 但当障碍物面向机器人运动时，根据光追法原理，上一帧的障碍物在地图上由于当前帧同一障碍物的遮挡保留在地图中，会对当前的规划造成困扰。
• 当障碍物面向机器人左右移动时，会因为当前帧点云没有抵达上一帧障碍物所在点的后方，上一帧障碍物也依然会保留在地图中

针对以上的问题，处理思路主要如下:

• 通过yaml文件对障碍物进行参数滤波
• 优化障碍物的光追算法
• 对障碍物进行跟踪(后期优化)，通过卡尔曼滤波对障碍物进行跟踪，增加语义信息增加障碍物跟踪的稳定性。

4.1 可调参数

max_obstacle_height: 2.0 #设置障碍物最大高度，将超过该高度的障碍物进行滤除
min_obstacle_height: 0.0 #设置障碍物最小高度，将小于该高度额障碍物进行滤除
obstacle_max_range: 2.5 # 障碍物最大检测距离
obstacle_min_range: 0.0 # 障碍物最小检测距离
raytrace_max_range: 3.0 # 光追最大距离
raytrace_min_range: 0.0 # 光追最小距离
clearing: True #是否清除
marking: True #是否标记

4.2 优化光追算法

使用光线跟踪 raytraceFreespace清除障碍物的逻辑为:

raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); 会将所有在(x0,y0)>>(x1,y1)之间的所有cell标记为FREE_SPACE。
而updateRaytraceBounds 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）。
updateBounds 在根据测量数据完成clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle ：
参考链接: costmap_2d: obstacle_layer中关于激光雷达障碍物清除不干净的解决

4.3 障碍物跟踪