260422

继续往下，就是我们的控制循环了：

    def _control_loop(self) -> None:
        # ...
        selected_heading_error, selected_clearance, scan_range_limit, should_reverse = self._select_heading(
            goal_heading_error=heading_error,
            goal_distance=distance,
        )
        # ...

这里我们新增了四个变量，分别是 selected_heading_error, selected_clearance, scan_range_limit 和 should_reverse，分别对应best_heading, best_clearance, scan_range_limit, should_reverse

best_heading: 最佳行驶方向（相对于当前朝向的角度误差）
best_clearance: 该方向的净空距离
scan_range_limit: 激光雷达有效范围
should_reverse: 是否应该倒车行驶

通过我们昨天的编写，我们知道我们的 _select_heading 是负责在所有扇区中，按照我们的算法来选出分数最高的一个扇区。

然后根据这四个新的参数来限制我们实际的线速度：

    def _control_loop(self) -> None:
        # ...
        turn_only_angle = cast(float, self.get_parameter('turn_only_angle').value)
        reverse_turn_only_angle = cast(float, self.get_parameter('reverse_turn_only_angle').value)
        linear_direction = -1.0 if should_reverse else 1.0
        active_turn_only_angle = reverse_turn_only_angle if should_reverse else turn_only_angle

        if abs(selected_heading_error) < active_turn_only_angle:
            heading_tolerance = cast(float, self.get_parameter('heading_tolerance').value)
            motion_heading_error = selected_heading_error
            if should_reverse:
                motion_heading_error = self._normalize_angle(selected_heading_error - math.pi)

            heading_speed_factor = 1.0
            if abs(motion_heading_error) >= heading_tolerance:
                heading_speed_factor = max(0.0, math.cos(motion_heading_error))

            min_speed_factor = cast(float, self.get_parameter('min_speed_factor').value)
            clearance_speed_factor = self._clamp(selected_clearance / scan_range_limit, min_speed_factor, 1.0)
            commanded_speed = linear_speed * heading_speed_factor * clearance_speed_factor
            if should_reverse:
                commanded_speed *= cast(float, self.get_parameter('reverse_speed_ratio').value)

            command.linear.x = linear_direction * commanded_speed

        # ...

这四个新的变量就一起控制了我们的小车是否能前进、前进多快以及是否倒车。

总的来说，我们现在的整个控制循环就长这样了：

    def _control_loop(self) -> None:
        if self.current_mode != FOLLOW_MODE_VALUE:
            return

        if self.robot_x is None or self.robot_y is None or self.robot_yaw is None:
            self.cmd_vel_raw_publisher.publish(Twist())
            return

        if self.flag_x is None or self.flag_y is None:
            self.cmd_vel_raw_publisher.publish(Twist())
            return

        dx = self.flag_x - self.robot_x
        dy = self.flag_y - self.robot_y
        distance = math.hypot(dx, dy)
        stop_distance = cast(float, self.get_parameter('stop_distance').value)
        if distance <= stop_distance:
            self.cmd_vel_raw_publisher.publish(Twist())
            return

        target_yaw = math.atan2(dy, dx)
        heading_error = self._normalize_angle(target_yaw - self.robot_yaw)

        if not self._has_fresh_scan():
            self.cmd_vel_raw_publisher.publish(Twist())
            return

        selected_heading_error, selected_clearance, scan_range_limit, should_reverse = self._select_heading(
            goal_heading_error=heading_error,
            goal_distance=distance,
        )

        linear_speed = min(
            cast(float, self.get_parameter('max_linear_speed').value),
            cast(float, self.get_parameter('linear_gain').value) * max(0.0, distance - stop_distance),
        )
        angular_speed = max(
            -cast(float, self.get_parameter('max_angular_speed').value),
            min(
                cast(float, self.get_parameter('max_angular_speed').value),
                cast(float, self.get_parameter('angular_gain').value) * selected_heading_error,
            ),
        )

        command = Twist()
        command.angular.z = angular_speed

        turn_only_angle = cast(float, self.get_parameter('turn_only_angle').value)
        reverse_turn_only_angle = cast(float, self.get_parameter('reverse_turn_only_angle').value)
        linear_direction = -1.0 if should_reverse else 1.0
        active_turn_only_angle = reverse_turn_only_angle if should_reverse else turn_only_angle

        if abs(selected_heading_error) < active_turn_only_angle:
            heading_tolerance = cast(float, self.get_parameter('heading_tolerance').value)
            motion_heading_error = selected_heading_error
            if should_reverse:
                motion_heading_error = self._normalize_angle(selected_heading_error - math.pi)

            heading_speed_factor = 1.0
            if abs(motion_heading_error) >= heading_tolerance:
                heading_speed_factor = max(0.0, math.cos(motion_heading_error))

            min_speed_factor = cast(float, self.get_parameter('min_speed_factor').value)
            clearance_speed_factor = self._clamp(selected_clearance / scan_range_limit, min_speed_factor, 1.0)
            commanded_speed = linear_speed * heading_speed_factor * clearance_speed_factor
            if should_reverse:
                commanded_speed *= cast(float, self.get_parameter('reverse_speed_ratio').value)

            command.linear.x = linear_direction * commanded_speed

        self.cmd_vel_raw_publisher.publish(command)

