机器人学基础之坐标变换

一、机器人坐标变换常用库

（一）python常用库函数

1. numpy库介绍

（1）numpy库安装

sudo pip install numpy -i https://pypi.tuna.tsinghua.edu.cn/simple

（2）numpy库常见用法

import numpy as np
np.asarray(A).reshape(size*3, 3) 

import numpy as np
np.dot(A,B) 

import numpy as np
np.linalg.pinv(A)

import numpy as np
np.sqrt(A) 

import numpy as np
np.add(A,B)

import numpy as np
np.divide(A,2)

import numpy as np
np.subtract(A,B)

2. transforms3d库介绍

（1）transforms3d库安装

sudo pip install transforms3d -i https://pypi.tuna.tsinghua.edu.cn/simple

（2）transforms3d库常见用法

import transforms3d as tfs
tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz))

import transforms3d as tfs
# 其中rmat为旋转矩阵
tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])

import transforms3d as tfs
#	其中m为旋转矩阵
tfs.quaternions.mat2quat(m[0:3,0:3])

import transforms3d as tfs
tfs.quaternions.quat2mat([w,q[0],q[1],q[2]])

（二）C++常用库函数

1. tf库

sudo apt-get install tf

2. Eigen库

# eigen3软链接
sudo apt-get install libeigen3-dev
sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
sudo ln -sf /usr/include/eigen3/unsupported//usr/include/unsupported

二、齐次变换矩阵的合成与分解

（一）齐次变换矩阵的合成（python）

1. 根据旋转矩阵R和平移向量T，利用numpy库合成齐次变换矩阵：

#导入库
import numpy as np

# 定义旋转矩阵R和平移向量T
R = np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]])
T = np.asarray([1,0,1])

temp = np.hstack((R,T.reshape(3,1)))
np.vstack((temp,[0,0,0,1]))

2. 根据旋转矩阵R和平移向量T，利用tfs库合成齐次变换矩阵：

#导入库
import numpy as np
import transforms3d as tfs

# 定义旋转矩阵R和平移向量T
R = np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]])
T = np.asarray([1,0,1])

tfs.affines.compose(T,R,[1,1,1])

3. 根据四元数Q和平移向量T，利用tfs库合成齐次变换矩阵：

import numpy as np
import transforms3d as tfs

# 定义旋转矩阵R和平移向量T
T = np.asarray([1,0,1])

R = tfs.quaternions.quat2mat([1,0,0,0])
tfs.affines.compose(T,R,[1,1,1])

4. 根据四元数Q和平移向量T，利用numpy库合成齐次变换矩阵：

import numpy as np
import transforms3d as tfs

# 定义旋转矩阵R和平移向量T
T = np.asarray([1,0,1])

R = tfs.quaternions.quat2mat([1,0,0,0])
temp = np.hstack((R,T.reshape(3,1)))
np.vstack((temp,[0,0,0,1]))

（二）齐次变换矩阵的分解（python）

1. 分解为固定轴欧拉角和平移向量

import transforms3d as tfs
tfs.euler.mat2euler(T[0:3,0:3]),T[:3,3:4]

2. 分解为四元数和平移向量

import transforms3d as tfs
tfs.quaternions.mat2quat(T[0:3,0:3]),T[:3,3:4]

三、齐次变换矩阵的乘法

（一）齐次变换矩阵的乘法（python）

对应numpy中矩阵的乘法np.dot讲两个矩阵相乘即可.

四、四元数、欧拉角、旋转矩阵互相转换

（一）四元数转换成旋转矩阵

理论推导：

$$四元数q= \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix} \tag{4.1.1}$$

$$R = \begin{bmatrix} 1-2q_2^2-2q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_1q_3+2q_0q_1 \\ 2q_1q_3+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2 \end{bmatrix} \tag{4.1.2}$$

1. 四元数转换成旋转矩阵（python）

（1） 利用transforms3d.quaternions.quat2mat()函数
用法：
示例：

