260419

最后就是我们当前阶段的最终目标了——实现避障+导航到目标点。

为了实现这一点，我们需要将我们的小车认为是一个球体，才能尽可能避免碰撞等情况。

为此，我们可以以半径为小车轮子的极远点作对角线来作该外接球就可以了。

不过这有一个弊端，小车在面对狭窄过道或者窄门时，如果只用单球体膨胀算法，小车很容易报出“前方有障碍物”而拒绝通行的问题。

然后就是避障的算法了，我们可以将小车的一周且分为多个扇区，然后通过这些扇区中的信息来得出一个分数，让小车往分数高的分数走。

我们的分数受到下面几个因素影响：

1. 宽度分数：指定方向上的宽度。
2. 目标分数：往指定方向上走能否达到旗帜。
3. 转圈分数：往指定方向上走是否需要原地转圈。

通常来说，第一个因素的影响最大，第二个其次，最下面的则最低。

我们假设下面这三个场景：

通过加权，我们很可能会选择 C 方向来进行前进。

除此之外，我们要理解一个点就是，我们的旗帜也是一种“障碍物”，因此当接近旗帜的时候，我们需要降低宽度分数的权重。

以及，我们在实现后，由于我们并没有采用视觉的路线，因此当小车距离旗帜较近时，如果周围有障碍物，它可能会继续进行转圈。以及，如果面临只有后方可以走的情况时，如果朝后的路径较长，小车由于并没有对地形进行建模，因此可能会出现反复前进后退的情况。这是我们当前方案的弊端。

如果我们想解决这两个弊端的话，我们后续需要引入视觉(识别旗帜是否就在眼前) + 小车自主对地形建模(拥有记忆, 避免原地打转)。

但这是我们下一个机器人学习项目的目标了，不是我们现在的目标。

这两个问题想解决的话，

接下来就进行实现：

我们先新增这么一批参数：

class FollowController(Node):
    def __init__(self) -> None:
        # ...
        params = [
            # ...
            ('scan_topic', 'scan'),
            # ...
            ('scan_timeout', 0.30),
            # ...
            ('turn_only_angle', 0.80),
            ('sector_count', 31),
            ('sector_fov', math.pi * 2),
            ('avoidance_range', 2.50),
            ('robot_body_radius', 0.371),
            ('obstacle_padding', 0.05),
            ('blocked_distance_scale', 1.10),
            ('min_speed_factor', 0.28),
            ('clearance_weight', 0.52),
            ('goal_weight', 0.34),
            ('heading_weight', 0.14),
            ('lidar_offset_x', 0.20),
            ('lidar_offset_y', 0.0),
        ]

我们逐个解释一下：

• scan_topic：说明小车从哪个话题获取雷达的信息，默认为 scan。
• scan_timeout：如果太久没有收到雷达的消息，则就认为感知失效，此时就直接停下，我们设置为 0.3s。
• turn_only_angle：用于判定小车在面临需要转的角度较大时，应该进行大的原地转向，还是边走边转的角度阈值，避免出现小车边转边走最终撞到障碍物上的问题，这里是 0.8，即大约 45.8 度左右。
• sector_count：扇区的数量，我们设置为 31。
• sector_fov：候选方向的总方向角，我们设置为 pi * 2，即 360 度。
• avoidance_range：我们局部规划只看前面多远，避免引入过多噪声。
• robot_body_radius：机器人外接球的半径，通过计算，我们设置为 0.37。
  • 我们是这样计算的，由于我们的小车近似一个矩形，且我们不需要考虑 z 轴上的情况，因此我们可以将前轮极远点的一半作为宽，一侧轮子极远点的一半作为长来作为这个矩形的宽和长。
  • 我们经过计算得到这两个值分别为 0.26 和 0.265，
  • 以此来计算对角线即可得到0.37，这个值即半径。
• obstacle_padding：我们不能把我们的小车的外接球半径就认为障碍物相切就可以通过了，保险起见需要有一个安全范围。
• blocked_distance_scale：多近算“这个方向已经很危险”，它和安全半径相乘得到阻塞阈值。
• min_speed_factor：近障碍时也别让速度直接归零到太夸张，保留一个下限比例，不然可能特别容易卡住。
• clearance_weight / goal_weight / heading_weight：这就是我们之前说的宽度分数、目标分数和直行分数的权重参数了。
• lidar_offset_x / lidar_offset_y：我们的激光雷达不是装在的小车中心，因此需要有一个偏移。