机械手基坐标系和工具坐标系的相互转换

Posted 2022-11-11 Thomas会写字

机械手基坐标系和工具坐标系的相互转换，主要是通过欧拉角来完成的。

double D2R(double D)

	return D / 180 * M_PI;


double R2D(double R)

	return R / M_PI * 180;




void Rx(cv::Mat& mat, double dAngel)

	mat = cv::Mat(3, 3, CV_64FC1);
	double dAngle_ = D2R(dAngel);

	mat.at<double>(0, 0) = 1;
	mat.at<double>(0, 1) = 0;
	mat.at<double>(0, 2) = 0;
	mat.at<double>(1, 0) = 0;
	mat.at<double>(1, 1) = std::cos(dAngle_);
	mat.at<double>(1, 2) = std::sin(dAngle_)*(-1);
	mat.at<double>(2, 0) = 0;
	mat.at<double>(2, 1) = std::sin(dAngle_);
	mat.at<double>(2, 2) = std::cos(dAngle_);



void Ry(cv::Mat& mat, double dAngel)

	mat = cv::Mat(3, 3, CV_64FC1);
	double dAngle_ = D2R(dAngel);

	mat.at<double>(0, 0) = std::cos(dAngle_);
	mat.at<double>(0, 1) = 0;
	mat.at<double>(0, 2) = std::sin(dAngle_);
	mat.at<double>(1, 0) = 0;
	mat.at<double>(1, 1) = 1;
	mat.at<double>(1, 2) = 0;
	mat.at<double>(2, 0) = std::sin(dAngle_)*(-1);
	mat.at<double>(2, 1) = 0;
	mat.at<double>(2, 2) = std::cos(dAngle_);



void Rz(cv::Mat& mat, double dAngel)

	mat = cv::Mat(3, 3, CV_64FC1);
	double dAngle_ = D2R(dAngel);

	mat.at<double>(0, 0) = std::cos(dAngle_);
	mat.at<double>(0, 1) = std::sin(dAngle_)*(-1);
	mat.at<double>(0, 2) = 0;
	mat.at<double>(1, 0) = std::sin(dAngle_);
	mat.at<double>(1, 1) = std::cos(dAngle_);
	mat.at<double>(1, 2) = 0;
	mat.at<double>(2, 0) = 0;
	mat.at<double>(2, 1) = 0;
	mat.at<double>(2, 2) = 1;



/*
说明：从基坐标系到工具坐标系 依据欧拉角换算旋转矩阵
1. EulerAngle工具坐标系相对于基坐标系的欧拉角
2. matR 基坐标系到工具坐标系的旋转矩阵
*/
void EulerAngleToRotateMat_B2T(RobotEulerAngle EulerAngle, cv::Mat& matR)

	cv::Mat rx, ry, rz;
	Rx(rx, EulerAngle.rx);
	Ry(ry, EulerAngle.ry);
	Rz(rz, EulerAngle.rz);
	matR = rz*ry*rx;



/*
说明：从基坐标系到工具坐标系 依据旋转矩阵换算欧拉角
1. matR 基坐标系到工具坐标系的旋转矩阵
2. EulerAngle工具坐标系相对于基坐标系的欧拉角
*/
void RotateMatToEulerAngle_B2T(cv::Mat matR, RobotEulerAngle& EulerAngle)

	EulerAngle.ry =
		std::atan2(
		(-1)*matR.at<double>(2, 0),
		std::sqrt(
		std::pow(matR.at<double>(2, 1), 2) +
		std::pow(matR.at<double>(2, 2), 2)));

	EulerAngle.rx =
		std::atan2(
		matR.at<double>(2, 1) / std::cos(EulerAngle.ry),
		matR.at<double>(2, 2) / std::cos(EulerAngle.ry));

	EulerAngle.rz =
		std::atan2(
		matR.at<double>(1, 0) / std::cos(EulerAngle.ry),
		matR.at<double>(0, 0) / std::cos(EulerAngle.ry));

	EulerAngle.rx = R2D(EulerAngle.rx);
	EulerAngle.ry = R2D(EulerAngle.ry);
	EulerAngle.rz = R2D(EulerAngle.rz);



