4. 空间圆弧插补

任务目标

给定三维空间中不共线的三个点，确定其所在平面上的圆弧（圆心、半径、起始角、终止角及旋转方向），并按指定步长输出插补点序列，用于机器人末端沿空间圆弧轨迹运动。

实现思路

三点确定空间圆弧的核心步骤：

1. 由三点构成两个向量，叉乘得到平面法向量，进而建立以圆心为原点的局部坐标系（u轴指向起点，v轴在平面内垂直于u）；
2. 圆心通过三点共面条件与到三点等距条件联立求解；
3. 在局部坐标系中将圆弧参数化为角度θ，按弧长或角度步长插值，再变换回世界坐标系。

关键完整代码

import numpy as np

def spatial_arc_interp(p1, p2, p3, step_angle=np.deg2rad(1.0)):
    """
    空间圆弧插补
    参数:
        p1, p2, p3: 不共线三点, shape=(3,)
        step_angle: 插补角度步长(弧度)
    返回:
        arc_points: 插补点序列, shape=(N, 3)
        center: 圆心坐标
        radius: 圆弧半径
        total_angle: 圆弧总角度(弧度)
    """
    p1 = np.asarray(p1, dtype=float)
    p2 = np.asarray(p2, dtype=float)
    p3 = np.asarray(p3, dtype=float)

    # 1. 计算平面法向量
    v1 = p2 - p1
    v2 = p3 - p1
    n = np.cross(v1, v2)
    norm_n = np.linalg.norm(n)
    if norm_n < 1e-12:
        raise ValueError("三点共线，无法确定圆弧")
    n = n / norm_n

    # 2. 求解圆心: 圆心在p1,p2,p3所在平面上, 且到三点距离相等
    #    圆心满足: (c - p1)·(p2-p1) = |p2-p1|²/2
    #              (c - p1)·(p3-p1) = |p3-p1|²/2
    #              (c - p1)·n = 0
    A = np.array([
        [v1[0], v1[1], v1[2]],
        [v2[0], v2[1], v2[2]],
        [n[0],  n[1],  n[2]]
    ])
    b = np.array([
        np.dot(v1, v1) / 2.0,
        np.dot(v2, v2) / 2.0,
        0.0
    ])
    center = np.linalg.solve(A, b) + p1

    # 3. 建立局部坐标系
    u = (p1 - center) / np.linalg.norm(p1 - center)   # 指向起点
    w = n                                            # 平面法向
    v = np.cross(w, u)                               # 平面内垂直于u

    # 4. 计算起始角和终止角
    start_angle = 0.0
    # p2和p3在局部坐标系中的角度
    def angle_of_point(p):
        d = p - center
        x = np.dot(d, u)
        y = np.dot(d, v)
        return np.arctan2(y, x)

    a2 = angle_of_point(p2)
    a3 = angle_of_point(p3)

    # 判断旋转方向: 从p1出发经过p2到达p3
    # 若a2在0~a3之间(绕w正方向), 则正向旋转; 否则反向
    if a2 > 0 and a2 < a3:
        end_angle = a3
        direction = 1.0
    else:
        end_angle = a3 - 2 * np.pi if a3 > 0 else a3
        direction = -1.0

    total_angle = abs(end_angle - start_angle)
    radius = np.linalg.norm(p1 - center)

    # 5. 插补
    num_steps = max(int(np.ceil(total_angle / step_angle)), 2)
    angles = np.linspace(start_angle, end_angle, num_steps)
    arc_points = []
    for theta in angles:
        point = center + radius * (np.cos(theta) * u + np.sin(theta) * v)
        arc_points.append(point)

    return np.array(arc_points), center, radius, total_angle

# ===== 使用示例 =====
if __name__ == "__main__":
    # 定义三个不共线的空间点
    p1 = np.array([0.0, 0.0, 0.0])
    p2 = np.array([1.0, 0.0, 0.0])
    p3 = np.array([0.0, 1.0, 0.0])

    pts, c, r, ang = spatial_arc_interp(p1, p2, p3, step_angle=np.deg2rad(5.0))

    print(f"圆心: {c}")
    print(f"半径: {r:.4f}")
    print(f"总角度: {np.rad2deg(ang):.2f}°")
    print(f"插补点数: {len(pts)}")
    print("前3个插补点:")
    for pt in pts[:3]:
        print(f"  {pt}")

运行与验证

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

验证方法：

• 示例中三点位于z=0平面，预期圆心为(0.5, 0.5, 0)，半径为√2/2 ≈ 0.7071，总角度为90°；
• 检查输出插补点是否满足：①所有点均在三点确定的平面上；②每个点到圆心的距离等于半径（误差<1e-6）；③首点为p1，末点为p3；
• 将插补点用matplotlib绘制，观察是否形成经过三点的平滑圆弧。

# 验证代码(可附加运行)
# 检查点到圆心距离是否等于半径
dists = np.linalg.norm(pts - c, axis=1)
assert np.allclose(dists, r, atol=1e-6), "半径误差过大"
# 检查首末点
assert np.allclose(pts[0], p1), "起点不匹配"
assert np.allclose(pts[-1], p3), "终点不匹配"
print("验证通过: 所有插补点均在圆弧上, 且首末点正确")

常见问题

• 三点近似共线：法向量模长接近0时程序报错，实际应用中应检查三点最小夹角，建议大于1°；
• 旋转方向判断：本实现按“从p1经p2到p3”的路径确定方向，若三点顺序改变，圆弧路径随之改变；
• 整圆插补：当终点与起点重合时（三点无法确定唯一圆弧），需额外指定圆心或旋转圈数，本案例不覆盖该场景。