机械手基坐标系和工具坐标系的相互转换
机械手基坐标系和工具坐标系的相互转换，主要是通过欧拉角来完成的。
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(0, 0) = 1;
mat.at(0, 1) = 0;
mat.at(0, 2) = 0;
mat.at(1, 0) = 0;
mat.at(1, 1) = std::cos(dAngle_);
mat.at(1, 2) = std::sin(dAngle_)*(-1);
mat.at(2, 0) = 0;
mat.at(2, 1) = std::sin(dAngle_);
mat.at(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(0, 0) = std::cos(dAngle_);
mat.at(0, 1) = 0;
mat.at(0, 2) = std::sin(dAngle_);
mat.at(1, 0) = 0;
mat.at(1, 1) = 1;
mat.at(1, 2) = 0;
mat.at(2, 0) = std::sin(dAngle_)*(-1);
mat.at(2, 1) = 0;
mat.at(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(0, 0) = std::cos(dAngle_);
mat.at(0, 1) = std::sin(dAngle_)*(-1);
mat.at(0, 2) = 0;
mat.at(1, 0) = std::sin(dAngle_);
mat.at(1, 1) = std::cos(dAngle_);
mat.at(1, 2) = 0;
mat.at(2, 0) = 0;
mat.at(2, 1) = 0;
mat.at(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(2, 0),
std::sqrt(
std::pow(matR.at(2, 1), 2) +
std::pow(matR.at(2, 2), 2)));
EulerAngle.rx =
std::atan2(
matR.at(2, 1) / std::cos(EulerAngle.ry),
matR.at(2, 2) / std::cos(EulerAngle.ry));
EulerAngle.rz =
std::atan2(
matR.at(1, 0) / std::cos(EulerAngle.ry),
matR.at(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_(1, 3) << robotData.posInBase.x, robotData.posInBase.y, robotData.posInBase.z) * matR;
robotData.posInTool.x = matRes.at(0, 0);
robotData.posInTool.y = matRes.at(0, 1);
robotData.posInTool.z = matRes.at(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_(1, 3) << robotData.posInTool.x, robotData.posInTool.y, robotData.posInTool.z) * matR;
robotData.posInBase.x = matRes.at(0, 0);
robotData.posInBase.y = matRes.at(0, 1);
robotData.posInBase.z = matRes.at(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