/*
说明：基坐标系上的点（向量）换算到工具坐标系
1. 基坐标系上的点：   robotData.posInBase
2. 工具坐标系的姿态： robotData.EulerAngleTakePhoto
3. 换算的结果：       robotData.posInTool
*/
void BasePointToTool(RobotStruct & robotData)

	cv::Mat matR, matRes;
	EulerAngleToRotateMat_B2T(robotData.EulerAngleTakePhoto, matR);

	matRes = (cv::Mat_<double>(1, 3) << robotData.posInBase.x, robotData.posInBase.y, robotData.posInBase.z) * matR;
	robotData.posInTool.x = matRes.at<double>(0, 0);
	robotData.posInTool.y = matRes.at<double>(0, 1);
	robotData.posInTool.z = matRes.at<double>(0, 2);



/*
说明：从工具坐标系到基坐标系 依据欧拉角换算旋转矩阵
1. EulerAngle工具坐标系相对于基坐标系的欧拉角
2. matR 基坐标系到工具坐标系的旋转矩阵
*/
void EulerAngleToRotateMat_T2B(RobotEulerAngle EulerAngle, cv::Mat& matR)

	cv::Mat rx, ry, rz;
	Rx(rx, EulerAngle.rx*(-1));
	Ry(ry, EulerAngle.ry*(-1));
	Rz(rz, EulerAngle.rz*(-1));
	matR = rx*ry*rz;



/*
说明：从工具坐标系到基坐标系 依据旋转矩阵换算欧拉角
1. matR 基坐标系到工具坐标系的旋转矩阵
2. EulerAngle工具坐标系相对于基坐标系的欧拉角
*/
void RotateMatToEulerAngle_T2B(cv::Mat matR, RobotEulerAngle& EulerAngle)

	cv::Mat matR_T;
	cv::transpose(matR, matR_T);
	RotateMatToEulerAngle_B2T(matR_T, EulerAngle);



/*
说明：工具坐标系上的点（向量）换算到基坐标系
1. 工具坐标系上的点： robotData.posInTool
2. 工具坐标系的姿态： robotData.EulerAngleTakePhoto
3. 换算的结果：       robotData.posInBase
*/
void ToolPointToBase(RobotStruct & robotData)

	cv::Mat matR, matRes;
	EulerAngleToRotateMat_T2B(robotData.EulerAngleTakePhoto, matR);

	matRes = (cv::Mat_<double>(1, 3) << robotData.posInTool.x, robotData.posInTool.y, robotData.posInTool.z) * matR;
	robotData.posInBase.x = matRes.at<double>(0, 0);
	robotData.posInBase.y = matRes.at<double>(0, 1);
	robotData.posInBase.z = matRes.at<double>(0, 2);



/*
说明：相对基坐标系，先有两个工具坐标的欧拉姿态 EulerAngleA EulerAngleB
      需要得到第三个欧拉姿态 EulerAngleC，可以直接从 EulerAngleA 转换到 EulerAngleB
EulerAngleA 欧拉姿态，相对基坐标系
EulerAngleB 欧拉姿态，相对基坐标系
EulerAngleC 欧拉姿态，相对 EulerAngleA
*/
void EulerAToEulerB(RobotEulerAngle EulerAngleA, RobotEulerAngle EulerAngleB, RobotEulerAngle & EulerAngleC)

	cv::Mat matRA, matRAinv, matRB, matRC;

	EulerAngleToRotateMat_B2T(EulerAngleA, matRA);
	EulerAngleToRotateMat_B2T(EulerAngleB, matRB);
	cv::invert(matRA, matRAinv);
	matRC = matRAinv * matRB;
	RotateMatToEulerAngle_B2T(matRC, EulerAngleC);



/*
说明：工具坐标系相对偏移运动，根据此时的欧拉姿态，将工具端的相对运动量转换到基坐标系
1. robotCurPos.posInBase 此时基坐标系的点坐标
2. robotCurPos.EulerAngleTakePhoto 此时工具坐标系的欧拉姿态（相对基坐标系）
3. posRelative 工具坐标系的相对偏移量
4. robotCurPos.posInBase 最后结果更新到这个变量中
*/
void RobotToolRelativeMove(RobotStruct& robotCurPos, RobotPos posRelative)

	// 交换数据，借用变量 
	RobotPos posInBase, posInTool;
	robotCurPos.posInBase.CopyTo(posInBase);
	robotCurPos.posInTool.CopyTo(posInTool);
	posRelative.CopyTo(robotCurPos.posInTool);

	// 投影向量
	ToolPointToBase(robotCurPos);

	// 还原数据
	robotCurPos.posInBase.x += posInBase.x;
	robotCurPos.posInBase.y += posInBase.y;
	robotCurPos.posInBase.z += posInBase.z;
	posInTool.CopyTo(robotCurPos.posInTool);