#	四元数Quaternion的格式为(w,x,y,z)
import transforms3d as tfs
Rot_matrix = tfs.quaternions.quat2mat(Quaternion)

（2） 根据公式进行转换

def quaternion_to_rotation_matrix(q):  # x, y ,z ,w
    rot_matrix = np.array(
        [[1.0 - 2 * (q[1] * q[1] + q[2] * q[2]), 2 * (q[0] * q[1] - q[3] * q[2]), 2 * (q[3] * q[1] + q[0] * q[2])],
         [2 * (q[0] * q[1] + q[3] * q[2]), 1.0 - 2 * (q[0] * q[0] + q[2] * q[2]), 2 * (q[1] * q[2] - q[3] * q[0])],
         [2 * (q[0] * q[2] - q[3] * q[1]), 2 * (q[1] * q[2] + q[3] * q[0]), 1.0 - 2 * (q[0] * q[0] + q[1] * q[1])]],
        dtype=q.dtype)
    return rot_matrix

（3） 用tf函数进行转换

from tf.listener import xyzw_to_mat44

class oritation:
    def __init__(self):
        self.x = 0
        self.y = 0
        self.z = 0
        self.w = 0

ori = oritation()
ori.x = rot[0]
ori.y = rot[1]
ori.z = rot[2]
ori.w = rot[3]
mat44 = xyzw_to_mat44(ori) # 转换的结果为4 × 4 矩阵

（4）用scipy函数进行转换

from scipy.spatial.transform import Rotation as R

# (x, y, z, w) format
r = R.from_quat([-0.716556549511624,-0.6971278819736084, -0.010016582945017661,  0.02142651612120239])
r.as_matrix()
print('rotation:\n',r.as_matrix())
rotation_matrix = r.as_matrix()
print(rotation_matrix)

2. 四元数转换成旋转矩阵（C++）

（1）利用Eigen库

Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
{  
    Eigen::Quaterniond q;  
    q.x() = x;  
    q.y() = y;  
    q.z() = z;  
    q.w() = w;  
  
    Eigen::Matrix3d R = q.normalized().toRotationMatrix();  
    cout << “Quaternion2RotationMatrix result is:” <<endl;  
    cout << ”R = ” << endl << R << endl<< endl;  
    return R;  
} 

（2）利用tf库

tf::createQuaternionMsgFromYaw(double y);
//只通过y即绕z的旋转角度计算四元数，用于平面小车。返回四元数

（二）旋转矩阵转换成四元数

理论推导：

$$旋转矩阵R= \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \tag{4.2.1}$$

$$四元数 \begin{cases} w = \frac{\sqrt{tr(R)+1}}{2} \\ x = \frac{{r_{32}-r_{23}}}{4w} \\ y = \frac{{r_{12}-r_{31}}}{4w} \\ z = \frac{{r_{21}-r_{12}}}{4w} \end{cases} \tag{4.2.2}$$

1. 旋转矩阵转换成四元数（python）

(1) transforms3d库

import transforms3d as tfs
#	其中m为旋转矩阵
tfs.quaternions.mat2quat(m[0:3,0:3])

(2)根据表达式

function q = vgg_quat_from_rotation_matrix( R )
% vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix 
%            q = vgg_quat_from_rotation_matrix(R)

q = [   (1 + R(1,1) + R(2,2) + R(3,3))
    (1 + R(1,1) - R(2,2) - R(3,3))
    (1 - R(1,1) + R(2,2) - R(3,3))
    (1 - R(1,1) - R(2,2) + R(3,3)) ];

if ~issym(q)
  % Pivot to avoid division by small numbers
  [b I] = max(abs(q));
else
  % For symbolic quats, just make sure we're nonzero
  for k=1:4
    if q(k) ~= 0
      I = k;
      break
    end
  end
end

q(I) = sqrt(q(I)) / 2 ;

