利用python通过双矢量计算两坐标系相对姿态_python判断两坐标相对位置-CSDN博客

本文链接：https://blog.csdn.net/huangzhe0701/article/details/122568274

calOrientationFrom2Vectors

功能

根据两个不共线的矢量分别在两个坐标系下的矢量坐标求解这两个坐标系之间的旋转矩阵。

原理

若已知两个不共线的矢量分别在两个坐标系下的方向矢量，即可以方向矢量构建参考坐标系从而求得两个坐标系之间的旋转矩阵。

如图所示，两个坐标系分别为坐标系C、坐标系S，两个矢量分别为V1、V2，参考坐标系（坐标系n）根据矢量V1、V2建立。

在这里插入图片描述

参考坐标系在坐标系C下的正交坐标基为
$\left\{ \begin{aligned} a & = \ V_1^c \\ b & = \ \frac{(V_1^c \times V_2^c)}{|V_1^c \times V_2^c|} \\ c & = \ a \times b \end{aligned} \right.$
坐标系C到参考坐标系的姿态转换矩阵为
$R_c^n = \begin{bmatrix} a^T\\ b^T\\c^T\end{bmatrix}$
同理，参考坐标系在坐标系S下的正交坐标基为
$\left\{ \begin{aligned} A & = \ V_1^s \\ B & = \ \frac{(V_1^s \times V_2^s)}{|V_1^s \times V_2^s|} \\ C & = \ A \times B \end{aligned} \right.$
坐标系S到参考坐标系的姿态转换矩阵为
$R_s^n = \begin{bmatrix} A^T\\ B^T\\C^T\end{bmatrix}$
坐标系S到坐标系C的旋转矩阵
$R_s^c=(R_c^n)^{-1} \cdot R_s^n$

使用

输入

两个矢量在坐标系s下的坐标： $V s 1 (x s 1, y s 1, z s 1)$ ， $V s 2 (x s 2, y s 2, z s 2)$ ；

在坐标系n下的坐标： $V n 1 (x n 1, y n 1, z n 1)$ ， $V n 2 (x n 2, y n 2, z n 2)$ 。

输出

坐标系n到坐标系s的旋转矩阵 $R_n^s$ 。

DEMO

import algorithm as al
import numpy as np

meas = al.Test()
Vs1 = np.array([0.55397988, 0.82791962, -0.08749517])
Vs2 = np.array([0.02063334, -0.26258813, -0.96468738])
Vn1 = np.array([0.97066373, 0.20744552, 0.12156592])
Vn2 = np.array([0, 0, -1])

print(meas.calOrientationFrom2Vectors(Vs1, Vs2, Vn1, Vn2))