/*
说明：工具坐标系相对旋转运动，根据此时的欧拉姿态，工具坐标系继续做一个三轴旋转
      并将 robotCurPos.EulerAngleTakePhoto 和 EulerAngle 合成一个新的欧拉角
robotCurPos.EulerAngleTakePhoto 工具末端初始欧拉姿态
EulerAngle 相对旋转量
robotCurPos.EulerAngleTakePhoto 最后结果更新到这个变量中
*/
void RobotToolRelativeRotate(RobotStruct& robotCurPos, RobotEulerAngle EulerAngle)

	// 计算旋转矩阵
	cv::Mat matR1, matR2, matR;
	EulerAngleToRotateMat_B2T(robotCurPos.EulerAngleTakePhoto, matR1);
	EulerAngleToRotateMat_B2T(EulerAngle, matR2);
	matR = matR1*matR2;

	// 反算此时欧拉角
	RotateMatToEulerAngle_B2T(matR, robotCurPos.EulerAngleTakePhoto);



/*
说明：相对法兰中心的空间某点，做一个相对的平移和旋转。
1. RelativePos 相对法兰中心的空间某点 A
2. RelativeMove 相对 A 点做偏移运动，结果输出在robotCurPos.EulerAngleTakePhoto
3. RelativeRotate 相对 A 点做旋转运动，结果输出在robotCurPos.posInBase
4. 
*/
void RobotToolRelativeMoveRotate(RobotPos RelativePos, RobotPos RelativeMove, RobotEulerAngle RelativeRotate, RobotStruct& robotCurPos)

	// 保持当前姿态下，计算工具端相对偏移量，相当于法兰中心移到了工具末端位置
	RobotToolRelativeMove(robotCurPos,  RelativePos.x + RelativeMove.x, RelativePos.y + RelativeMove.y, RelativePos.z + RelativeMove.z );

	// 调整姿态
	RobotToolRelativeRotate(robotCurPos, RelativeRotate);

	// 反向调整工具端位置偏移
	RobotToolRelativeMove(robotCurPos,  RelativePos.x *(-1), RelativePos.y *(-1), RelativePos.z *(-1) );

Matlab中欧拉角和四元素的互换

R_All = Rx(rx)' * Ry(ry)' * Rz(rz)'; 
[Qur_W, Qur_X, Qur_Y, Qur_Z] = GetQurFromRotateMat(R_All);
Qur_Norm = norm([Qur_X, Qur_Y, Qur_Z, Qur_W]);% =1
Qur_R = ...
    [1 - 2*Qur_Y^2 - 2*Qur_Z^2, 2*Qur_X*Qur_Y - 2*Qur_Z*Qur_W, 2*Qur_X*Qur_Z + 2*Qur_Y*Qur_W;
     2*Qur_X*Qur_Y + 2*Qur_Z*Qur_W, 1 - 2*Qur_X^2 - 2*Qur_Z^2, 2*Qur_Y*Qur_Z - 2*Qur_X*Qur_W;
     2*Qur_X*Qur_Z - 2*Qur_Y*Qur_W, 2*Qur_Y*Qur_Z + 2*Qur_X*Qur_W, 1 - 2*Qur_X^2 - 2*Qur_Y^2];
% R_All=Qur_R 就对了，本质上都是旋转矩阵


function [Qur_W, Qur_X, Qur_Y, Qur_Z] = GetQurFromRotateMat(Mat)

    Qur_W = sqrt(1+Mat(1,1)+Mat(2,2)+Mat(3,3))/2;
    Qur_X = (Mat(3,2)-Mat(2,3))/(4*Qur_W);
    Qur_Y = (Mat(1,3)-Mat(3,1))/(4*Qur_W);
    Qur_Z = (Mat(2,1)-Mat(1,2))/(4*Qur_W);
end