25、基于传感器的机器人蒙特卡罗定位技术详解

基于传感器的机器人蒙特卡罗定位技术详解

1. 利用传感器跟踪相对姿态

要跟踪机器人的相对姿态，我们首先需要计算编码器读数的增量，并将这些增量输入到 convert_odometry_to_motion 函数中：

rot1, trans, rot2 = self.convert_odometry_to_motion(
    new_encoder_left - self.last_encoder_left, 
    new_encoder_right - self.last_encoder_right
)

之后，更新最后的编码器读数，以便下次计算增量：

self.last_encoder_left = new_encoder_left
self.last_encoder_right = new_encoder_right

最后，将这些里程计值应用到机器人的姿态上：

self.move_poses(rot1, trans, rot2)

为了调用这个运动模型，在 Simulation.main 中添加以下代码：

while True:
    await asyncio.sleep(0.05)
    self.motion_model()
    send_poses(self.poses)

此代码会在每个周期应用运动模型，然后发送姿态信息。由于运动模型的运行需要时间，因此减少了 sleep 时间以保持 UART 数据速率稳定。将 robot 文件夹的内容复制到 Raspberry Pi Pico 上，并启动 display_from_robot 应用程序。按下启动按钮后，你将看到机器人在场地中移动时姿态的变化。

2. 姿态移动概率

在现实世界中，机器人的移动并非完全确定。即使使用了编码器，车轮可能会打滑，车轮尺寸可能存在细微差异，计算也可能不完全准确。如图 13.11 所示，初始的机器人姿态猜测会在某个点周围形成一个集群，当机器人朝特定方向移动时，由于运动的不确定性，这个集群会散开。

为了模拟这种不确定性，我们使用以 0 为均值的概率分布。这里选择将两个从 -1.0 到 1.0 的均匀分布样本相加并除以 2 的方式，得到近似三角形的分布（n = 2）。在 robot/code.py 中，在 class Simulation 之前添加以下代码：

def get_random_sample(mean, scale):
    return mean + (random.uniform(-scale, scale) + random.uniform(-scale, scale)) / 2

同时，考虑到运动越大，随机误差因子也越大，我们在 Simulation.__init__ 类中添加以下参数：

self.last_encoder_right = robot.right_encoder.read()
self.alpha_rot = 0.09
self.alpha_rot_trans = 0.05
self.alpha_trans = 0.12
self.alpha_trans_rot = 0.05

这些参数的值应在 0 到 1 之间，并且需要根据机器人的实际误差情况进行调整。

接下来，在 Simulation 类中添加 randomise_motion 方法：

def randomise_motion(self, rot1, trans, rot2):
    rot1_scale = self.alpha_rot * abs(rot1) + self.alpha_rot_trans * abs(trans)
    trans_scale = self.alpha_trans * abs(trans) + self.alpha_trans_rot * (abs(rot1) + abs(rot2))
    rot2_scale = self.alpha_rot * abs(rot2) + self.alpha_rot_trans * abs(trans)
    rot1_model = np.array([get_random_sample(rot1, rot1_scale) for _ in range(self.poses.shape[0])])
    trans_model = np.array([get_random_sample(trans, trans_scale) for _ in range(self.poses.shape[0])])
    rot2_model = np.array([get_random_sample(rot2, rot2_scale) for _ in range(self.poses.shape[0])])
    return rot1_model, trans_model, rot2_model

在 motion_model 方法中使用这个随机化的运动模型：

def motion_model(self):
    """Apply the motion model"""
    new_encoder_left = robot.left_encoder.read()
    new_encoder_right = robot.right_encoder.read()
    rot1, trans, rot2 = self.convert_odometry_to_motion(
        new_encoder_left - self.last_encoder_left, 
        new_encoder_right - self.last_encoder_right
    )
    self.last_encoder_left = new_encoder_left
    self.last_encoder_right = new_encoder_right
    rot1_model, trans_model, rot2_model = self.randomise_motion(rot1, trans, rot2)
    self.move_poses(rot1_model, trans_model, rot2_model)

将 robot 文件夹复制到机器人上，并在计算机上运行 display_from_robot.py 应用程序，你会看到机器人的运动带有一定的随机性。

