Python机器人编程常用库推荐：OpenCV、NumPy与PyRobotics

import numpy as np

def create_transform_matrix_2d(x, y, theta_rad):
    """构造二维平面移动机器人的 3x3 齐次变换矩阵"""
    cos_t = np.cos(theta_rad)
    sin_t = np.sin(theta_rad)
    T = np.array([
        [cos_t, -sin_t, x],
        [sin_t,  cos_t, y],
        [    0,      0, 1]
    ], dtype=np.float64)
    return T

# 1. 机器人在全局世界地图中的当前位姿：x=2.0m, y=1.5m, 朝向角 theta=30度 (π/6)
T_world_robot = create_transform_matrix_2d(2.0, 1.5, np.pi / 6)

# 2. 机载传感器（如激光或前置超声波）相对机器人底盘中心的安装偏置：前方 0.3m, 左偏 0.1m, 无安装偏角
T_robot_sensor = create_transform_matrix_2d(0.3, 0.1, 0.0)

# 3. 传感器在局部坐标系中探测到障碍物点：位于传感器正前方 1.2m, 右侧 0.4m
p_sensor_local = np.array([1.2, -0.4, 1.0])  # 齐次坐标 (x, y, 1)

# 4. 级联矩阵乘法：求障碍物在全局地图中的绝对坐标
# 公式：P_world = T_world_robot @ T_robot_sensor @ P_sensor
T_world_sensor = T_world_robot @ T_robot_sensor
p_world_homo = T_world_sensor @ p_sensor_local
p_world = p_world_homo[:2]

print("全局世界坐标系下的障碍物坐标 (X, Y):")
print(f"X: {{p_world[0]:.3f}} 米, Y: {{p_world[1]:.3f}} 米")

import cv2
import numpy as np

def estimate_marker_pose(image_frame, marker_length=0.1):
    """
    检测图像中的 ArUco 标记并解算其空间位姿
    :param image_frame: 相机捕获的 BGR 图像
    :param marker_length: 标定码实际物理边长（单位：米）
    :return: 标记检测结果列表，每项包含 ID 与 4x4 齐次变换矩阵
    """
    # 相机标定所得内参矩阵 K 与畸变参数 dist (实际使用时通过相机标定工具获取)
    camera_matrix = np.array([
        [650.0,   0.0, 320.0],
        [  0.0, 650.0, 240.0],
        [  0.0,   0.0,   1.0]
    ], dtype=np.float64)
    dist_coeffs = np.zeros((4, 1), dtype=np.float64)  # 假设无畸变或已去畸变

    # 1. 定义并创建 ArUco 检测器字典
    aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
    parameters = cv2.aruco.DetectorParameters()
    detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)

    # 2. 检测标记角点
    gray = cv2.cvtColor(image_frame, cv2.COLOR_BGR2GRAY)
    corners, ids, rejected = detector.detectMarkers(gray)

    results = []
    if ids is not None:
        # 定义标记自身物理坐标系下的4个角点坐标 (Z=0 平面)
        half_len = marker_length / 2.0
        obj_points = np.array([
            [-half_len,  half_len, 0.0],
            [ half_len,  half_len, 0.0],
            [ half_len, -half_len, 0.0],
            [-half_len, -half_len, 0.0]
        ], dtype=np.float64)

        for i in range(len(ids)):
            img_points = corners[i].reshape(-1, 2)
            # 3. 执行 PnP 求解
            success, rvec, tvec = cv2.solvePnP(
                obj_points, img_points, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_IPPE_SQUARE
            )
            if success:
                # 4. 旋转向量转为 3x3 旋转矩阵
                R, _ = cv2.Rodrigues(rvec)
                # 构造 4x4 齐次位姿矩阵
                T_cam_marker = np.eye(4)
                T_cam_marker[:3, :3] = R
                T_cam_marker[:3, 3] = tvec.flatten()
                results.append((int(ids[i][0]), T_cam_marker, tvec.flatten()))
    return results

