机器人 & AI 完全指南: 从 ROS2 到人形机器人

import numpy as np

def dh_transform(a, d, alpha, theta):
    """Denavit-Hartenberg 变换矩阵计算"""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct,    -st*ca,  st*sa,  a*ct],
        [st,     ct*ca, -ct*sa,  a*st],
        [0,      sa,     ca,     d   ],
        [0,      0,      0,      1   ]
    ])

def forward_kinematics(joint_angles, dh_params):
    """
    正运动学: 关节角度 -> 末端执行器位置/姿态
    dh_params: [(a, d, alpha, theta_offset), ...]
    """
    T = np.eye(4)
    for i, theta in enumerate(joint_angles):
        a, d, alpha, theta_off = dh_params[i]
        T = T @ dh_transform(a, d, alpha, theta + theta_off)
    return T

# 2-DOF 平面机械臂示例 (每段连杆长 1.0m)
dh_params_2dof = [
    (1.0, 0, 0, 0),
    (1.0, 0, 0, 0),
]
joint_angles = [np.pi/4, np.pi/4]
T_end = forward_kinematics(joint_angles, dh_params_2dof)
print("末端执行器位置:", T_end[:3, 3])

import rclpy
from rclpy.node import Node
from std_msgs.msg import String

class TalkerNode(Node):
    def __init__(self):
        super().__init__('talker')
        self.publisher = self.create_publisher(String, 'chatter', 10)
        self.timer = self.create_timer(0.5, self.publish_msg)
        self.count = 0

    def publish_msg(self):
        msg = String()
        msg.data = f'Hello ROS2: {self.count}'
        self.publisher.publish(msg)
        self.get_logger().info(f'Published: {msg.data}')
        self.count += 1

class ListenerNode(Node):
    def __init__(self):
        super().__init__('listener')
        self.subscription = self.create_subscription(
            String, 'chatter', self.callback, 10
        )

    def callback(self, msg):
        self.get_logger().info(f'Received: {msg.data}')

def main():
    rclpy.init()
    talker = TalkerNode()
    rclpy.spin(talker)
    rclpy.shutdown()

# launch/my_robot.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='my_robot',
            executable='talker',
            name='talker_node',
            output='screen',
            parameters=[{'use_sim_time': False}],
        ),
        Node(
            package='my_robot',
            executable='listener',
            name='listener_node',
            output='screen',
        ),
    ])

import open3d as o3d
import numpy as np

def process_pointcloud(pcd_path):
    # 加载点云
    pcd = o3d.io.read_point_cloud(pcd_path)

    # 统计离群点去除
    pcd_clean, _ = pcd.remove_statistical_outlier(
        nb_neighbors=20, std_ratio=2.0
    )

    # 体素下采样 (降低分辨率)
    pcd_down = pcd_clean.voxel_down_sample(voxel_size=0.05)

    # 法线估计
    pcd_down.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(
            radius=0.1, max_nn=30
        )
    )

    # RANSAC 平面分割 (去除地面)
    plane_model, inliers = pcd_down.segment_plane(
        distance_threshold=0.01,
        ransac_n=3,
        num_iterations=1000
    )
    [a, b, c, d] = plane_model
    print(f"地面平面方程: {a:.2f}x + {b:.2f}y + {c:.2f}z + {d:.2f} = 0")

    # 只提取排除地面后的物体
    objects_pcd = pcd_down.select_by_index(inliers, invert=True)
    return objects_pcd

# 安装并运行 Cartographer ROS2
sudo apt install ros-humble-cartographer-ros

# 运行 Cartographer (以 TurtleBot3 为例)
ros2 launch turtlebot3_cartographer cartographer.launch.py

import numpy as np

def rrt_star(start, goal, obstacles, max_iter=1000, step=0.5, goal_radius=0.5):
    """
    简单的 2D RRT* 实现
    start, goal: np.array([x, y])
    obstacles: [(cx, cy, radius), ...]
    """
    nodes = [start]
    parents = [-1]
    costs = [0.0]

    def is_collision(p1, p2):
        """检查线段 p1-p2 是否与障碍物碰撞"""
        for (cx, cy, r) in obstacles:
            center = np.array([cx, cy])
            d = p2 - p1
            t = np.clip(np.dot(center - p1, d) / (np.dot(d, d) + 1e-9), 0, 1)
            closest = p1 + t * d
            if np.linalg.norm(closest - center) < r:
                return True
        return False

    for _ in range(max_iter):
        rand = goal if np.random.random() < 0.05 else np.random.uniform(-10, 10, 2)

        dists = [np.linalg.norm(np.array(n) - rand) for n in nodes]
        nearest_idx = int(np.argmin(dists))
        nearest = np.array(nodes[nearest_idx])

        direction = rand - nearest
        norm = np.linalg.norm(direction)
        if norm < 1e-9:
            continue
        new_node = nearest + step * direction / norm

        if is_collision(nearest, new_node):
            continue

        nodes.append(new_node)
        parents.append(nearest_idx)
        costs.append(costs[nearest_idx] + step)

        if np.linalg.norm(new_node - goal) < goal_radius:
            print(f"目标到达! 总节点数: {len(nodes)}")
            return nodes, parents

    return nodes, parents

# 执行示例
start = np.array([0.0, 0.0])
goal = np.array([9.0, 9.0])
obstacles = [(5.0, 5.0, 1.5), (3.0, 7.0, 1.0)]
nodes, parents = rrt_star(start, goal, obstacles)

