小蓝xlanll

这个屌丝很懒，什么也没留下！

热门标签

机器人位姿数据形式转换与旋转矩阵总结（欧拉角、RPY、NOAP）_坐标和姿态[x, y, z, rx, ry, rz]

作者：小蓝xlanll | 2024-05-13 01:37:14

踩

坐标和姿态[x, y, z, rx, ry, rz]

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

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

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 代表三个坐标轴上的位置；Rx、Ry、Rz 代表机器人姿态，即新坐标分别绕原坐标系中 X，Y，Z 三个坐标轴旋转的角度。

3.四元数（ABB）

暂略。

4.旋转矩阵与NOAP

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

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

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;
}

四、坐标转换

1.一般情况，对于形如下的旋转矩阵 $Rot$ ，对原坐标点 $P_{A}$ 进行左乘，得到旋转后的坐标 $P_{B}$ 。

$Rot=\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}$

$P_{B}=Rot\times P_{A}$

2.若对于已经转置的旋转矩阵 $Rot^{T}$ ，则对原坐标点的转置 $P_{A}^{T}$ 进行右乘，得到旋转后的坐标转置 $P_{B}^{T}$ 。

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

$P_{B}^{T}= P_{A}^{T} \times Rot^{T}$

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/561340