# 构造模拟测试图像 (验证函数结构)
test_frame = np.zeros((480, 640, 3), dtype=np.uint8)
detected = estimate_marker_pose(test_frame, marker_length=0.1)
print(f"检测完成，视野内有效标记数: {{len(detected)}}")

import math
import random
import numpy as np

class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.path_x = [x]
        self.path_y = [y]
        self.parent = None
        self.cost = 0.0

class RRTStarPlanner:
    def __init__(self, start, goal, obstacle_list, rand_area, expand_dis=0.5, max_iter=200):
        self.start = Node(start[0], start[1])
        self.goal = Node(goal[0], goal[1])
        self.min_rand, self.max_rand = rand_area
        self.expand_dis = expand_dis
        self.max_iter = max_iter
        self.obstacle_list = obstacle_list  # [(x, y, radius), ...]
        self.node_list = [self.start]

    def plan(self):
        for i in range(self.max_iter):
            # 1. 目标偏置采样 (10% 概率直接采样目标点以加速收敛)
            if random.random() > 0.1:
                rnd = [random.uniform(self.min_rand, self.max_rand),
                       random.uniform(self.min_rand, self.max_rand)]
            else:
                rnd = [self.goal.x, self.goal.y]

            # 2. 查找树中最近节点
            d_list = [(node.x - rnd[0])**2 + (node.y - rnd[1])**2 for node in self.node_list]
            min_idx = d_list.index(min(d_list))
            nearest_node = self.node_list[min_idx]

            # 3. 沿方向步进扩展生成新节点
            theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)
            new_node = Node(nearest_node.x + self.expand_dis * math.cos(theta),
                            nearest_node.y + self.expand_dis * math.sin(theta))
            new_node.parent = nearest_node
            new_node.cost = nearest_node.cost + self.expand_dis

            # 4. 碰撞检测
            if not self.check_collision(nearest_node, new_node):
                continue

            self.node_list.append(new_node)

            # 5. 判断是否接近目标点
            d_to_goal = math.hypot(new_node.x - self.goal.x, new_node.y - self.goal.y)
            if d_to_goal <= self.expand_dis:
                final_node = Node(self.goal.x, self.goal.y)
                final_node.parent = new_node
                return self.generate_final_course(final_node)
        return None

    def check_collision(self, n1, n2):
        for (ox, oy, size) in self.obstacle_list:
            # 检测两点连线是否侵入障碍物圆形边界
            dx = n2.x - n1.x
            dy = n2.y - n1.y
            dist = math.hypot(dx, dy)
            if dist == 0:
                continue
            # 离散检测采样
            steps = int(dist / 0.1)
            for s in range(steps + 1):
                u = s / max(steps, 1)
                cx = n1.x + u * dx
                cy = n1.y + u * dy
                if math.hypot(cx - ox, cy - oy) <= size:
                    return False
        return True

    def generate_final_course(self, goal_node):
        path = []
        node = goal_node
        while node is not None:
            path.append([node.x, node.y])
            node = node.parent
        return path[::-1]

# 实例化规划器测试
planner = RRTStarPlanner(
    start=(0.0, 0.0),
    goal=(4.0, 4.0),
    obstacle_list=[(2.0, 2.0, 0.8), (1.5, 3.0, 0.6)],
    rand_area=(-1.0, 5.0)
)
random.seed(42)
path = planner.plan()
print(f"路径规划成功！生成路标点总数: {{len(path) if path else 0}}")

import numpy as np
import cv2

