开源底盘+机械臂机器人：Lekiwi驱动链路分析

系统架构

硬件组成

Lekiwi是一个底盘+机械臂的结构。

• 机械臂： 6个自由度（shoulder_pan, shoulder_lift, elbow_flex, wrist_flex, wrist_roll, gripper）
• 移动底盘：3个全向轮，三轮全向移动（left_wheel, back_wheel, right_wheel）

github官网Lekiwi使用 Koch v1.1 机械臂、U2D2 电机控制器和 Dynamixel XL430 电机作为移动基座。我这里买到的使用的是feetech电机，机械臂和底盘一共9个motor接入到一个串口总线上，对于机械臂和底盘移动只需要通过一个串口总线进行。

软件架构

我这里将软件架构分为3层。

• 应用层：对设备的操作，实例化设备一个设备后，对设备进行连接，移动控制，观测数据获取。
• 总线层：实现一个MotorBus基类，对设备的一些操作进行统一定义、约束。实现操作的逻辑，具体的实现由继承设备来实现。
• 设备层：具体设备的实现，继承与MotorBus，实现对电机底层通信接口。

驱动链路

初始化

class LeKiwi(Robot):
    config_class = LeKiwiConfig
    name = "lekiwi"

    def __init__(self, config: LeKiwiConfig):
        super().__init__(config)
        self.config = config
        norm_mode_body = MotorNormMode.DEGREES if config.use_degrees else MotorNormMode.RANGE_M100_100
        self.bus = FeetechMotorsBus(
            port=self.config.port,
            motors={
                # arm
                "arm_shoulder_pan": Motor(1, "sts3215", norm_mode_body),
                "arm_shoulder_lift": Motor(2, "sts3215", norm_mode_body),
                "arm_elbow_flex": Motor(3, "sts3215", norm_mode_body),
                "arm_wrist_flex": Motor(4, "sts3215", norm_mode_body),
                "arm_wrist_roll": Motor(5, "sts3215", norm_mode_body),
                "arm_gripper": Motor(6, "sts3215", MotorNormMode.RANGE_0_100),
                # base
                "base_left_wheel": Motor(7, "sts3215", MotorNormMode.RANGE_M100_100),
                "base_back_wheel": Motor(8, "sts3215", MotorNormMode.RANGE_M100_100),
                "base_right_wheel": Motor(9, "sts3215", MotorNormMode.RANGE_M100_100),
            },
            calibration=self.calibration,
        )
        self.arm_motors = [motor for motor in self.bus.motors if motor.startswith("arm")]
        self.base_motors = [motor for motor in self.bus.motors if motor.startswith("base")]
        self.cameras = make_cameras_from_configs(config.cameras)

继承Robot，指定了配置类为LeKiwiConfig其定义了uart的端口、相机的编号等信息。将角度统一归一化到[-100,100],创建Feetech电机总线实例，创建Feetech电机型号sts3215，配置了机械臂电机ID为1至6，底盘编号为7至9编号。同时对相机也进行了初始化，用于后续的视觉观测。

连接

robot.connect()------------>

    def connect(self, calibrate: bool = True) -> None:
        if self.is_connected:
            raise DeviceAlreadyConnectedError(f"{self} already connected")

        self.bus.connect()
        if not self.is_calibrated and calibrate:
            logger.info(
                "Mismatch between calibration values in the motor and the calibration file or no calibration file found"
            )
            self.calibrate()

        for cam in self.cameras.values():
            cam.connect()

        self.configure()
        logger.info(f"{self} connected.")

初始化完成之后即可进行发起连接，连接主要是根据指定的串口好进行打开，然后进行握手验证，ping所有配置的电机，检查校准文件与电机的状态，并设置PID、加速度等参数。

校准