现在的完整逻辑是这样的：

模式门控：

if self.current_mode != FOLLOW_MODE_VALUE:
    return

只有在 FOLLOW模式下，这个控制循环才会被激活，否则直接返回。

检查自己的位姿信息：

if self.robot_x is None or self.robot_y is None or self.robot_yaw is None:
    self.cmd_vel_raw_publisher.publish(Twist())
    return

如果机器人不知道自己的位姿信息，也就没法进行任何方向计算，立刻停止。

检查目标位姿：

if self.flag_x is None or self.flag_y is None:
    self.cmd_vel_raw_publisher.publish(Twist())
    return

同上，如果不知道目标在哪，也应当立刻停止。

计算到目标的相对位置：

dx = self.flag_x - self.robot_x
dy = self.flag_y - self.robot_y
distance = math.hypot(dx, dy)

典型的直线距离计算，不多做解释。

到达停止距离就停车

stop_distance = cast(float, self.get_parameter('stop_distance').value)
if distance <= stop_distance:
    self.cmd_vel_raw_publisher.publish(Twist())
    return

如果距离目标已经足够近，比如 0.6m 左右，就可以直接停车了。

计算目标方向误差：

target_yaw = math.atan2(dy, dx)
heading_error = self._normalize_angle(target_yaw - self.robot_yaw)

这里得到机器人如果想直达目标，需要转多少角度的角。

检查激光雷达的数据是否可用：

if not self._has_fresh_scan():
    self.cmd_vel_raw_publisher.publish(Twist())
    return

即使目标和自己的位姿都知道，但雷达数据过期了，也不允许运动。因为这个控制器依赖实时障碍物信息做避障。

这个函数貌似我们之前没有提过，这个函数的代码是这样的：

    def _has_fresh_scan(self) -> bool:
        if self.latest_scan is None or self.last_scan_received_time is None:
            return False

        scan_timeout = cast(float, self.get_parameter('scan_timeout').value)
        scan_age = (self.get_clock().now() - self.last_scan_received_time).nanoseconds / 1e9
        return scan_age <= scan_timeout

这里如果超过了我们设置的超时扫描时间都没有收到新的激光雷达消息的话，则就认为激光雷达有点过时了。

选择耳咽管实际可执行的方向：

selected_heading_error, selected_clearance, scan_range_limit, should_reverse = self._select_heading(
    goal_heading_error=heading_error,
    goal_distance=distance,
)

这部分代码也是我们控制循环的核心，我们目前新增的代码的核心也是为了这个东西。

最终得到最佳行驶角度和当前角度的误差、该方向的净空距离、激光雷达有效范围和是否应该倒车行驶。

然后就是我们的基础线速度了：

linear_speed = min(
    max_linear_speed,
    linear_gain * max(0.0, distance - stop_distance),
)