class RobotNavigationPipeline:
    def __init__(self, T_robot_camera):
        """
        :param T_robot_camera: 相机在机器人坐标系中的 4x4 外参齐次矩阵
        """
        self.T_robot_cam = T_robot_camera
        # 机器人底盘当前世界位姿 (x, y, yaw)
        self.current_pose = np.array([0.0, 0.0, 0.0])

    def get_world_target_from_vision(self, rvec, tvec):
        """步骤 1 & 2: 将 OpenCV 解算的相机系位姿转换至全局世界系"""
        # 构造相机系下的齐次目标点
        p_cam = np.array([tvec[0], tvec[1], tvec[2], 1.0])

        # 底盘在世界坐标系的齐次矩阵
        x, y, yaw = self.current_pose
        cos_y, sin_y = np.cos(yaw), np.sin(yaw)
        T_world_robot = np.array([
            [cos_y, -sin_y, 0, x],
            [sin_y,  cos_y, 0, y],
            [    0,      0, 1, 0],
            [    0,      0, 0, 1]
        ])

        # NumPy 级联乘法解算世界坐标
        p_world = T_world_robot @ self.T_robot_cam @ p_cam
        return p_world[:2]  # 返回二维地面平面坐标 (x, y)

    def compute_pure_pursuit_cmd(self, target_world, lookahead_dist=0.6, wheel_base=0.4):
        """步骤 3 & 4: 简易纯追踪控制器计算线速度与角速度"""
        # 转换目标点到底盘局部坐标系
        x, y, yaw = self.current_pose
        dx = target_world[0] - x
        dy = target_world[1] - y

        # 旋转到底盘局部坐标系 (旋转 -yaw)
        local_x =  math_cos = np.cos(-yaw) * dx - np.sin(-yaw) * dy
        local_y =  math_sin = np.sin(-yaw) * dx + np.cos(-yaw) * dy

        # 计算曲率与转向角
        dist = np.hypot(local_x, local_y)
        if dist < 0.05:
            return 0.0, 0.0  # 已到达目标点，停止

        alpha = np.arctan2(local_y, local_x)
        # 纯追踪前向转向曲率 kappa = 2 * sin(alpha) / lookahead_dist
        steer_angle = np.arctan2(2.0 * wheel_base * np.sin(alpha), lookahead_dist)
        linear_vel = 0.3  # 巡航线速度 0.3 m/s
        angular_vel = linear_vel / wheel_base * np.tan(steer_angle)

        return linear_vel, angular_vel

# 模拟初始化与运行
T_cam = np.eye(4)
T_cam[:3, 3] = [0.2, 0.0, 0.3]  # 相机安装在底盘前方0.2m，高度0.3m
pipeline = RobotNavigationPipeline(T_cam)

# 假设 OpenCV 在相机前方解算到地标 (X=0.1m, Y=0.0m, Z=1.5m)
fake_tvec = np.array([0.1, 0.0, 1.5])
fake_rvec = np.zeros(3)

target_world_xy = pipeline.get_world_target_from_vision(fake_rvec, fake_tvec)
v, w = pipeline.compute_pure_pursuit_cmd(target_world_xy)
print(f"全局目标投影: X={{target_world_xy[0]:.2f}}m, Y={{target_world_xy[1]:.2f}}m")
print(f"控制指令输出: 线速度 v={{v:.2f}} m/s, 角速度 w={{w:.2f}} rad/s")

核心结论总结

OpenCV、NumPy 与 PyRobotics 构成了 Python 机器人编程坚实而高效的“黄金三角”：

• OpenCV 解决“眼睛”的问题：输入真实世界的光影像素，输出严谨的空间特征与位姿参数；
• NumPy 解决“骨架”的问题：以高效的多维数组和刚体变换齐次矩阵，统一了所有传感器与物理结构的坐标基底；
• PyRobotics 解决“大脑”的问题：以清晰直接的数学逻辑实现路径避障与轨迹追踪。

在日常开发中，先用 OpenCV 提取特征、用 NumPy 进行对齐与坐标系转换、用 PyRobotics 验证运动规划逻辑，能够大幅缩减机器人原型验证周期，为后续工程落地奠定扎实的代码与算法基础。