24、机器人空间建模与姿态跟踪：从数据显示到运动模拟

机器人空间建模与姿态跟踪：从数据显示到运动模拟

1. 空间建模与数据显示

1.1 蓝牙连接管理

在与机器人进行通信时，我们需要确保能够建立和关闭连接。以下是实现关闭连接和发送数据的代码：

async def close(self):
    await self.ble_client.disconnect()

async def send_uart_data(self, data):
    await self.ble_client.write_gatt_char(self.tx_gatt, data)

上述代码中， close 方法用于断开蓝牙连接， send_uart_data 方法用于向机器人发送数据。

1.2 机器人数据在计算机屏幕上的显示

我们可以使用 matplotlib 库将机器人的数据显示在计算机屏幕上。首先，创建一个名为 display_from_robot.py 的文件，并进行如下导入：

import asyncio
import json
import matplotlib.pyplot as plt
from robot_ble_connection import BleConnection

然后，定义一个 RobotDisplay 类来处理显示逻辑：

class RobotDisplay:
    def __init__(self):
        self.ble_connection = BleConnection(self.handle_data)
        self.buffer = ""
        self.arena = {}
        self.closed = False
        self.fig, self.axes = plt.subplots()

    def handle_close(self, _):
        self.closed = True

    def handle_data(self, data):
        self.buffer += data.decode()
        while "\n" in self.buffer:
            line, self.buffer = self.buffer.split("\n", 1)
            print(f"Received data: {line}")
            try:
                message = json.loads(line)
            except ValueError:
                print("Error parsing JSON")
                return
            if "arena" in message:
                self.arena = message

    def draw(self):
        self.axes.clear()
        if self.arena:
            for line in self.arena["arena"]:
                self.axes.plot(
                    [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
                )

    async def main(self):
        plt.ion()
        await self.ble_connection.connect()
        try:
            request = json.dumps({"command": "arena"}).encode()
            print(f"Sending request for arena: {request}")
            await self.ble_connection.send_uart_data(request)
            self.fig.canvas.mpl_connect("close_event", self.handle_close)
            while not self.closed:
                self.draw()
                plt.draw()
                plt.pause(0.05)
                await asyncio.sleep(0.01)
        finally:
            await self.ble_connection.close()

最后，创建类的实例并启动主循环：

robot_display = RobotDisplay()
asyncio.run(robot_display.main())

操作步骤如下：
1. 将机器人文件夹发送到 Raspberry Pi Pico。
2. 打开电池电源。
3. 在计算机上运行以下命令启动显示代码：

python3 display_from_robot.py

1.3 代码逻辑流程图

graph TD;
    A[开始] --> B[初始化RobotDisplay类];
    B --> C[连接蓝牙];
    C --> D[发送请求获取竞技场数据];
    D --> E[监听数据];
    E --> F{是否接收到数据};
    F -- 是 --> G[解析数据];
    G --> H{是否包含竞技场数据};
    H -- 是 --> I[更新竞技场数据];
    I --> J[绘制竞技场];
    F -- 否 --> E;
    H -- 否 --> E;
    J --> K{窗口是否关闭};
    K -- 否 --> E;
    K -- 是 --> L[关闭蓝牙连接];
    L --> M[结束];

2. 姿态设置与显示

2.1 随机姿态的创建

在 robot/code.py 中，我们使用 NumPy 库创建随机姿态。首先进行导入：

import asyncio
import json
import random
from ulab import numpy as np

然后定义 Simulation 类来存储姿态：

class Simulation:
    def __init__(self):
        self.population_size = 20
        self.poses = np.array(
            [(
                int(random.uniform(0, arena.width)),
                int(random.uniform(0, arena.height)),
                int(random.uniform(0, 360))
            ) for _ in range(self.population_size)],
            dtype=np.float
        )

2.2 姿态数据的发送

定义一个函数来发送姿态数据：

def send_poses(samples):
    send_json({
        "poses": np.array(samples[:, :2], dtype=np.int16).tolist()
    })

2.3 命令处理程序

async def command_handler(simulation):
    print("Starting handler")
    while True:
        if robot.uart.in_waiting:
            request = read_json()
            print("Received: ", request)
            if request["command"] == "arena":
                send_json({
                    "arena": arena.boundary_lines
                })
                send_poses(simulation.poses)
        await asyncio.sleep(0.1)

simulation = Simulation()
asyncio.run(command_handler(simulation))

2.4 计算机端的姿态显示

在 display_from_robot.py 中进行修改以显示姿态：

import asyncio
import json
import numpy as np
import matplotlib.pyplot as plt

class RobotDisplay:
    def __init__(self):
        self.ble_connection = BleConnection(self.handle_data)
        self.buffer = ""
        self.arena = {}
        self.poses = None
        self.closed = False
        self.fig, self.axes = plt.subplots()

    def handle_data(self, data):
        self.buffer += data.decode()
        while "\n" in self.buffer:
            line, self.buffer = self.buffer.split("\n", 1)
            print(f"Received data: {line}")
            try:
                message = json.loads(line)
            except ValueError:
                print("Error parsing JSON")
                return
            if "arena" in message:
                self.arena = message
            if "poses" in message:
                self.poses = np.array(message["poses"], dtype=np.int16)

    def draw(self):
        self.axes.clear()
        if self.arena:
            for line in self.arena["arena"]:
                self.axes.plot(
                    [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
                )
        if self.poses is not None:
            self.axes.scatter(self.poses[:, 0], self.poses[:, 1], color="blue")

    async def main(self):
        plt.ion()
        await self.ble_connection.connect()
        try:
            request = json.dumps({"command": "arena"}).encode()
            print(f"Sending request for arena: {request}")
            await self.ble_connection.send_uart_data(request)
            self.fig.canvas.mpl_connect("close_event", self.handle_close)
            while not self.closed:
                self.draw()
                plt.draw()
                plt.pause(0.05)
                await asyncio.sleep(0.01)
        finally:
            await self.ble_connection.close()

robot_display = RobotDisplay()
asyncio.run(robot_display.main())

2.5 姿态设置与显示流程表格

步骤	操作	代码文件
1	导入必要的库	robot/code.py
2	定义 Simulation 类创建随机姿态	robot/code.py
3	定义 send_poses 函数发送姿态数据	robot/code.py
4	实现命令处理程序	robot/code.py
5	在 display_from_robot.py 中导入 NumPy 库	display_from_robot.py
6	修改 RobotDisplay 类以处理姿态数据	display_from_robot.py
7	修改 draw 方法显示姿态	display_from_robot.py

3. 碰撞避免与机器人移动

3.1 距离传感器跟踪器

在 robot/code.py 中定义 DistanceSensorTracker 类来跟踪距离传感器数据：

class DistanceSensorTracker:
    def __init__(self):
        robot.left_distance.distance_mode = 2
        robot.right_distance.distance_mode = 2
        self.left = 300
        self.right = 300

    async def main(self):
        robot.left_distance.start_ranging()
        robot.right_distance.start_ranging()
        while True:
            if robot.left_distance.data_ready and robot.left_distance.distance:
                self.left = robot.left_distance.distance * 10
                robot.left_distance.clear_interrupt()
            if robot.right_distance.data_ready and robot.right_distance.distance:
                self.right = robot.right_distance.distance * 10
                robot.right_distance.clear_interrupt()
            await asyncio.sleep(0.01)

3.2 碰撞避免器

定义 CollisionAvoid 类来实现碰撞避免：

class CollisionAvoid:
    def __init__(self, distance_sensors):
        self.speed = 0.6
        self.distance_sensors = distance_sensors

    async def main(self):
        while True:
            robot.set_right(self.speed)
            while self.distance_sensors.left < 300 or self.distance_sensors.right < 300:
                robot.set_left(-self.speed)
                await asyncio.sleep(0.3)
            robot.set_left(self.speed)
            await asyncio.sleep(0)

3.3 模拟主方法

在 Simulation 类中添加传感器和碰撞避免器，并实现主方法：

class Simulation:
    def __init__(self):
        self.population_size = 20
        self.poses = np.array(
            [(
                int(random.uniform(0, arena.width)),
                int(random.uniform(0, arena.height)),
                int(random.uniform(0, 360))
            ) for _ in range(self.population_size)],
            dtype=np.float
        )
        self.distance_sensors = DistanceSensorTracker()
        self.collision_avoider = CollisionAvoid(self.distance_sensors)

    async def main(self):
        asyncio.create_task(self.distance_sensors.main())
        collision_avoider = asyncio.create_task(self.collision_avoider.main())
        try:
            while True:
                await asyncio.sleep(0.1)
                send_poses(self.poses)
        finally:
            collision_avoider.cancel()
            robot.stop()

3.4 命令处理程序的扩展

async def command_handler(simulation):
    print("Starting handler")
    simulation_task = None
    while True:
        if robot.uart.in_waiting:
            request = read_json()
            print("Received: ", request)
            if request["command"] == "arena":
                send_json({
                    "arena": arena.boundary_lines
                })
                send_poses(simulation.poses)
            elif request["command"] == "start":
                if not simulation_task:
                    simulation_task = asyncio.create_task(simulation.main())
        await asyncio.sleep(0.1)

simulation = Simulation()
asyncio.run(command_handler(simulation))

3.5 碰撞避免与机器人移动流程图

graph TD;
    A[开始命令处理程序] --> B{是否接收到请求};
    B -- 是 --> C{请求命令是否为arena};
    C -- 是 --> D[发送竞技场数据和姿态数据];
    C -- 否 --> E{请求命令是否为start};
    E -- 是 --> F{模拟任务是否已启动};
    F -- 否 --> G[启动模拟主任务];
    F -- 是 --> B;
    E -- 否 --> B;
    D --> B;
    G --> H[启动距离传感器跟踪器];
    H --> I[启动碰撞避免器];
    I --> J[循环发送姿态数据];
    J --> K{是否出现错误};
    K -- 是 --> L[取消碰撞避免器任务并停止机器人];
    K -- 否 --> J;
    L --> M[结束];
    B -- 否 --> B;

4. 计算机端启动按钮的添加

4.1 导入必要的库

在 display_from_robot.py 中添加以下导入：

from matplotlib.widgets import Button

4.2 发送命令的辅助方法

在 RobotDisplay 类中添加 send_command 方法：

async def send_command(self, command):
    request = (json.dumps({"command": command})).encode()
    print(f"Sending request: {request}")
    await self.ble_connection.send_uart_data(request)

4.3 启动按钮处理程序

def start(self, _):
    self.button_task = asyncio.create_task(self.send_command("start"))

4.4 修改主方法

async def main(self):
    plt.ion()
    await self.ble_connection.connect()
    try:
        await self.send_command("arena")
        self.fig.canvas.mpl_connect("close_event", self.handle_close)
        start_button = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Start")
        start_button.on_clicked(self.start)
        while not self.closed:
            self.draw()
            plt.draw()
            plt.pause(0.05)
            await asyncio.sleep(0.01)
    finally:
        await self.ble_connection.close()

4.5 添加启动按钮操作步骤

步骤	操作	代码文件
1	导入 Button 类	display_from_robot.py
2	在 RobotDisplay 类中添加 send_command 方法	display_from_robot.py
3	实现 start 按钮处理程序	display_from_robot.py
4	修改 main 方法添加启动按钮	display_from_robot.py

5. 编码器驱动的姿态移动

5.1 底盘数据的存储

在 robot/robot.py 中存储底盘相关数据：

ticks_per_revolution = encoder_poles * gear_ratio
ticks_to_mm = wheel_circumference_mm / ticks_per_revolution
ticks_to_m = ticks_to_mm / 1000
m_to_ticks = 1 / ticks_to_m
wheelbase_mm = 170

5.2 编码器数据到运动的转换

在 Simulation 类中添加 convert_odometry_to_motion 方法：

def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
    left_mm = left_encoder_delta * robot.ticks_to_mm
    right_mm = right_encoder_delta * robot.ticks_to_mm
    if left_mm == right_mm:
        return 0, left_mm, 0
    radius = (robot.wheelbase_mm / 2) * (left_mm + right_mm) / (right_mm - left_mm)
    d_theta = (right_mm - left_mm) / robot.wheelbase_mm
    arc_length = d_theta * radius
    rot1 = np.degrees(d_theta / 2)
    rot2 = rot1
    return rot1, arc_length, rot2

5.3 姿态移动方法

def move_poses(self, rot1, trans, rot2):
    self.poses[:, 2] += rot1
    rot1_radians = np.radians(self.poses[:, 2])
    self.poses[:, 0] += trans * np.cos(rot1_radians)
    self.poses[:, 1] += trans * np.sin(rot1_radians)
    self.poses[:, 2] += rot2
    self.poses[:, 2] = np.array([float(theta % 360) for theta in self.poses[:, 2]])

5.4 运动模型方法

def motion_model(self):
    new_encoder_left = robot.left_encoder.read()
    new_encoder_right = robot.right_encoder.read()
    left_encoder_delta = new_encoder_left - self.last_encoder_left
    right_encoder_delta = new_encoder_right - self.last_encoder_right
    rot1, trans, rot2 = self.convert_odometry_to_motion(left_encoder_delta, right_encoder_delta)
    self.move_poses(rot1, trans, rot2)
    self.last_encoder_left = new_encoder_left
    self.last_encoder_right = new_encoder_right

5.5 编码器驱动姿态移动流程图

graph TD;
    A[开始运动模型] --> B[读取最新编码器读数];
    B --> C[计算编码器增量];
    C --> D[将编码器增量转换为运动];
    D --> E[移动姿态];
    E --> F[更新最后编码器读数];
    F --> G[结束];

通过以上步骤，我们实现了机器人的空间建模、姿态跟踪、碰撞避免和运动模拟，并且可以在计算机上直观地显示机器人的相关信息。

6. 关键代码总结与注意事项

6.1 关键代码文件及功能总结

代码文件	主要功能	关键类和函数
robot/code.py	管理机器人姿态、传感器数据、碰撞避免和命令处理	Simulation 类、 DistanceSensorTracker 类、 CollisionAvoid 类、 send_poses 函数、 command_handler 函数
display_from_robot.py	在计算机上显示机器人的竞技场和姿态数据，并提供启动按钮	RobotDisplay 类、 send_command 函数、 start 函数
robot/robot.py	存储底盘相关数据	存储 ticks_per_revolution 、 ticks_to_mm 、 ticks_to_m 、 m_to_ticks 、 wheelbase_mm 等变量

6.2 注意事项

• 传感器设置 ：在 DistanceSensorTracker 类中，要确保传感器模式设置正确，以适应竞技场的大小。同时，在传感器读取数据时，要注意处理可能出现的错误，如数据未准备好等情况。
• 姿态数据处理 ：在发送和接收姿态数据时，要注意数据类型的转换，如将数据转换为 int16 类型以减少数据传输量。同时，在处理姿态角度时，要将角度约束在 0 到 360 度之间。
• 异步任务管理 ：在使用 asyncio 进行异步编程时，要注意任务的创建、取消和异常处理，确保程序的稳定性。例如，在 Simulation 类的 main 方法中，要正确处理可能出现的错误，取消碰撞避免器任务并停止机器人。

7. 常见问题及解决方法

7.1 距离传感器显示错误

如果距离传感器显示错误，可能是传感器的接线出现问题。可以按照以下步骤进行排查：
1. 检查传感器的连接是否牢固，确保没有松动或接触不良的情况。
2. 参考相关文档，检查传感器的配置是否正确，如传感器模式的设置。
3. 进行传感器的测试，使用测试代码验证传感器是否正常工作。

7.2 机器人转向过度并被困在角落

如果机器人转向过度并被困在角落，可能是因为在碰撞避免时，电机的反向转动时间过长。可以通过以下方法解决：
1. 降低 CollisionAvoid 类中 await asyncio.sleep(0.3) 的时间，减少电机反向转动的时间。
2. 调整机器人的运动策略，例如增加传感器的检测范围或调整转向角度。

7.3 机器人运动速度过快

如果机器人运动速度过快，可能会导致碰撞或姿态跟踪不准确。可以采取以下措施：
1. 降低 CollisionAvoid 类中的 self.speed 值，减小机器人的运动速度。
2. 更换具有更大齿轮比的电机，如推荐的 298:1 齿轮比，以降低机器人的运动速度。

常见问题及解决方法表格

问题描述	可能原因	解决方法
距离传感器显示错误	传感器接线问题、配置错误	检查连接、参考文档配置、进行测试
机器人转向过度并被困在角落	电机反向转动时间过长	降低反向转动时间、调整运动策略
机器人运动速度过快	速度设置过高、电机齿轮比不合适	降低速度设置、更换电机

8. 代码优化建议

8.1 数据处理优化

• 在处理传感器数据时，可以增加数据滤波算法，如卡尔曼滤波，以提高数据的准确性和稳定性。
• 在处理姿态数据时，可以采用更高效的数据结构和算法，减少内存占用和计算时间。

8.2 异步任务优化

• 可以使用 asyncio.gather 函数同时运行多个异步任务，提高程序的并发性能。
• 对异步任务进行合理的调度和管理，避免任务之间的冲突和资源竞争。

8.3 代码结构优化

• 将一些通用的功能封装成独立的函数或类，提高代码的复用性和可维护性。
• 增加代码注释，提高代码的可读性，方便后续的开发和维护。

代码优化建议列表

1. 数据处理 ：增加数据滤波算法、采用高效数据结构和算法。
2. 异步任务 ：使用 asyncio.gather 函数、合理调度和管理任务。
3. 代码结构 ：封装通用功能、增加代码注释。

9. 总结与展望

9.1 总结

通过以上的步骤和代码实现，我们完成了机器人的空间建模、姿态跟踪、碰撞避免和运动模拟，并在计算机上实现了数据的可视化显示。在这个过程中，我们使用了蓝牙通信、异步编程、传感器数据处理等技术，解决了一些常见的问题，如传感器错误、机器人运动异常等。

9.2 展望

未来，我们可以进一步扩展这个系统的功能，例如：
- 增加更多的传感器，如视觉传感器，以获取更丰富的环境信息。
- 实现更复杂的运动规划算法，如路径规划和避障算法，提高机器人的自主性和智能性。
- 开发更友好的用户界面，方便用户对机器人进行控制和监控。

系统功能扩展展望表格

扩展方向	具体功能
传感器扩展	增加视觉传感器、获取更丰富环境信息
运动规划	实现路径规划、避障算法
用户界面	开发友好用户界面、方便控制和监控

10. 流程图总结

10.1 整体流程回顾

为了更清晰地展示整个系统的工作流程，我们将之前的流程图进行整合。以下是整体的 mermaid 流程图：

graph LR;
    A[开始] --> B[初始化RobotDisplay类];
    B --> C[连接蓝牙];
    C --> D[发送请求获取竞技场数据];
    D --> E[监听数据];
    E --> F{是否接收到数据};
    F -- 是 --> G[解析数据];
    G --> H{是否包含竞技场数据};
    H -- 是 --> I[更新竞技场数据];
    I --> J[绘制竞技场];
    F -- 否 --> E;
    H -- 否 --> E;
    J --> K{是否接收到start命令};
    K -- 是 --> L[启动模拟主任务];
    K -- 否 --> E;
    L --> M[启动距离传感器跟踪器];
    M --> N[启动碰撞避免器];
    N --> O[循环发送姿态数据];
    O --> P[读取最新编码器读数];
    P --> Q[计算编码器增量];
    Q --> R[将编码器增量转换为运动];
    R --> S[移动姿态];
    S --> T[更新最后编码器读数];
    T --> O;
    O --> U{是否出现错误};
    U -- 是 --> V[取消碰撞避免器任务并停止机器人];
    U -- 否 --> O;
    V --> W[关闭蓝牙连接];
    W --> X[结束];
    J --> Y{窗口是否关闭};
    Y -- 是 --> W;
    Y -- 否 --> E;

这个流程图展示了从系统启动到机器人运动模拟的整个过程，包括数据的获取、处理、显示，以及机器人的运动控制和姿态更新。通过这个流程图，我们可以更直观地理解系统的工作原理和各个模块之间的交互关系。

通过以上的内容，我们全面地介绍了机器人的空间建模、姿态跟踪、碰撞避免和运动模拟的实现过程，包括代码实现、常见问题解决、优化建议和未来展望等方面。希望这些内容对大家有所帮助，能够进一步推动机器人技术的发展和应用。