教程 6：四轮机器人（90分钟）

目标与简介

本教程将教你如何创建一个四轮机器人，并编写控制器来控制其运动。你将学习如何设计机器人的机械结构、配置传感器和执行器，以及实现各种运动模式。本教程基于官方教程6。

1. 环境准备

在开始本教程之前，请确保你已经完成了前面的教程，并且熟悉基本的Webots操作。我们将创建一个新的环境来学习四轮机器人的设计和控制。

📝 动手实践 #1：创建一个新的Webots项目，命名为 four_wheeled_robot。在场景中添加一个 RectangleArena 作为基础环境。确保仿真处于暂停状态，虚拟时间计数器显示 0:00:00:000。

2. 机器人设计

四轮机器人通常由车身、四个轮子和各种传感器组成。我们将创建一个简单但功能完整的四轮机器人。

📝 动手实践 #2：创建机器人主体：

在场景树中选中 RectangleArena，点击按钮
选择 PROTO nodes (Webots Projects) / robots / generic / FourWheelsRobot (Robot)
设置 translation 为 0 0 0.1
设置 rotation 为 0 0 1 0
观察机器人的结构：车身和四个轮子

📝 动手实践 #3：自定义机器人外观：

展开 FourWheelsRobot 节点，找到 body 字段
在 body 的 appearance 中，设置 diffuseColor 为 0.2 0.6 1.0（蓝色）
设置 shininess 为 0.7
为轮子设置不同的颜色，例如 0.3 0.3 0.3（深灰色）

3. 轮子配置

四轮机器人的轮子需要正确配置才能实现有效的运动控制。每个轮子都有自己的电机和编码器。

📝 动手实践 #4：配置轮子参数：

展开 FourWheelsRobot，找到 wheels 字段
设置每个轮子的 radius 为 0.05（5厘米）
设置 wheelDistance 为 0.2（20厘米）
设置 wheelThickness 为 0.02（2厘米）
确保所有轮子的 type 都设置为 "motorized"

📝 动手实践 #5：设置轮子物理属性：

为每个轮子设置 mass 为 0.1（100克）
设置 centerOfMass 为 0 0 0
设置 inertiaMatrix 为 0.0001 0.0001 0.0001 0 0 0
设置 damping 为 0.1
配置碰撞几何体为圆柱体

4. 传感器配置

为了能够感知环境并做出相应的反应，我们需要为机器人添加各种传感器。

📝 动手实践 #6：添加距离传感器：

在 FourWheelsRobot 中添加 DistanceSensor 节点
设置 name 为 "ds_front"
设置 translation 为 0.15 0 0.05（前方）
设置 rotation 为 0 1 0 0（向前）
设置 lookupTable 为 [0 4096 0.1, 0.5 4096 0.1, 1 0 0.1]

📝 动手实践 #7：添加编码器传感器：

为每个轮子添加 PositionSensor 节点
设置 name 分别为 "wheel1_encoder"、"wheel2_encoder" 等
设置 resolution 为 4096（每转4096个脉冲）
确保所有编码器都正确连接到对应的轮子

5. 控制器编程

现在我们需要编写Python控制器来控制机器人的运动。控制器将处理传感器数据并控制轮子电机。

📝 动手实践 #8：创建基础控制器：

from controller import Robot, Motor, DistanceSensor, PositionSensor
import math

TIME_STEP = 64

class FourWheeledRobot:
    def __init__(self):
        self.robot = Robot()
        self.time_step = TIME_STEP
        
        # 获取轮子电机
        self.wheel1 = self.robot.getDevice('wheel1')
        self.wheel2 = self.robot.getDevice('wheel2')
        self.wheel3 = self.robot.getDevice('wheel3')
        self.wheel4 = self.robot.getDevice('wheel4')
        
        # 获取距离传感器
        self.ds_front = self.robot.getDevice('ds_front')
        self.ds_front.enable(TIME_STEP)
        
        # 获取编码器
        self.encoder1 = self.robot.getDevice('wheel1_encoder')
        self.encoder2 = self.robot.getDevice('wheel2_encoder')
        self.encoder3 = self.robot.getDevice('wheel3_encoder')
        self.encoder4 = self.robot.getDevice('wheel4_encoder')
        
        # 启用编码器
        self.encoder1.enable(TIME_STEP)
        self.encoder2.enable(TIME_STEP)
        self.encoder3.enable(TIME_STEP)
        self.encoder4.enable(TIME_STEP)
        
        # 设置电机为速度控制模式
        self.wheel1.setPosition(float('inf'))
        self.wheel2.setPosition(float('inf'))
        self.wheel3.setPosition(float('inf'))
        self.wheel4.setPosition(float('inf'))
        
        # 设置初始速度
        self.wheel1.setVelocity(0.0)
        self.wheel2.setVelocity(0.0)
        self.wheel3.setVelocity(0.0)
        self.wheel4.setVelocity(0.0)
    
    def set_wheel_velocities(self, v1, v2, v3, v4):
        """设置四个轮子的速度"""
        self.wheel1.setVelocity(v1)
        self.wheel2.setVelocity(v2)
        self.wheel3.setVelocity(v3)
        self.wheel4.setVelocity(v4)
    
    def move_forward(self, speed=2.0):
        """向前移动"""
        self.set_wheel_velocities(speed, speed, speed, speed)
    
    def move_backward(self, speed=2.0):
        """向后移动"""
        self.set_wheel_velocities(-speed, -speed, -speed, -speed)
    
    def turn_left(self, speed=1.0):
        """左转"""
        self.set_wheel_velocities(-speed, speed, -speed, speed)
    
    def turn_right(self, speed=1.0):
        """右转"""
        self.set_wheel_velocities(speed, -speed, speed, -speed)
    
    def stop(self):
        """停止"""
        self.set_wheel_velocities(0.0, 0.0, 0.0, 0.0)
    
    def run(self):
        """主运行循环"""
        while self.robot.step(self.time_step) != -1:
            # 获取距离传感器值
            distance = self.ds_front.getValue()
            
            # 简单的避障行为
            if distance < 0.3:  # 如果前方障碍物距离小于30cm
                self.turn_right(1.5)
            else:
                self.move_forward(2.0)

