将自己最近所了解的机械臂运动学正解验证知识总结,供大家参考。
以遨博I5机械臂为例,使用改进的Dh参数,在matlab机器人工具箱中进行验证,最后将变换矩阵T转为位置和姿态(欧拉角)。
验证环境:
matlab2020b(ubuntu18.04)
Eigen库将矩阵转为欧拉角
1、auboI5改进Dh参数
2、matlab验证程序及结果
clc
d1 = 0.0985
d2 = 0.1215
a2 = 0.408
a3 = 0.376
d5 = 0.1025
d6 = 0.094
L(1)=Link([0,d1,0,0,0,pi], 'modified')
L(2)=Link([0,d2,0,-pi/2,0,-pi/2], 'modified')
L(3)=Link([0,0,a2,pi], 'modified')
L(4)=Link([0,0,a3,pi,0,-pi/2], 'modified')
L(5)=Link([0,d5,0,-pi/2], 'modified')
L(6)=Link([0,d6,0,pi/2], 'modified')
robot =SerialLink(L,'name','aubo_i5')
title("T")
T = robot.fkine([0 0 0 0 0 0])
robot.plot([0 0 0 0 0 0])
左上角3*3矩阵表示姿态,红框3*1依次是X Y Z。
3、aubo机械臂位姿
对上了~
4、使用Eigen将姿态矩阵转为欧拉角
程序如下:
const double ARC_TO_DEG = 57.29577951308238;
const double DEG_TO_ARC = 0.0174532925199433;
bool isRotationMatirx(Eigen::Matrix3d R)
{
double err=1e-6;
Eigen::Matrix3d shouldIdenity;
shouldIdenity=R*R.transpose();
Eigen::Matrix3d I=Eigen::Matrix3d::Identity();
return (shouldIdenity - I).norm() < err;
}
Eigen::Vector3d rotationMatrixToEulerAngles(Eigen::Matrix3d &R)
{
assert(isRotationMatirx(R));
double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = atan2( R(2,1), R(2,2));
y = atan2(-R(2,0), sy);
z = atan2( R(1,0), R(0,0));
}
else
{
x = atan2(-R(1,2), R(1,1));
y = atan2(-R(2,0), sy);
z = 0;
}
return {x, y, z};
}
int main(int argc, char *argv[])
{
// 设定欧拉角(角度),绕固定轴
double roll_deg = 90.0f; // 绕X轴
double pitch_deg = 0.0f; // 绕Y轴
double yaw_deg = 0.0f; // 绕Z轴
// double roll_deg = 178.340073; // 绕X轴
// double pitch_deg = 1.419545; // 绕Y轴
// double yaw_deg = 133.484192; // 绕Z轴
// 转化为弧度
double roll_arc = roll_deg * DEG_TO_ARC; // 绕X轴
double pitch_arc = pitch_deg * DEG_TO_ARC; // 绕Y轴
double yaw_arc = yaw_deg * DEG_TO_ARC; // 绕Z轴
cout << endl;
cout << "roll_arc = " << roll_arc << endl;
cout << "pitch_arc = " << pitch_arc << endl;
cout << "yaw_arc = " << yaw_arc << endl;
// 初始化欧拉角(rpy),对应绕x轴,绕y轴,绕z轴的旋转角度
Eigen::Vector3d euler_angle(roll_arc, pitch_arc, yaw_arc);
// 使用Eigen库将欧拉角转换为旋转矩阵
Eigen::Matrix3d rotation_matrix1;
rotation_matrix1 = Eigen::AngleAxisd(euler_angle[2], Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(euler_angle[1], Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(euler_angle[0], Eigen::Vector3d::UnitX());
cout << "\nrotation matrix1 =\n" << rotation_matrix1 << endl << endl;
// 使用自定义函数将旋转矩阵转换为欧拉角
Eigen::Vector3d eulerAngle2 = rotationMatrixToEulerAngles(rotation_matrix1); // roll,pitch,yaw
cout << "roll_2 pitch_2 yaw_2 = " << eulerAngle2[0] << " " << eulerAngle2[1]
<< " " << eulerAngle2[2] << endl << endl;
}
结果如下:
最后:参考文章
Ubuntu下安装matlab: https://zhuanlan.zhihu.com/p/429084566
机械臂改进型DH参数:https://blog.csdn.net/ACE_YOUNG/article/details/125595485
Matlab验证:https://blog.csdn.net/qq_31253399/article/details/106188637
Matlab内部有直接姿态矩阵转欧拉角的,奈何ubuntu虚拟机空间不够,开始时没装航空工具箱。。。
原理推导等有机会再详细总结。
欢迎关注公众号:
如需完整程序可后台留言"机械臂运动学正解验证"。