if I == 1 
    q(2) = (R(3,2) - R(2,3)) / (4*q(I));
    q(3) = (R(1,3) - R(3,1)) / (4*q(I));
    q(4) = (R(2,1) - R(1,2)) / (4*q(I));
elseif I==2
    q(1) = (R(3,2) - R(2,3)) / (4*q(I));
    q(3) = (R(2,1) + R(1,2)) / (4*q(I));
    q(4) = (R(1,3) + R(3,1)) / (4*q(I));
elseif I==3
    q(1) = (R(1,3) - R(3,1)) / (4*q(I));
    q(2) = (R(2,1) + R(1,2)) / (4*q(I));
    q(4) = (R(3,2) + R(2,3)) / (4*q(I));
elseif I==4
    q(1) = (R(2,1) - R(1,2)) / (4*q(I));
    q(2) = (R(1,3) + R(3,1)) / (4*q(I));
    q(3) = (R(3,2) + R(2,3)) / (4*q(I));
end

2. 旋转矩阵转换成四元数（C++）

(1) Eigen库

Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
{  
    Eigen::Quaterniond q = Eigen::Quaterniond(R);  
    q.normalize();  
    cout << “RotationMatrix2Quaterniond result is:” <<endl;  
    cout << ”x = ” << q.x() <<endl;  
    cout << ”y = ” << q.y() <<endl;  
    cout << ”z = ” << q.z() <<endl;  
    cout << ”w = ” << q.w() <<endl<<endl;  
    return q;  
} 

（三）旋转矩阵转换成欧拉角

理论推导：

$$旋转矩阵R= \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} \tag{4.3.1}$$

$$欧拉角 \begin{cases} \Theta_x = atan2(r_{32}, r_{33}) \\ \Theta_y = atan2(-r_{31}, \sqrt{r_{32}^2+r_{33}^2}) \\ \Theta_z = atan2(r_{21}, r_{11}) \end{cases} \tag{4.3.2}$$

1. 旋转矩阵转换成欧拉角（Python）

(1)根据表达式

def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))
    
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    
    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

(2)transforms3d库函数

import transforms3d as tfs
import numpy as np 
tfs.euler.mat2euler(np.asarray([[1., 0., 0.],[0., 1., 0.],[0., 0., 1.]]),"sxyz")

2. 旋转矩阵转换成欧拉角（C++）

(1) Eigen库

    static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R)
    {
        Eigen::Vector3d n = R.col(0);
        Eigen::Vector3d o = R.col(1);
        Eigen::Vector3d a = R.col(2);

        Eigen::Vector3d ypr(3);
        double y = atan2(n(1), n(0));
        double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
        double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
        ypr(0) = y;
        ypr(1) = p;
        ypr(2) = r;

        return ypr / M_PI * 180.0;
    }

（四）欧拉角转换成旋转矩阵

理论推导：

$$R= R_z(\phi)R_y(\theta)R_x(\psi) \\ = \begin{bmatrix} cos\phi cos\theta & sin\psi sin\theta cos\phi - cos\psi sin\phi & cos\psi sin\theta cos\phi + sin\psi sin\phi \\ sin\phi cos\theta & sin\psi sin\theta sin\phi + cos\psi cos\phi & cos\psi sin\theta sin\phi - sin\psi cos\phi \\ -sin\theta & sin\psi cos\theta & cos\psi cos\theta \end{bmatrix} \tag{4.4.1}$$

1. 欧拉角转换成旋转矩阵（Python）

（1）根据表达式

def eulerAnglesToRotationMatrix(theta) :
    
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
        
        
                    
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
                
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                    
                    
    R = np.dot(R_z, np.dot( R_y, R_x ))

    return R

（2）transforms3d库

import transforms3d as tfs
tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz),"sxyz")

2. 欧拉角转换成旋转矩阵（C++）

(1)Eigen库

static Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr2R(const Eigen::MatrixBase<Derived> &ypr)
    {
        typedef typename Derived::Scalar Scalar_t;

        Scalar_t y = ypr(0) / 180.0 * M_PI;
        Scalar_t p = ypr(1) / 180.0 * M_PI;
        Scalar_t r = ypr(2) / 180.0 * M_PI;

        Eigen::Matrix<Scalar_t, 3, 3> Rz;
        Rz << cos(y), -sin(y), 0,
            sin(y), cos(y), 0,
            0, 0, 1;

        Eigen::Matrix<Scalar_t, 3, 3> Ry;
        Ry << cos(p), 0., sin(p),
            0., 1., 0.,
            -sin(p), 0., cos(p);

        Eigen::Matrix<Scalar_t, 3, 3> Rx;
        Rx << 1., 0., 0.,
            0., cos(r), -sin(r),
            0., sin(r), cos(r);

        return Rz * Ry * Rx;
    }

（五）欧拉角转换成四元数

理论推导：

$$roll,pitch,yaw分别为 \alpha ， \beta ， \gamma ,则有：$$

$$\begin{cases} x = sin(\beta /2) sin(\gamma /2) cos(\alpha /2) + cos(\beta /2) cos(\gamma /2) sin(\alpha /2) \\ y = sin(\beta /2) cos(\gamma /2) cos(\alpha /2) + cos(\beta /2) sin(\gamma /2) sin(\alpha /2) \\ z = cos(\beta /2) sin(\gamma /2) cos(\alpha /2) - sin(\beta /2) cos(\gamma /2) sin(\alpha /2) \\ w = cos(\beta /2) cos(\gamma /2) cos(\alpha /2) - sin(\beta /2) sin(\gamma /2) sin(\alpha /2) \end{cases} \tag{4.5.1}$$

1. 欧拉角转换成四元数（Python）

（1）根据表达式

def rpy2quaternion(roll, pitch, yaw):
    x=sin(pitch/2)sin(yaw/2)cos(roll/2)+cos(pitch/2)cos(yaw/2)sin(roll/2)
    y=sin(pitch/2)cos(yaw/2)cos(roll/2)+cos(pitch/2)sin(yaw/2)sin(roll/2)
    z=cos(pitch/2)sin(yaw/2)cos(roll/2)-sin(pitch/2)cos(yaw/2)sin(roll/2)
    w=cos(pitch/2)cos(yaw/2)cos(roll/2)-sin(pitch/2)sin(yaw/2)sin(roll/2)
    return x, y, z, w

(2)transforms3d库函数

import transforms3d as tfs
tfs.euler.euler2quat(0,0,0,"sxyz")

2. 欧拉角转换成四元数（C++）

（1）tf库函数

#include <tf/tf.h>
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
#create ros msg
tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)

（六）四元数转换成欧拉角

理论推导：

$$roll,pitch,yaw分别为 \alpha ， \beta ， \gamma ,则有：$$

$$\begin{cases} \alpha = atan2(2(wx+yz), 1-2(x^2 + y^2)) \\ \beta= asin(2(wy-xz)) \\ \gamma = atan2(2(wz+xy), 1-2(z^2 + y^2)) \end{cases} \tag{4.6.1}$$

1. 四元数转换成欧拉角（Python）

（1）根据表达式

def quart_to_rpy(x, y, z, w):
    roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
    pitch = math.asin(2 * (w * y - x * z))
    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (z * z + y * y))
    return roll, pitch, yaw

（2）tf库函数

import tf
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])

（3）transforms3d库函数

import transforms3d as tfs
tfs.euler.quat2euler([1,0,0,0],"sxyz")

2. 四元数转换成欧拉角（C++）

（1）tf库

#include <tf/tf.h>
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);

double roll, pitch, yaw;//定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换

参考链接
1. 欧拉角，四元数，旋转矩阵相互转化（c++, python)
2. 开源推荐：写机器人算法，你必须掌握的python开源库
3. 《动手学ROS2》7.3.2 动手学齐次坐标变换
4. Transforms3d
5. 《动手学ROS2》7.2.4动手学姿态的多种表示