# 创建并运行机器人
robot = FourWheeledRobot()
robot.run()

6. 运动控制

四轮机器人可以实现多种运动模式，包括直线运动、转弯、原地旋转等。让我们实现更复杂的运动控制算法。

📝 动手实践 #9：实现差速转向：

def differential_drive(self, linear_speed, angular_speed):
    """差速转向控制
    
    Args:
        linear_speed: 线速度 (m/s)
        angular_speed: 角速度 (rad/s)
    """
    # 机器人参数
    wheel_radius = 0.05  # 轮子半径 (m)
    wheel_distance = 0.2  # 轮距 (m)
    
    # 计算左右轮速度
    left_speed = (linear_speed - angular_speed * wheel_distance / 2) / wheel_radius
    right_speed = (linear_speed + angular_speed * wheel_distance / 2) / wheel_radius
    
    # 设置轮子速度 (前左, 前右, 后左, 后右)
    self.set_wheel_velocities(left_speed, right_speed, left_speed, right_speed)

def follow_path(self):
    """路径跟踪"""
    target_x = 5.0  # 目标x坐标
    target_y = 0.0  # 目标y坐标
    
    while self.robot.step(self.time_step) != -1:
        # 获取当前位置 (简化版本)
        current_x = 0.0  # 实际应用中需要从GPS或编码器获取
        current_y = 0.0
        
        # 计算误差
        error_x = target_x - current_x
        error_y = target_y - current_y
        
        # 简单的P控制器
        linear_speed = 2.0
        angular_speed = 0.5 * math.atan2(error_y, error_x)
        
        self.differential_drive(linear_speed, angular_speed)

7. 高级功能

我们可以为四轮机器人添加更多高级功能，如SLAM导航、多传感器融合、复杂路径规划等。

📝 动手实践 #10：添加GPS和指南针传感器：

在机器人中添加 GPS 节点
设置 name 为 "gps"
设置 translation 为 0 0 0.1
添加 Compass 节点
设置 name 为 "compass"
启用这些传感器并读取数据

📝 动手实践 #11：实现巡逻行为：

def patrol_behavior(self):
    """巡逻行为"""
    patrol_points = [
        (0, 0), (5, 0), (5, 5), (0, 5)
    ]
    current_point = 0
    
    while self.robot.step(self.time_step) != -1:
        # 获取当前位置
        gps_values = self.gps.getValues()
        current_x, current_y = gps_values[0], gps_values[1]
        
        # 获取目标点
        target_x, target_y = patrol_points[current_point]
        
        # 计算距离
        distance = math.sqrt((target_x - current_x)**2 + (target_y - current_y)**2)
        
        if distance < 0.5:  # 到达目标点
            current_point = (current_point + 1) % len(patrol_points)
            self.stop()
            self.robot.step(1000)  # 停留1秒
        else:
            # 计算朝向目标的角度
            angle = math.atan2(target_y - current_y, target_x - current_x)
            
            # 获取当前朝向
            compass_values = self.compass.getValues()
            current_heading = math.atan2(compass_values[1], compass_values[0])
            
            # 计算角度差
            angle_diff = angle - current_heading
            
            # 标准化角度到[-π, π]
            while angle_diff > math.pi:
                angle_diff -= 2 * math.pi
            while angle_diff < -math.pi:
                angle_diff += 2 * math.pi
            
            # 控制运动
            linear_speed = 1.0
            angular_speed = 2.0 * angle_diff
            
            self.differential_drive(linear_speed, angular_speed)

8. 总结

恭喜你完成了Webots四轮机器人教程！你已经学会了：

如何设计和配置四轮机器人的机械结构
如何正确配置轮子电机和编码器
如何添加和配置各种传感器（距离传感器、GPS、指南针等）
如何编写Python控制器来控制机器人运动
如何实现差速转向和路径跟踪
如何实现复杂的机器人行为（避障、巡逻等）

这些技能将帮助你创建功能强大的四轮机器人仿真。在下一个教程中，我们将学习Webots的高级功能。

下一步

继续学习教程7：高级功能，了解Webots的更多高级特性和应用。

教程 6：四轮机器人 （90分钟）