3. 双臂笛卡尔空间直线运动

任务目标

实现双臂机器人末端执行器在笛卡尔空间中的直线运动控制：给定起始点和终止点，规划末端位置线性插值路径，保持姿态恒定，并验证位置误差小于1毫米。

核心实现

import numpy as np

class CartesianLinePlanner:
    """笛卡尔空间直线路径规划器（双臂通用）"""
    
    def __init__(self, step_size=0.005):
        self.step_size = step_size  # 插值步长（米）
    
    def plan(self, start_pos, end_pos, start_ori, end_ori):
        """
        生成直线路径点序列
        参数:
            start_pos: 起始位置 [x, y, z] (米)
            end_pos:   终止位置 [x, y, z] (米)
            start_ori: 起始姿态 (3x3旋转矩阵或四元数)
            end_ori:   终止姿态
        返回:
            path: 路径点列表，每个点包含位置和姿态
        """
        # 位置线性插值
        delta = np.array(end_pos) - np.array(start_pos)
        distance = np.linalg.norm(delta)
        num_steps = max(int(distance / self.step_size), 1)
        
        path = []
        for i in range(num_steps + 1):
            t = i / num_steps
            pos = np.array(start_pos) + t * delta
            # 姿态恒定（保持起始姿态）
            ori = start_ori if np.allclose(start_ori, end_ori) else self._slerp(start_ori, end_ori, t)
            path.append({'position': pos, 'orientation': ori})
        
        return path
    
    def _slerp(self, q1, q2, t):
        """四元数球面线性插值（姿态变化时使用）"""
        q1 = q1 / np.linalg.norm(q1)
        q2 = q2 / np.linalg.norm(q2)
        dot = np.dot(q1, q2)
        
        if dot < 0:
            q2 = -q2
            dot = -dot
        
        if dot > 0.9995:
            result = q1 + t * (q2 - q1)
            return result / np.linalg.norm(result)
        
        theta_0 = np.arccos(dot)
        theta = theta_0 * t
        sin_theta = np.sin(theta)
        sin_theta_0 = np.sin(theta_0)
        
        s1 = np.cos(theta) - dot * sin_theta / sin_theta_0
        s2 = sin_theta / sin_theta_0
        return (s1 * q1 + s2 * q2) / np.linalg.norm(s1 * q1 + s2 * q2)

def verify_path_accuracy(path, start_pos, end_pos, start_ori):
    """验证路径精度：检查端点误差和直线偏差"""
    # 端点位置误差
    start_err = np.linalg.norm(path[0]['position'] - np.array(start_pos))
    end_err = np.linalg.norm(path[-1]['position'] - np.array(end_pos))
    
    # 直线偏差：中间点偏离理想直线的最大距离
    line_vec = np.array(end_pos) - np.array(start_pos)
    line_len = np.linalg.norm(line_vec)
    line_unit = line_vec / line_len
    
    max_deviation = 0.0
    for point in path:
        vec_from_start = point['position'] - np.array(start_pos)
        proj_len = np.dot(vec_from_start, line_unit)
        proj_point = np.array(start_pos) + proj_len * line_unit
        deviation = np.linalg.norm(point['position'] - proj_point)
        max_deviation = max(max_deviation, deviation)
    
    # 姿态误差（假设使用四元数，比较与起始姿态的夹角）
    ori_err = 0.0
    for point in path:
        q = point['orientation']
        # 计算与起始姿态的四元数夹角
        dot = np.abs(np.dot(q, start_ori))
        angle = 2 * np.arccos(min(dot, 1.0))
        ori_err = max(ori_err, angle)
    
    return {
        'start_position_error_m': start_err,
        'end_position_error_m': end_err,
        'max_line_deviation_m': max_deviation,
        'max_orientation_error_rad': ori_err
    }

# ===== 使用示例 =====
if __name__ == "__main__":
    # 定义双臂任务：左臂和右臂各规划一条直线路径
    # 起始姿态（四元数，w,x,y,z）
    q_start = np.array([1.0, 0.0, 0.0, 0.0])  # 单位四元数，零旋转
    
    # 左臂路径
    left_start = np.array([0.3, -0.2, 0.5])
    left_end = np.array([0.5, -0.2, 0.5])
    
    # 右臂路径
    right_start = np.array([0.3, 0.2, 0.5])
    right_end = np.array([0.5, 0.2, 0.5])
    
    planner = CartesianLinePlanner(step_size=0.005)
    
    # 规划双臂路径
    left_path = planner.plan(left_start, left_end, q_start, q_start)
    right_path = planner.plan(right_start, right_end, q_start, q_start)
    
    # 验证精度
    left_result = verify_path_accuracy(left_path, left_start, left_end, q_start)
    right_result = verify_path_accuracy(right_path, right_start, right_end, q_start)
    
    print("左臂路径点数:", len(left_path))
    print("右臂路径点数:", len(right_path))
    print("\n左臂精度验证:")
    print(f"  起始点误差: {left_result['start_position_error_m']*1000:.4f} mm")
    print(f"  终止点误差: {left_result['end_position_error_m']*1000:.4f} mm")
    print(f"  最大直线偏差: {left_result['max_line_deviation_m']*1000:.4f} mm")
    print(f"  最大姿态误差: {left_result['max_orientation_error_rad']:.6f} rad")
    
    print("\n右臂精度验证:")
    print(f"  起始点误差: {right_result['start_position_error_m']*1000:.4f} mm")
    print(f"  终止点误差: {right_result['end_position_error_m']*1000:.4f} mm")
    print(f"  最大直线偏差: {right_result['max_line_deviation_m']*1000:.4f} mm")
    print(f"  最大姿态误差: {right_result['max_orientation_error_rad']:.6f} rad")
    
    # 判定是否满足精度要求
    threshold_mm = 1.0
    left_pass = (left_result['end_position_error_m']*1000 < threshold_mm and 
                 left_result['max_line_deviation_m']*1000 < threshold_mm)
    right_pass = (right_result['end_position_error_m']*1000 < threshold_mm and 
                  right_result['max_line_deviation_m']*1000 < threshold_mm)
    
    print(f"\n左臂精度达标: {'是' if left_pass else '否'}")
    print(f"右臂精度达标: {'是' if right_pass else '否'}")

运行与验证

环境依赖：仅需NumPy库（pip install numpy）。

运行方式：

python cartesian_line_planner.py

验证方法：程序输出双臂路径的端点位置误差、最大直线偏差和姿态误差。判定通过条件为：

• 终止点位置误差 < 1 mm（0.001 m）
• 路径上所有中间点偏离理想直线的最大距离 < 1 mm
• 姿态误差保持为0（姿态恒定）

预期输出：由于插值算法直接计算端点，起始点和终止点误差应为0（浮点精度范围内）；最大直线偏差也为0（所有点严格位于直线上）；姿态误差为0（未使用姿态插值）。程序将打印双臂均"精度达标: 是"。

关键说明

本实现采用等距参数化插值（t = i/N），保证路径点严格位于起始点和终止点的连线上。姿态恒定通过直接复用起始四元数实现；若需要姿态同步变化，代码中已包含SLERP插值函数。实际机器人控制中，需将规划出的笛卡尔路径点通过逆运动学转换为关节角指令下发执行，本案例聚焦于笛卡尔空间路径生成与精度验证部分。