# MoveIt2 Python API 示例
import rclpy
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState

rclpy.init()
moveit = MoveItPy(node_name='moveit_py_planning')
panda_arm = moveit.get_planning_component('panda_arm')

# 设置目标位姿
from geometry_msgs.msg import PoseStamped
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'panda_link0'
goal_pose.pose.position.x = 0.5
goal_pose.pose.position.y = 0.0
goal_pose.pose.position.z = 0.5
goal_pose.pose.orientation.w = 1.0

panda_arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='panda_hand')

# 路径规划与执行
plan_result = panda_arm.plan()
if plan_result:
    moveit.execute(plan_result.trajectory)

from ultralytics import YOLO
import cv2

# 加载 YOLOv11
model = YOLO('yolo11n.pt')

def detect_objects(frame):
    """从相机帧中检测物体"""
    results = model(frame, conf=0.5)
    annotated = results[0].plot()

    for box in results[0].boxes:
        cls_id = int(box.cls[0])
        label = model.names[cls_id]
        conf = float(box.conf[0])
        x1, y1, x2, y2 = map(int, box.xyxy[0])
        print(f"{label}: {conf:.2f} at ({x1},{y1})-({x2},{y2})")

    return annotated

# 网络摄像头实时检测
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
    annotated_frame = detect_objects(frame)
    cv2.imshow('Robot Vision', annotated_frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
cap.release()

import cv2
import numpy as np

def estimate_pose(object_points, image_points, camera_matrix, dist_coeffs):
    """
    利用 PnP 算法进行 6D 位姿估计
    object_points: 3D 点 (物体坐标系)
    image_points: 2D 点 (图像坐标)
    """
    success, rvec, tvec = cv2.solvePnP(
        object_points, image_points,
        camera_matrix, dist_coeffs,
        flags=cv2.SOLVEPNP_ITERATIVE
    )
    if success:
        R, _ = cv2.Rodrigues(rvec)
        print(f"位置: {tvec.flatten()}")
        print(f"旋转矩阵:\n{R}")
        return R, tvec
    return None, None

import gymnasium as gym
from stable_baselines3 import SAC
from stable_baselines3.common.callbacks import EvalCallback
import os

# 在 MuJoCo Ant 环境中训练 SAC
env = gym.make('Ant-v4', render_mode=None)
eval_env = gym.make('Ant-v4', render_mode=None)

# 保存最优模型的回调
eval_callback = EvalCallback(
    eval_env,
    best_model_save_path='./logs/best_model',
    log_path='./logs/',
    eval_freq=10000,
    deterministic=True,
    render=False
)

# 创建 SAC 模型
model = SAC(
    'MlpPolicy',
    env,
    verbose=1,
    learning_rate=3e-4,
    buffer_size=1_000_000,
    batch_size=256,
    ent_coef='auto',
    gamma=0.99,
    tau=0.005,
    tensorboard_log='./tb_logs/'
)

# 训练
model.learn(total_timesteps=1_000_000, callback=eval_callback)
model.save('ant_sac_final')

# 推理 (部署)
obs, _ = env.reset()
total_reward = 0
for _ in range(1000):
    action, _ = model.predict(obs, deterministic=True)
    obs, reward, done, truncated, info = env.step(action)
    total_reward += reward
    if done or truncated:
        print(f"回合结束,总奖励: {total_reward:.2f}")
        obs, _ = env.reset()
        total_reward = 0

class AutonomousDrivingSystem:
    """自动驾驶系统的基本流水线"""

    def __init__(self, perception, prediction, planning, control):
        self.perception = perception    # 感知模块
        self.prediction = prediction    # 预测模块
        self.planning = planning        # 规划模块
        self.control = control          # 控制模块

    def run_step(self, sensor_data):
        # 1. 感知: 从传感器数据中检测并跟踪物体
        detected_objects = self.perception.detect(sensor_data)
        ego_state = self.perception.localize(sensor_data)

        # 2. 预测: 预测周围车辆/行人的未来轨迹
        predicted_trajectories = self.prediction.predict(
            detected_objects, horizon=3.0  # 预测 3 秒
        )

        # 3. 规划: 规划通往目标的安全路径
        trajectory = self.planning.plan(
            ego_state, predicted_trajectories, goal=self.goal
        )

        # 4. 控制: 跟踪已规划的路径
        steering, throttle, brake = self.control.compute(
            ego_state, trajectory
        )

        return steering, throttle, brake

# OpenVLA 推理示例 (概念代码)
from transformers import AutoModelForVision2Seq, AutoProcessor
import torch

model_name = "openvla/openvla-7b"
processor = AutoProcessor.from_pretrained(model_name, trust_remote_code=True)
model = AutoModelForVision2Seq.from_pretrained(
    model_name,
    torch_dtype=torch.bfloat16,
    device_map='auto',
    trust_remote_code=True
)

# 通过视觉-语言指令生成动作
image = get_camera_image()  # 机器人相机图像
instruction = "pick up the red block and place it in the box"

inputs = processor(instruction, image).to('cuda:0', dtype=torch.bfloat16)
action = model.predict_action(**inputs, unnorm_key='bridge_orig', do_sample=False)
# action: [dx, dy, dz, droll, dpitch, dyaw, gripper]
robot.execute_action(action)