这只是一个和目标距离有关的“理想速度”，离目标远，速度快；离目标近，速度慢。

这里还没有考虑朝向偏差和障碍物，只是“从任务角度看，希望跑多快”。

计算基础角速度：

angular_speed = max(
    -max_angular_speed,
    min(max_angular_speed, angular_gain * selected_heading_error),
    )
)

角速度由选中的方向误差决定，偏左就左转，偏右就右转。误差越大，转得越快，但最终限制在最大角速度内。

初始化速度命令：

command = Twist()
command.angular.z = angular_speed

先创建一个默认的 Twist。

决定是前进还是倒车的配置：

turn_only_angle = cast(float, self.get_parameter('turn_only_angle').value)
reverse_turn_only_angle = cast(float, self.get_parameter('reverse_turn_only_angle').value)
linear_direction = -1.0 if should_reverse else 1.0
active_turn_only_angle = reverse_turn_only_angle if should_reverse else turn_only_angle

这里做两件事，一个是用 linear_direction 决定最终线速度正负，还有一个是如果是倒车，就允许更大的角度误差下也可以运动。

因为机器人在倒车的时候，允许车尾方向和目标方向有较大的偏差，不需要像前进那样基本对准。

判断现在是否允许进行线速度运动：

if abs(selected_heading_error) < active_turn_only_angle:

只有选中的方向误差足够小，才允许加线速度。否则只保留角速度，让机器人先转到差不多对准再动。

这是为了避免我们的机器人小车一边拐大角度一边冲出去，因为那样也是很容易撞墙的。

计算运动方向上的误差：

heading_tolerance = cast(float, self.get_parameter('heading_tolerance').value)
motion_heading_error = selected_heading_error
if should_reverse:
    motion_heading_error = self._normalize_angle(selected_heading_error - math.pi)

前进时：车头和运动方向上的误差。
倒车时：车尾和运动方向上的误差。

因为倒车时真正的运动方向相当于和车头差，所以要减去做修正。

计算朝向速度因子：

heading_speed_factor = 1.0
if abs(motion_heading_error) >= heading_tolerance:
    heading_speed_factor = max(0.0, math.cos(motion_heading_error))

这个因子的作用是，朝向越对，线速度越大，这使得我们的机器人可以在没有对准的时候自动减速，而不是直接按照基础速度猛冲。

计算障碍物间隙速度因子：

min_speed_factor = cast(float, self.get_parameter('min_speed_factor').value)
clearance_speed_factor = self._clamp(selected_clearance / scan_range_limit, min_speed_factor, 1.0)

这个因子表示：这个方向上空间越宽，线速度越大。

即，即使前面可以走，但如果前方的空间狭窄，则也会主动慢下来。

计算最终的线速度：

commanded_speed = linear_speed * heading_speed_factor * clearance_speed_factor
if should_reverse:
    commanded_speed *= reverse_speed_ratio

最终线速度是几层因素叠加后的结果：

linear_speed：任务上，理想想跑多快
heading_speed_factor：朝向允许跑多快
clearance_speed_factor：环境允许跑多快
reverse_speed_ratio：倒车时额外降速

这就是我们最终线速度的来源了。

最后就是写入我们的线速度方向了：

command.linear.x = linear_direction * commanded_speed

前进为正速度，反之为负速度。

随后发布即可：

self.cmd_vel_raw_publisher.publish(command)

现在来看看我们实际的效果：

可以看到，除了最后由于我们技术预期就无法避免的问题：

小车在面对狭窄过道或者窄门时，如果只用单球体膨胀算法，小车很容易报出“前方有障碍物”而拒绝通行的问题。
1. 我们的小车在目标被夹在两个障碍物间时，障碍物间距较小时不会主动进洞。
小车由于并没有对地形进行建模，因此可能会出现反复前进后退的情况。

就这样，我们完成了小车的自主导航+自主避障。

第一阶段，DONE。

以上。

KiSpace

【Gazebo 学习】260422

260422

Jese__Ki

260422

【Gazebo 学习】260424

【Gazebo 学习】260421

Jese__Ki