def calibrate(self) -> None:
        if self.calibration:
            # Calibration file exists, ask user whether to use it or run new calibration
            user_input = input(
                f"Press ENTER to use provided calibration file associated with the id {self.id}, or type 'c' and press ENTER to run calibration: "
            )
            if user_input.strip().lower() != "c":
                logger.info(f"Writing calibration file associated with the id {self.id} to the motors")
                self.bus.write_calibration(self.calibration)
                return
        logger.info(f"\nRunning calibration of {self}")

        motors = self.arm_motors + self.base_motors

        self.bus.disable_torque(self.arm_motors)
        for name in self.arm_motors:
            self.bus.write("Operating_Mode", name, OperatingMode.POSITION.value)

        input("Move robot to the middle of its range of motion and press ENTER....")
        homing_offsets = self.bus.set_half_turn_homings(self.arm_motors)

        homing_offsets.update(dict.fromkeys(self.base_motors, 0))

        full_turn_motor = [
            motor for motor in motors if any(keyword in motor for keyword in ["wheel", "wrist_roll"])
        ]
        unknown_range_motors = [motor for motor in motors if motor not in full_turn_motor]

        print(
            f"Move all arm joints except '{full_turn_motor}' sequentially through their "
            "entire ranges of motion.\nRecording positions. Press ENTER to stop..."
        )
        range_mins, range_maxes = self.bus.record_ranges_of_motion(unknown_range_motors)
        for name in full_turn_motor:
            range_mins[name] = 0
            range_maxes[name] = 4095

        self.calibration = {}
        for name, motor in self.bus.motors.items():
            self.calibration[name] = MotorCalibration(
                id=motor.id,
                drive_mode=0,
                homing_offset=homing_offsets[name],
                range_min=range_mins[name],
                range_max=range_maxes[name],
            )

        self.bus.write_calibration(self.calibration)
        self._save_calibration()
        print("Calibration saved to", self.calibration_fpath)

首次校准，从归零到测量范围，生成并保存文件流程，后续使用按回车复用已有校准；输入 'c' 重新校准。可以总结为如下：

LeKiwi.calibrate()
  → 用户选择（使用已有/重新校准）
  → 归零设置
  → 运动范围测量
  → 校准参数构建
  → FeetechMotorsBus.write_calibration()
    → MotorsBus.write_calibration()
      → 逐个写入电机寄存器

校准主要是限定几个参数，如归零偏移、运动范围、驱动模式。校准的流程是由用户选择使用已经有的校准文件直接写入校准还是进行重新启动校准流程校准。

动作执行

def send_action(self, action: dict[str, Any]) -> dict[str, Any]:

        if not self.is_connected:
            raise DeviceNotConnectedError(f"{self} is not connected.")

        arm_goal_pos = {k: v for k, v in action.items() if k.endswith(".pos")}
        base_goal_vel = {k: v for k, v in action.items() if k.endswith(".vel")}

        base_wheel_goal_vel = self._body_to_wheel_raw(
            base_goal_vel["x.vel"], base_goal_vel["y.vel"], base_goal_vel["theta.vel"]
        )

        # Cap goal position when too far away from present position.
        # /!\ Slower fps expected due to reading from the follower.
        if self.config.max_relative_target is not None:
            present_pos = self.bus.sync_read("Present_Position", self.arm_motors)
            goal_present_pos = {key: (g_pos, present_pos[key]) for key, g_pos in arm_goal_pos.items()}
            arm_safe_goal_pos = ensure_safe_goal_position(goal_present_pos, self.config.max_relative_target)
            arm_goal_pos = arm_safe_goal_pos

        # Send goal position to the actuators
        arm_goal_pos_raw = {k.replace(".pos", ""): v for k, v in arm_goal_pos.items()}
        self.bus.sync_write("Goal_Position", arm_goal_pos_raw)
        self.bus.sync_write("Goal_Velocity", base_wheel_goal_vel)

        return {**arm_goal_pos, **base_goal_vel}

输入为动作的序列，输出为实际发送的动作。首先将机械臂（后缀.pos）目标位置和底盘目标速度（后缀.vel）进行分离，如下：

