4. Stewart平台奇异位形分析

任务目标

针对6-6构型Stewart平台（六根支腿连接固定基座与动平台），建立位置反解模型，通过雅可比矩阵行列式检测奇异位形，并设计一条避开奇异点的动平台轨迹。

核心原理与实现方案

Stewart平台奇异发生在雅可比矩阵J降秩时，此时平台失去瞬时刚度，控制力可能趋于无穷。本案例采用数值搜索法：在工作空间网格上计算det(J)，将接近零的区域标记为奇异区域；随后规划一条从起点到终点、绕过奇异区域的直线插值轨迹，并在每个插值点验证行列式安全裕度。

关键完整代码

import numpy as np
from itertools import combinations

# ---------- 平台几何参数 ----------
# 基座铰点（固定坐标系，半径2.0，角度间隔60°）
base_pts = []
for i in range(6):
    ang = np.deg2rad(i * 60)
    base_pts.append([2.0 * np.cos(ang), 2.0 * np.sin(ang), 0.0])
base_pts = np.array(base_pts)

# 动平台铰点（动坐标系，半径1.0，角度偏移30°）
top_pts = []
for i in range(6):
    ang = np.deg2rad(i * 60 + 30)
    top_pts.append([1.0 * np.cos(ang), 1.0 * np.sin(ang), 0.0])
top_pts = np.array(top_pts)

# ---------- 位置反解：给定位姿求六根腿长 ----------
def inverse_kinematics(pos, rpy):
    """pos: [x,y,z], rpy: [roll,pitch,yaw] (rad)"""
    cr, sr = np.cos(rpy[0]), np.sin(rpy[0])
    cp, sp = np.cos(rpy[1]), np.sin(rpy[1])
    cy, sy = np.cos(rpy[2]), np.sin(rpy[2])
    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr]
    ])
    leg_vecs = []
    for i in range(6):
        top_world = pos + R @ top_pts[i]
        leg_vecs.append(top_world - base_pts[i])
    leg_vecs = np.array(leg_vecs)
    lengths = np.linalg.norm(leg_vecs, axis=1)
    return leg_vecs, lengths

# ---------- 雅可比矩阵（基于力雅可比，行列式与速度雅可比相同） ----------
def jacobian_matrix(pos, rpy):
    leg_vecs, _ = inverse_kinematics(pos, rpy)
    J = np.zeros((6, 6))
    cr, sr = np.cos(rpy[0]), np.sin(rpy[0])
    cp, sp = np.cos(rpy[1]), np.sin(rpy[1])
    cy, sy = np.cos(rpy[2]), np.sin(rpy[2])
    R = np.array([
        [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr],
        [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr],
        [-sp,   cp*sr,            cp*cr]
    ])
    for i in range(6):
        s = leg_vecs[i] / np.linalg.norm(leg_vecs[i])  # 单位方向向量
        r_i = R @ top_pts[i]                           # 动平台铰点相对位置
        J[i, :3] = s
        J[i, 3:] = np.cross(r_i, s)
    return J

# ---------- 奇异检测：网格搜索 ----------
def find_singular_regions(z_level=1.2, grid_n=20, det_thresh=0.5):
    """在x-y平面网格上计算行列式，返回奇异点列表"""
    singular_pts = []
    x_vals = np.linspace(-1.5, 1.5, grid_n)
    y_vals = np.linspace(-1.5, 1.5, grid_n)
    for x in x_vals:
        for y in y_vals:
            pos = np.array([x, y, z_level])
            rpy = np.array([0.0, 0.0, 0.0])  # 固定姿态
            J = jacobian_matrix(pos, rpy)
            det_val = np.linalg.det(J)
            if abs(det_val) < det_thresh:
                singular_pts.append((x, y, det_val))
    return np.array(singular_pts) if singular_pts else np.empty((0, 3))

# ---------- 轨迹规划：带奇异回避的直线插值 ----------
def plan_trajectory(start_xy, end_xy, z=1.2, det_min=1.0, step=0.05):
    """
    在x-y平面从start到end直线插值，若某点行列式低于det_min，
    则在中点插入绕行点（偏移到奇异区域外）。
    返回路径点列表（每个点为[x, y, z]）。
    """
    start = np.array([start_xy[0], start_xy[1], z])
    end = np.array([end_xy[0], end_xy[1], z])
    dist = np.linalg.norm(end - start)
    n_steps = max(int(dist / step), 2)
    path = []
    for i in range(n_steps + 1):
        t = i / n_steps
        pt = start + t * (end - start)
        J = jacobian_matrix(pt, np.zeros(3))
        det_val = np.linalg.det(J)
        if abs(det_val) < det_min:
            # 绕行：向y正方向偏移0.5再继续
            detour_pt = pt + np.array([0.0, 0.5, 0.0])
            path.append(detour_pt.tolist())
            # 重新规划剩余路径
            remaining = plan_trajectory(detour_pt[:2], end_xy, z, det_min, step)
            path.extend(remaining)
            return path
        path.append(pt.tolist())
    return path

# ---------- 主程序：执行分析并输出结果 ----------
if __name__ == "__main__":
    # 1. 奇异区域检测
    singular = find_singular_regions(z_level=1.2, grid_n=15, det_thresh=0.5)
    print(f"检测到奇异点数量: {len(singular)}")
    if len(singular) > 0:
        print("奇异点示例 (x, y, det):")
        for pt in singular[:5]:
            print(f"  ({pt[0]:.3f}, {pt[1]:.3f}, det={pt[2]:.4f})")

    # 2. 规划一条从(0,0)到(1.2,0.8)的轨迹（该直线可能穿过奇异区）
    start = (0.0, 0.0)
    end = (1.2, 0.8)
    path = plan_trajectory(start, end, z=1.2, det_min=1.0, step=0.05)

    # 3. 验证路径上所有点的行列式
    min_det = float('inf')
    for pt in path:
        J = jacobian_matrix(np.array(pt), np.zeros(3))
        det_val = np.linalg.det(J)
        min_det = min(min_det, abs(det_val))

    print(f"路径点数: {len(path)}")
    print(f"路径上最小|det(J)|: {min_det:.4f}")
    print(f"路径是否安全: {'是' if min_det > 1.0 else '否'}")

运行与验证

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

运行方式：将代码保存为singularity_analysis.py，执行python singularity_analysis.py。

预期输出与判定标准：

程序首先输出检测到的奇异点数量（网格搜索下通常为几十个），并列出前几个奇异点的坐标与行列式值（值小于0.5）。
随后规划轨迹并输出路径点数量、路径上最小行列式值。若该值大于设定的安全阈值1.0，则输出“路径是否安全: 是”，说明绕行策略生效。
可通过修改start和end坐标测试不同路径：当直线路径穿过奇异区域时，程序自动插入绕行点；当直线路径本身远离奇异区时，路径保持直线且最小行列式较大。

奇异区域可视化

图中展示了在z=1.2平面、零姿态下检测到的奇异区域分布（红色圆点）。蓝色虚线为原始直线路径，会穿过奇异区域；绿色实线为算法自动生成的绕行路径，通过向y正方向偏移避开奇异点，保证路径上所有点的|det(J)|均大于安全阈值。

常见问题

奇异点数量为0：若网格分辨率过低或阈值过大，可能漏检。可增大grid_n或减小det_thresh。
绕行后仍不安全：当奇异区域范围较大时，固定偏移量0.5可能不足。可改为动态偏移（如沿梯度方向搜索安全点）。
姿态影响：本案例固定姿态为零，实际应用中姿态变化会改变奇异分布，需在完整六维空间搜索。

📚 课程目录