机器人位姿数据形式转换与旋转矩阵总结（欧拉角、RPY、NOAP）

一、机器人位姿数据的基本概念

以下概念仅指机器人轨迹规划领域内的位姿坐标，与广义概念无关。

1.欧拉角（KUKA）

欧拉角用来唯一地确定定点转动刚体位置的三个一组独立角参量，由章动角θ、进动角ψ和自转角φ组成。

机器人位姿数据中，数据格式为{ X , Y , Z , A , B , C }

其中，X、Y、Z 代表三个坐标轴上的位置；A、B、C 代表机器人姿态，即新坐标系分别绕原坐标系中 Z，Y，X 三个坐标轴旋转的角度。

2.RPY（新松）

RPY角是一种表示机体姿态的旋转角度，它由三个分量组成：Roll（横滚）、Pitch（俯仰）和Yaw（偏航）。

机器人位姿数据中，数据格式为{ X , Y , Z , Rx , Ry , Rz }

其中，X、Y、Z 代表三个坐标轴上的位置；A、B、C 代表机器人姿态，即新坐标系分别绕原坐标系中 X，Y，Z 三个坐标轴旋转的角度。

3.四元数（ABB）

暂略。

4.旋转矩阵与NOAP

旋转矩阵：旋转矩阵是在乘以一个向量的时候改变向量方向但不改变向量大小，并保持手性的矩阵。顺便一提，机器人中的XYZ坐标系均符合右手定则且正交。

机器人位姿数据中，通用的旋转矩阵为4×4正交矩阵。

NOAP坐标：N、O、A均为用原本XYZ坐标系中的坐标来进行描述的单位向量，三者正交并组成了新的右手坐标系，以此描述此时机器人位姿坐标系与原XYZ坐标系间的变化关系。

对于欧拉角，用NOAP描述的旋转矩阵，形如下：

$\begin{bmatrix} N_{C}& O_{C}& A_{C}& P_{X}\\ N_{B}& O_{B}& A_{B}& P_{Y}\\ N_{A}& O_{A}& A_{A}& P_{Z}\\ 0& 0& 0& 1\\ \end{bmatrix}$

对于RPY角，用NOAP描述的旋转矩阵，形如下：

$\begin{bmatrix} N_{X}& O_{X}& A_{X}& P_{X}\\ N_{Y}& O_{Y}& A_{Y}& P_{Y}\\ N_{Z}& O_{Z}& A_{Z}& P_{Z}\\ 0& 0& 0& 1\\ \end{bmatrix}$

二、各类位姿数据转换公式

1.欧拉角与RPY角转换

由定义易知，{ X , Y , Z , A , B , C } 与 { X , Y , Z , Rx , Ry , Rz } 间互相转换，仅需将A、C或Rx、Rz对调即可。

2.RPY角数据转NOAP坐标公式

$\begin{bmatrix} N_{X}& O_{X}& A_{X}& P_{X}\\ N_{Y}& O_{Y}& A_{Y}& P_{Y}\\ N_{Z}& O_{Z}& A_{Z}& P_{Z}\\ 0& 0& 0& 1\\ \end{bmatrix} = \begin{bmatrix} cosZ cosY&cosZ sinY sinX - sinZ cosX & cosZsinYcosX + sinZsinX& P_{X}\\ sinZcosY& sinZsinYsinX + cosZcosX& sinZsinYcosX - cosZsinX& P_{Y}\\ -sinY& cosYsinX& cosYcosX& P_{Z}\\ 0& 0& 0& 1\\ \end{bmatrix}$

欧拉角数据转NOAP坐标公式仅需将上一公式中的X与Z对调。

3.NOAP坐标转RPY角数据公式

$Y= arctan\left ( \frac{-N_{Z}}{\sqrt{N_{X}^{2}+N_{Y}^{2}}}\right )$

$X= arctan\left ( \frac{O_{Z}}{N_{Z}} \right )$

$Z= arctan\left ( \frac{N_{Y}}{N_{Z}} \right )$

NOAP坐标转欧拉角数据公式仅需将上一公式中的X与Z对调。

三、代码实现

1.RPY角数据转NOAP坐标

注：此函数已将输出转置。

vector <double> RpyTrans::Rpy_to_NOAP(double rx_, double ry_, double rz_)
{
	const double Pi = acos(-1);
	double rx = rx_ / 180.0 * Pi, ry = ry_ / 180.0 * Pi, rz = rz_ / 180.0 * Pi;
	vector <double> res;
	res.clear();
	res.resize(9);
	double Nx = cos(rz) * cos(ry);
	double Ny = sin(rz) * cos(ry);
	double Nz = -sin(ry);

	double Ox = (cos(rz) * sin(ry) * sin(rx)) - (sin(rz) * cos(rx));
	double Oy = (sin(rz) * sin(ry) * sin(rx)) + (cos(rz) * cos(rx));
	double Oz = cos(ry) * sin(rx);

	double Ax = (cos(rz) * sin(ry) * cos(rx)) + (sin(rz) * sin(rx));
	double Ay = (sin(rz) * sin(ry) * cos(rx)) - (cos(rz) * sin(rx));
	double Az = cos(ry) * cos(rx);

	res[0] = Nx; res[1] = Ny; res[2] = Nz;
	res[3] = Ox; res[4] = Oy; res[5] = Oz;
	res[6] = Ax; res[7] = Ay; res[8] = Az;

	return res;
}

2.NOAP坐标转RPY角数据

注：此函数已将输入转置。

vector <double> RpyTrans::NOAP_to_Rpy(vector <double> Rpy)
{
	double rx, ry, rz;
	Rpy.resize(9);
	const double Pi = acos(-1);

	ry = atan2(-Rpy[2], sqrt(pow(Rpy[0], 2) + pow(Rpy[1], 2)));
	rx = atan2(Rpy[5] / cos(ry), Rpy[8] / cos(ry));
	rz = atan2(Rpy[1] / cos(ry), Rpy[0] / cos(ry));
	
	double rx_, ry_, rz_;
	rx != 0 ? rx_ = rx / Pi * 180.0 : rx_ = 0;
	ry != 0 ? ry_ = ry / Pi * 180.0 : ry_ = 0;
	rz != 0 ? rz_ = rz / Pi * 180.0 : rz_ = 0;

	abs(rx_) < 0.00000001 ? rx_ = 0.0 : rx_ = rx_;
	abs(ry_) < 0.00000001 ? ry_ = 0.0 : ry_ = ry_;
	abs(rz_) < 0.00000001 ? rz_ = 0.0 : rz_ = rz_;

	vector <double> res;
	res.push_back(rx_);
	res.push_back(ry_);
	res.push_back(rz_);
	return res;
}