双臂协同：三维坐标系之间转换

做镜像和相同动作 从关节空间出发 去考虑 可以 规避求解 转换矩阵时得巨大误差 如果使用 转换矩阵 我用的时求解方程组 是有误差的 利用关节空间坐标系 可以很好规避一些 所以 用的时 关节空间坐标系进行的运动 如果 有可以更好的求解转换矩阵 欢迎补充！

做镜像动作 是将两个机械臂 摆放镜像位置 去观察 各个轴角 发现其规律 要对机械臂关节转动进行理解 才能得到 他是如何转化的 ，这也是从双臂做同样动作 中 找到的方法

叫双臂 做同样的动作 两个机械臂 是同一水平的机械臂 只要 转动z轴 就能确保 两个机械臂的基座标是相同姿态 只差 平移关系 所以 我们 找到 两个机械臂的基座标之间的转换矩阵 就可以 知道 从臂绕z轴旋转多少度 就可以和主臂是 相同姿态 的 下面是 求解 转换矩阵的程序 我是 找到了四个 在空间上 非共面的点对 这四个点对是在空间上不同坐标系下的表示，用这四个点对 进行 方程组求解 求解出转换矩阵。

import numpy as np
from scipy.optimize import least_squares


def estimate_transformation(points1, points2):
    assert points1.shape == points2.shape, "点对的数量必须相同"

    # 计算质心
    centroid1 = np.mean(points1, axis=0)
    centroid2 = np.mean(points2, axis=0)

    # 计算中心化后的点坐标
    centered_points1 = points1 - centroid1
    centered_points2 = points2 - centroid2

    # 构造矩阵 H
    H = np.dot(centered_points1.T, centered_points2)

    # 对矩阵 H 进行奇异值分解
    U, S, Vt = np.linalg.svd(H)

    # 计算旋转矩阵 R
    R = np.dot(U, Vt)

    # 确保旋转矩阵 R 是一个适当的旋转矩阵（行列式为 1）
    if np.linalg.det(R) < 0:
        Vt[2, :] *= -1
        R = np.dot(U, Vt)

    # 计算平移向量
    t = centroid2 - np.dot(R, centroid1)

    return R, t


def apply_transformation(R, t, points):
    # 应用旋转和平移
    return np.dot(points, R.T) + t


def estimate_transformation_ransac(points1, points2, iterations=100, inlier_threshold=5.0):
    best_inliers_count = 0
    best_R = None
    best_t = None
    inliers = None

    for i in range(iterations):
        # 随机选择两个点对
        indices = np.random.choice(len(points1), size=2, replace=False)
        point_pair1 = points1[indices]
        point_pair2 = points2[indices]

        # 使用这两对点估计变换
        R, t = estimate_transformation(point_pair1, point_pair2)

        # 应用变换并计算误差
        transformed_points1 = apply_transformation(R, t, points1)
        errors = np.linalg.norm(transformed_points1 - points2, axis=1)

        # 找到内点
        current_inliers = errors < inlier_threshold
        current_inlier_count = np.sum(current_inliers)

        # 如果当前变换有最多的内点，则保存这个变换
        if current_inlier_count > best_inliers_count:
            best_inliers_count = current_inlier_count
            best_R = R
            best_t = t
            inliers = current_inliers

    return best_R, best_t, inliers


def compute_error(params):
    R = params[:9].reshape((3, 3))
    t = params[9:]
    transformed_points1 = apply_transformation(R, t, points1)
    error = transformed_points1 - points2
    return np.ravel(error)  # 返回一维数组


def optimize_transformation(points1, points2):
    R, t, inliers = estimate_transformation_ransac(points1, points2)

    if R is not None and t is not None:
        print("找到有效的变换矩阵")
        print("旋转矩阵 R:\n", R)
        print("平移向量 t:", t)
        print("内点索引:", inliers)

        # 将旋转矩阵展平为一维数组，并将平移向量合并为优化参数的初始值
        initial_params = np.concatenate((R.flatten(), t))

        # 使用Levenberg-Marquardt算法进行优化
        result = least_squares(compute_error, initial_params)

        # 从优化结果中提取优化后的旋转矩阵和平移向量
        optimized_R = result.x[:9].reshape((3, 3))
        optimized_t = result.x[9:]

        # 检查旋转矩阵的行列式
        if np.linalg.det(optimized_R) != 1:
            print("警告：优化后的旋转矩阵行列式不为1，可能不是有效的旋转。")
        else:
            print("优化后的旋转矩阵 R 是有效的，行列式为1。")

        print("优化后的旋转矩阵 R:\n", optimized_R)
        print("优化后的平移向量 t:", optimized_t)
        # 应用优化后的变换到所有点
        optimized_transformed_points1 = apply_transformation(optimized_R, optimized_t, points1)
        print("优化后的转换后的点集:")
        print(optimized_transformed_points1)

        return optimized_R, optimized_t
    else:
        print("未找到有效的变换矩阵")
        return None, None


# 定义两个坐标系中的点对
points1 = np.array([
    [-477.6506042480469, -469.8567810058594, 190.0706787109375],
    [-474.9923095703125, -501.77972412109375, 216.66241455078125],
    [-495.1923828125, -366.2968444824219, 189.349853515625],
    [-643.6806030273438, -538.6202392578125, 217.26553344726562]
])

points2 = np.array([
    [-440.8072814941406, -443.6011962890625, 187.8321990966797],
    [-460.0429992675781, -431.256591796875, 214.06576538085938],
    [-347.4195556640625, -509.77410888671875, 187.9711151123047],
    [-366.8291320800781, -283.7540283203125, 213.47018432617188]
])

# 执行优化
optimized_R, optimized_t = optimize_transformation(points1, points2)

优化后的旋转矩阵 R:

[[-0.72221591 0.78143732 0.2869284 ]

[-0.70939602 -0.76178531 -0.37936752]

[ 0.00509816 0.00914455 0.99699903]]

优化后的平移向量 t: [ -473.14719568 -1068.2679845 5.06368521]

优化后点集与目标点集之间的误差: [1.13686838e-13 0.00000000e+00 1.27105749e-13 1.27105749e-13]

如果我们知道两个机械臂的旋转矩阵，并且这两个旋转矩阵是在同一水平面上，那么最终的旋转矩阵 R 将是这两个旋转矩阵的乘积。在这种情况下，如果你想要找出坐标系绕 z 轴旋转了多少度，我们可以通过观察旋转矩阵的前两行来估计绕 z 轴的旋转

import numpy as np

# 提取 cos(theta) 和 sin(theta) 的近似值
cos_theta = -0.72221591
sin_theta = 0.78143732

# 使用 atan2 来计算角度
theta = np.arctan2(sin_theta, cos_theta)

# 将角度从弧度转换为度
theta_degrees = np.degrees(theta)

print(f"绕 z 轴旋转的角度（度）: {theta_degrees}")

得到 绕 z 轴旋转的角度（度）: 132.74457038480327