3. 故障排除

如果上述示例无法正常工作，可以尝试以下方法：
- 将机器人支撑起来并连接到计算机，通过 Mu 编辑器的串口查看其状态，以检查代码是否存在错误。
- 如果机器人的移动距离过大或过小，调整 robot/robot.py 中的测量值，使其与你的机器人匹配。
- 如果遇到传感器或 I2C 问题，检查布线和之前的传感器演示示例，并确保使用的是新电池。

4. 蒙特卡罗定位

机器人的姿态可能会超出场地范围，而距离传感器的读数可以帮助我们判断哪些姿态猜测更有可能。蒙特卡罗模拟可以根据传感器读数的可能性来改进这些猜测。

模拟过程包括移动姿态、观察传感器状态以创建基于可能性的权重（观察模型），然后根据权重重新采样猜测，得到新的一代猜测。这个过程也被称为粒子滤波。

4.1 从位置生成姿态权重

初始的权重生成可以基于一个简单的问题：机器人是否在场地内？如果不在，降低该姿态的概率。在 robot/arena.py 文件中进行以下操作：
1. 添加一个表示极低概率的值：

low_probability = 10 ** -10

2. 添加一个函数来检查场地是否包含某个点：

def contains(x, y):
    if x < 0 or x > width \
            or y < 0 or y > height:
        return False
    if x > (width - cutout_width) and y < cutout_height:
        return False
    return True

3. 在 robot/code.py 的 Simulation 类中添加 observation_model 方法来生成权重：

def observation_model(self):
    weights = np.ones(self.poses.shape[0], dtype=np.float)
    for index, pose in enumerate(self.poses):
        if not arena.contains(pose[0], pose[1]):
            weights[index] = arena.low_probability
    return weights

4.2 重新采样姿态

为了让后续的粒子更倾向于更有可能的姿态，我们使用低方差重采样方法。在 robot/code.py 的 Simulation 类中添加 resample 方法：

def resample(self, weights, sample_count):
    samples = np.zeros((sample_count, 3))
    interval = np.sum(weights) / sample_count
    shift = random.uniform(0, interval)
    cumulative_weights = weights[0]
    source_index = 0
    for current_index in range(sample_count):
        weight_index = shift + current_index * interval
        while weight_index > cumulative_weights:
            source_index += 1
            source_index = min(len(weights) - 1, source_index)
            cumulative_weights += weights[source_index]
        samples[current_index] = self.poses[source_index]
    return samples

在 Simulation.__init__ 中设置种群大小：

def __init__(self):
    self.population_size = 200

在 Simulation.main 的主循环中应用观察模型和重采样：

while True:
    weights = self.observation_model()
    send_poses(self.resample(weights, 20))
    self.poses = self.resample(weights, self.population_size)
    await asyncio.sleep(0.05)
    self.motion_model()

将 robot 文件夹发送到 Raspberry Pi Pico 并启动应用程序，你会看到场地外的样本数量逐渐减少。

5. 结合距离传感器

机器人配备了两个距离传感器，我们的代码将比较机器人感知的距离与模型数据。为了更高效地建模，我们创建一个查找表。

5.1 在空间中建模距离传感器

在 robot/arena.py 文件的顶部导入 numpy ：

from ulab import numpy as np

设置网格单元大小和过扫描：

grid_cell_size = 50
overscan = 2

添加计算到线段距离的函数：

def get_distance_to_segment(x, y, segment):
    x1, y1 = segment[0]
    x2, y2 = segment[1]
    if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
        return abs(y - y1)
    if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
        return abs(x - x1)
    return np.sqrt(
        min(
            (x - x1) ** 2 + (y - y1) ** 2, 
            (x - x2) ** 2 + (y - y2) ** 2
        )
    )

添加将距离转换为似然值的函数：

def get_distance_likelihood(x, y):
    min_distance = None
    for segment in boundary_lines:
        distance = get_distance_to_segment(x, y, segment)
        if min_distance is None or distance < min_distance:
            min_distance = distance
    return 1.0 / (1 + min_distance/100) ** 2

添加生成距离网格的函数：