# 分离前
action = {
    "arm_shoulder_pan.pos": 45.0,    # 机械臂位置
    "arm_elbow_flex.pos": -30.0,
    "x.vel": 0.1,                    # 底盘速度
    "y.vel": 0.0,
    "theta.vel": 0.05
}

# 分离后
arm_goal_pos = {
    "arm_shoulder_pan.pos": 45.0,
    "arm_elbow_flex.pos": -30.0
}

base_goal_vel = {
    "x.vel": 0.1,
    "y.vel": 0.0,
    "theta.vel": 0.05
}

然后调用_body_to_wheel_raw进行底盘运动学转换，输入为底盘坐标系速度 (x, y, θ)，输出为三轮电机速度指令。转换的时候需要进行安全限制，实际获取机械臂的关节位置，然后计算步幅（目标位置-当前位置），超过max_relative_target 则裁剪使用安全的目标位置。

最后就是调用sync_write分别写入控制机械臂和底盘。下面总结一下流程：

LeKiwi.send_action(action)
  → 动作分离（机械臂位置 + 底盘速度）
  → 底盘运动学转换
  → 安全限制检查
  → FeetechMotorsBus.sync_write()
    → MotorsBus._sync_write()
      → 批量写入电机寄存器

对Lekiwi的控制，由于底盘是3个万向轮，所以需要进行运动学转换，现将机械臂和底盘进行分离，计算之处底盘坐标系转换的3轮速度，在确保安全限制的条件下，调用sync_write进行写入，sync_write是同时写入多个电机。

状态读取

    def get_observation(self) -> dict[str, Any]:
        if not self.is_connected:
            raise DeviceNotConnectedError(f"{self} is not connected.")

        # Read actuators position for arm and vel for base
        start = time.perf_counter()
        arm_pos = self.bus.sync_read("Present_Position", self.arm_motors)
        base_wheel_vel = self.bus.sync_read("Present_Velocity", self.base_motors)

        base_vel = self._wheel_raw_to_body(
            base_wheel_vel["base_left_wheel"],
            base_wheel_vel["base_back_wheel"],
            base_wheel_vel["base_right_wheel"],
        )

        arm_state = {f"{k}.pos": v for k, v in arm_pos.items()}

        obs_dict = {**arm_state, **base_vel}

        dt_ms = (time.perf_counter() - start) * 1e3
        logger.debug(f"{self} read state: {dt_ms:.1f}ms")

        # Capture images from cameras
        for cam_key, cam in self.cameras.items():
            start = time.perf_counter()
            obs_dict[cam_key] = cam.async_read()
            dt_ms = (time.perf_counter() - start) * 1e3
            logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")

        return obs_dict

首先调用sync_read分别读取机械臂位置和底盘的当前速度，然后调用底盘速度逆运动学转换（三轮电机->底盘坐标系速度(x, y, θ)），接着将机械臂位置和底盘转换的坐标系速度整合，再次就是对相机图像的待机，先遍历所有配置相机，将图像的数据添加到观测字段，最终再进行整合得到观测字典。

obs_dict = {
    # 机械臂位置
    "arm_shoulder_pan.pos": 45.2,
    "arm_shoulder_lift.pos": -12.8,
    # ... 其他关节
    
    # 底盘速度
    "x.vel": 0.15,
    "y.vel": -0.08,
    "theta.vel": 0.12,
    
    # 相机图像
    "camera_1": numpy_array,  # (H, W, 3)
    "camera_2": numpy_array,  # (H, W, 3)
}

下面是总结流程

LeKiwi.get_observation()
  → FeetechMotorsBus.sync_read()
    → MotorsBus._sync_read()
      → 批量读取电机状态
  → 底盘速度逆运动学
  → 相机图像采集

获取状态与动作执行相反，首先读取到电机的状态，然后通过逆运动学，将底盘的速度转换为坐标系。

关于Lekiwi的驱动链路分析就到这，更多文章欢迎访问我的博客www.laumy.tech