移动机器人自主导航 + 操作

移动操作（Mobile Manipulation）是Embodied AI的重要应用。本文讲解如何构建一个能够自主导航到目标位置并执行机械臂操作的移动机器人。

系统架构

┌─────────────────────────────────────────────┐
│              移动操作机器人                   │
├─────────────────────────────────────────────┤
│  感知层：激光雷达 + 深度相机 + IMU            │
├─────────────────────────────────────────────┤
│  定位层：SLAM（建图与定位）                   │
├─────────────────────────────────────────────┤
│  规划层：全局路径规划 + 局部避障               │
├─────────────────────────────────────────────┤
│  控制层：底盘运动 + 机械臂控制                │
└─────────────────────────────────────────────┘

搭建移动操作平台

from isaacsim.core import World
from isaacsim.assets import MobileManipulator

world = World()

# 创建移动操作机器人
robot = MobileManipulator(
    prim_path="/World/MobileManipulator",
    base_type="differential",  # 差速底盘
    arm_type="franka",          # Franka机械臂
    position=np.array([0.0, 0.0, 0.0])
)
world.scene.add(robot)
world.reset()

SLAM建图

import rclpy
from nav2_msgs.srv import MapSaver

class SLAMManager:
    def __init__(self, node):
        self.node = node

        # 创建SLAM节点
        self.slam_node = rclpy.create_node('slam_node')

    def start_slam(self):
        """启动SLAM建图"""
        # 使用slam_toolbox
        self.slam_process = subprocess.Popen([
            'ros2', 'launch', 'slam_toolbox', 'online_async_launch.py',
            'slam_params_file:=/path/to/slam_params.yaml'
        ])

    def save_map(self, map_name):
        """保存地图"""
        saver = self.slam_node.create_client(MapSaver, '/map_saver/save_map')
        request = MapSaver.Request()
        request.map_topic = '/map'
        request.map_url = map_name

        future = saver.call_async(request)
        rclpy.spin_until_future_complete(self.slam_node, future)

        return future.result()

    def stop_slam(self):
        """停止SLAM"""
        self.slam_process.terminate()

自主导航

from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

class Navigator:
    def __init__(self):
        self.navigator = BasicNavigator()

        # 等待导航启动
        self.navigator.waitUntilNav2Active()

    def navigate_to_pose(self, x, y, theta):
        """导航到目标位姿"""
        from geometry_msgs.msg import PoseStamped

        goal_pose = PoseStamped()
        goal_pose.header.frame_id = 'map'
        goal_pose.header.stamp = self.navigator.get_clock().now().to_msg()
        goal_pose.pose.position.x = x
        goal_pose.pose.position.y = y
        goal_pose.pose.orientation.z = np.sin(theta / 2)
        goal_pose.pose.orientation.w = np.cos(theta / 2)

        # 发送导航目标
        self.navigator.goToPose(goal_pose)

        # 等待完成
        while not self.navigator.isTaskComplete():
            feedback = self.navigator.getFeedback()
            if feedback:
                distance = feedback.distance_remaining
                self.navigator.get_logger().info(
                    f'距离目标: {distance:.2f}m'
                )

        # 检查结果
        result = self.navigator.getResult()
        if result == TaskResult.SUCCEEDED:
            return True
        else:
            return False

    def navigate_through_poses(self, waypoints):
        """通过多个路点导航"""
        poses = []
        for x, y, theta in waypoints:
            pose = PoseStamped()
            pose.header.frame_id = 'map'
            pose.pose.position.x = x
            pose.pose.position.y = y
            pose.pose.orientation.z = np.sin(theta / 2)
            pose.pose.orientation.w = np.cos(theta / 2)
            poses.append(pose)

        self.navigator.goThroughPoses(poses)

        while not self.navigator.isTaskComplete():
            pass

        return self.navigator.getResult() == TaskResult.SUCCEEDED

移动操作协调

class MobileManipulationController:
    def __init__(self, robot, navigator):
        self.robot = robot
        self.navigator = navigator

    def execute_task(self, target_object_pose):
        """执行完整的移动操作任务"""
        # 1. 导航到抓取位置（物体前方0.5m）
        approach_pose = self._compute_approach_pose(target_object_pose)
        success = self.navigator.navigate_to_pose(*approach_pose)

        if not success:
            print("导航失败")
            return False

        # 2. 停止底盘，锁定位置
        self.robot.lock_base()

        # 3. 执行机械臂抓取
        grasp_success = self._execute_grasp(target_object_pose)

        if not grasp_success:
            print("抓取失败")
            return False

        # 4. 导航到放置位置
        place_location = (2.0, 1.0, 0.0)  # 目标放置区域
        success = self.navigator.navigate_to_pose(*place_location)

        if not success:
            print("导航到放置位置失败")
            return False

        # 5. 放置物体
        self.robot.lock_base()
        self._execute_place()

        return True

    def _compute_approach_pose(self, object_pose):
        """计算接近物体的位姿"""
        x = object_pose[0] - 0.5  # 物体前方0.5m
        y = object_pose[1]
        theta = 0.0  # 面向物体
        return (x, y, theta)

    def _execute_grasp(self, object_pose):
        """执行抓取"""
        # 视觉定位物体精确位置
        precise_pose = self._visual_servoing(object_pose)

        # 规划抓取轨迹
        trajectory = self.robot.plan_grasp(precise_pose)

        # 执行抓取
        self.robot.follow_trajectory(trajectory)
        self.robot.close_gripper()

        return True

    def _visual_servoing(self, initial_pose):
        """视觉伺服精确定位"""
        for _ in range(20):
            # 获取相机图像
            image = self.robot.get_wrist_camera_image()

            # 检测物体
            detection = self._detect_object(image)

            if detection is None:
                break

            # 计算位姿偏差
            error = self._compute_pose_error(detection)

            if np.linalg.norm(error) < 0.01:
                break

            # 微调机械臂
            self.robot.move_delta(error * 0.5)

        return self.robot.get_end_effector_pose()

完整任务流程

def main():
    # 初始化
    rclpy.init()
    world = World()
    setup_scene(world)

    robot = MobileManipulator(...)
    navigator = Navigator()
    controller = MobileManipulationController(robot, navigator)

    # 任务1：建图
    slam = SLAMManager(rclpy.node)
    slam.start_slam()

    # 遥控机器人探索环境
    while not map_complete:
        teleop_step()

    slam.save_map("office_map")
    slam.stop_slam()

    # 任务2：执行移动操作
    target_object = np.array([3.0, 2.0, 0.1, 0.0, 0.0, 0.0])  # 6D位姿

    success = controller.execute_task(target_object)

    if success:
        print("任务完成！")
    else:
        print("任务失败")

    rclpy.shutdown()

if __name__ == "__main__":
    main()

FAQ

移动底盘和机械臂如何协调？

使用lock_base()在操作时锁定底盘，避免相互干扰。复杂任务需要全身运动规划。

导航精度不够怎么办？

检查激光雷达标定、地图质量。可以在接近目标时使用视觉伺服精确定位。

如何处理动态障碍物？

使用局部路径规划器（如DWA、TEB），它们会实时避障。