def make_distance_grid():
    grid = np.zeros((
        width // grid_cell_size + 2 * overscan, 
        height // grid_cell_size + 2 * overscan
    ), dtype=np.float)
    for x in range(grid.shape[0]):
        column_x = x * grid_cell_size - (overscan * grid_cell_size)
        for y in range(grid.shape[1]):
            row_y = y * grid_cell_size - (overscan * grid_cell_size)
            grid[x, y] = get_distance_likelihood(
                column_x, row_y
            )
    return grid

distance_grid = make_distance_grid()

5.2 从距离传感器生成权重

在 robot/robot.py 中添加距离传感器的位置信息：

dist_side_mm = 37
dist_forward_mm = 66

在 robot/arena.py 中添加查找距离网格中位置的函数：

def get_distance_likelihood_at(x, y):
    """Return the distance grid value at the given point."""
    grid_x = int(x // grid_cell_size + overscan)
    grid_y = int(y // grid_cell_size + overscan)
    if grid_x < 0 or grid_x >= distance_grid.shape[0] or \
            grid_y < 0 or grid_y >= distance_grid.shape[1]:
        return low_probability
    return distance_grid[grid_x, grid_y]

在 robot/code.py 的 Simulation 类中添加计算传感器端点的方法：

def get_sensor_endpoints(self, sensor_reading, right=False):
    adjacent = sensor_reading + robot.dist_forward_mm
    angle = np.atan(robot.dist_side_mm / adjacent)
    if right:
        angle = - angle
    hypotenuse = np.sqrt(robot.dist_side_mm**2 + adjacent**2)
    pose_angles = np.radians(self.poses[:,2]) + angle
    sensor_endpoints = np.zeros((self.poses.shape[0], 2), dtype=np.float)
    sensor_endpoints[:,0] = self.poses[:,0] + hypotenuse * np.cos(pose_angles)
    sensor_endpoints[:,1] = self.poses[:,1] + hypotenuse * np.sin(pose_angles)
    return sensor_endpoints

添加 observe_distance_sensors 方法来应用传感器信息到现有权重：

def observe_distance_sensors(self, weights):
    left_sensor = self.get_sensor_endpoints(self.distance_sensors.left)
    right_sensor = self.get_sensor_endpoints(self.distance_sensors.right, True)
    for index in range(self.poses.shape[0]):
        sensor_weight = arena.get_distance_likelihood_at(left_sensor[index,0], left_sensor[index,1])
        sensor_weight += arena.get_distance_likelihood_at(right_sensor[index,0], right_sensor[index,1])
        weights[index] *= sensor_weight
    return weights

修改 observation_model 方法：

def observation_model(self):
    weights = np.ones(self.poses.shape[0], dtype=np.float)
    for index, pose in enumerate(self.poses):
        if not arena.contains(pose[0], pose[1]):
            weights[index] = arena.low_probability
    weights = self.observe_distance_sensors(weights)
    weights = weights / np.sum(weights)
    return weights

将此代码发送到机器人后，机器人的姿态会根据两个距离传感器的信息进行加权和重采样，在显示屏上你会看到姿态形成、分散和重新形成的过程。

总结

通过以上步骤，我们实现了基于编码器的姿态运动模型，并结合了距离传感器和场地数据进行蒙特卡罗定位。整个过程包括运动模型的应用、姿态权重的生成、重采样以及距离传感器信息的融合，最终提高了机器人姿态估计的准确性。在实际应用中，需要根据机器人的具体情况调整参数，以达到最佳效果。同时，遇到问题时可以按照故障排除部分的方法进行检查和修复。

基于传感器的机器人蒙特卡罗定位技术详解

6. 关键技术点分析

为了更好地理解整个机器人蒙特卡罗定位的过程，我们对其中的关键技术点进行详细分析。

6.1 运动模型的随机化

运动模型的随机化是为了模拟现实世界中机器人运动的不确定性。通过 get_random_sample 函数生成随机样本，结合 alpha 参数来调整随机误差的大小。以下是随机化运动模型的关键步骤：
1. 计算缩放因子 ：根据 alpha 参数和当前的旋转、平移值计算缩放因子，如 rot1_scale 、 trans_scale 和 rot2_scale 。
2. 生成随机样本 ：使用 get_random_sample 函数，以当前的旋转、平移值为均值，缩放因子为范围，生成随机样本。
3. 应用随机样本 ：将生成的随机样本应用到 move_poses 方法中，实现姿态的随机化移动。

这个过程的流程图如下：

graph TD
    A[开始] --> B[计算旋转、平移值]
    B --> C[计算缩放因子]
    C --> D[生成随机样本]
    D --> E[应用随机样本到姿态]
    E --> F[结束]

6.2 低方差重采样方法

低方差重采样方法是蒙特卡罗定位中的重要步骤，用于根据姿态的权重重新采样，使得更有可能的姿态被保留下来。具体步骤如下：
1. 设置样本间隔和偏移 ：根据权重总和和样本数量计算样本间隔，并随机生成偏移值。
2. 计算累积权重 ：初始化累积权重和源样本索引。
3. 循环采样 ：对于每个样本，计算权重索引，找到满足条件的源样本索引，并将其添加到新样本中。

这个过程的伪代码如下：

function resample(weights, sample_count):
    samples = zeros(sample_count, 3)
    interval = sum(weights) / sample_count
    shift = random(0, interval)
    cumulative_weights = weights[0]
    source_index = 0
    for current_index from 0 to sample_count - 1:
        weight_index = shift + current_index * interval
        while weight_index > cumulative_weights:
            source_index = source_index + 1
            source_index = min(length(weights) - 1, source_index)
            cumulative_weights = cumulative_weights + weights[source_index]
        samples[current_index] = poses[source_index]
    return samples

7. 代码优化建议

在实际应用中，为了提高代码的性能和稳定性，可以考虑以下优化建议：

7.1 减少随机样本生成的次数

在 get_random_sample 函数中，每次调用都会生成两个随机样本。可以考虑缓存一些随机样本，减少随机数生成的开销。

7.2 优化距离网格的生成

距离网格的生成需要遍历整个网格，计算每个单元格的距离似然值。可以考虑使用并行计算或优化算法，减少生成时间。

7.3 动态调整参数

alpha 参数和其他测量值可以根据机器人的运行情况进行动态调整，以适应不同的环境和任务。

8. 应用案例与效果展示

通过实际应用，我们可以看到机器人蒙特卡罗定位技术的效果。以下是一个具体的应用案例：

在一个室内场地中，机器人配备了编码器和两个距离传感器。将上述代码部署到机器人上，并运行 display_from_robot.py 应用程序。随着机器人的移动，我们可以观察到以下现象：
- 姿态随机化 ：机器人的运动带有一定的随机性，模拟了现实世界中的不确定性。
- 样本空间缩小 ：通过蒙特卡罗模拟和重采样，样本空间逐渐缩小到更有可能的姿态，减少了场地外的样本数量。
- 姿态聚类 ：机器人的姿态会根据距离传感器的信息形成聚类，集中在可能的位置上。

以下是一个简单的表格，展示了不同阶段的样本数量变化：
| 阶段 | 总样本数量 | 场地内样本数量 | 场地外样本数量 |
| ---- | ---- | ---- | ---- |
| 初始 | 200 | 150 | 50 |
| 运行一段时间后 | 200 | 180 | 20 |

9. 总结与展望

通过本文的介绍，我们详细了解了基于传感器的机器人蒙特卡罗定位技术。从利用编码器跟踪相对姿态，到引入姿态移动概率的随机化，再到结合距离传感器和场地数据进行蒙特卡罗模拟，整个过程逐步提高了机器人姿态估计的准确性。

在未来的研究和应用中，我们可以进一步探索以下方向：
- 多传感器融合 ：除了编码器和距离传感器，还可以结合其他传感器，如摄像头、惯性测量单元等，提高定位的精度和可靠性。
- 实时优化 ：在机器人运行过程中，实时调整参数和算法，以适应不同的环境和任务。
- 复杂环境应用 ：将该技术应用到更复杂的环境中，如室外环境、动态环境等，拓展其应用范围。

总之，机器人蒙特卡罗定位技术为机器人的自主定位和导航提供了一种有效的方法，具有广阔的应用前景。通过不断的研究和优化，我们可以让机器人在各种环境中更加准确地定位和导航。