网站建设项目目标描述,互联虚拟主机,手游推广渠道,黄浦网站制作有任何问题请在评论区留言#xff0c;我尽可能的回复大家
一. 逆运动学的求解需要以下数学运算
利用DH参数得到每个关节的变换矩阵#xff1b;利用变换矩阵求出机械臂整个链的变换矩阵#xff1b;求出末端位姿#xff1b;利用已知末端位姿和整个链的变换矩阵#xff0c;…有任何问题请在评论区留言我尽可能的回复大家
一. 逆运动学的求解需要以下数学运算
利用DH参数得到每个关节的变换矩阵利用变换矩阵求出机械臂整个链的变换矩阵求出末端位姿利用已知末端位姿和整个链的变换矩阵通过逆运动学方程来求解关节角度根据需求选解。
二. 代码实现过程
利用DH参数得到每个关节的变换矩阵
Eigen::Matrix4d DH(double a, double d, double alpha, double theta) {Eigen::Matrix4d T;T cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta),sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),0, sin(alpha), cos(alpha), d,0, 0, 0, 1;return T;
}利用变换矩阵求出机械臂整个链的变换矩阵
Eigen::Matrix4d T_0e Eigen::Matrix4d::Identity(4,4);
for (int i 0; i n; i) {T_0e T_0e * DH(a[i], d[i], alpha[i], theta[i]);
}求出末端位姿
Eigen::Matrix4d T_ee;
T_ee 0.5938, -0.7381, 0.3254, 0.4494,0.8038, 0.5531, 0.2194, -0.1957,-0.0332, 0.3868, 0.9214, 0.6733,0, 0, 0, 1;然后利用已知末端位姿和整个链的变换矩阵通过逆运动学方程来求解关节角度。
三. 逆运动学方程求解关节角度
逆运动学方程求解关节角度是一个非线性方程组有多种方法求解如解析解、数值解等。这里以数值解的方法为例介绍如何用c代码实现逆运动学方程的求解。
实现齐次变换矩阵的逆变换
Eigen::Matrix4d invT(const Eigen::Matrix4d T) {Eigen::Matrix4d invT;invT.block3,3(0,0) T.block3,3(0,0).transpose();invT.block3,1(0,3) -invT.block3,3(0,0)*T.block3,1(0,3);invT.block1,4(3,0) 0, 0, 0, 1;return invT;
}实现逆运动学方程
Eigen::Matrixdouble,6,1 inverseKinematics(const Eigen::Matrix4d T_ee, const Eigen::Matrix4d T_0e, const Eigen::Vector3d p_e,const Eigen::Vector3d o_x,const Eigen::Vector3d o_y,const Eigen::Vector3d o_z) {Eigen::Matrixdouble,6,1 theta;Eigen::Matrix4d T_0e_inv invT(T_0e);Eigen::Matrix4d T_ee_0 T_ee * T_0e_inv;Eigen::Vector3d p_0 T_ee_0.block3,1(0,3);Eigen::Vector3d o_z_0 T_0e_inv.block3,3(0,0) * o_z;Eigen::Vector3d o_y_0 T_0e_inv.block3,3(0,0) * o_y;Eigen::Vector3d o_x_0 T_0e_inv.block3,3(0,0) * o_x;// 具体实现逆运动学方程这里省略return theta;
}其中逆运动学方程的计算的详细过程如下 ● 求解末端位置p和姿态R的关于机器人的参考坐标系的坐标。 ● 根据UR10机械臂的末端位置和姿态计算关节角度。 实现逆运动学方程的代码它计算出的结果是一个长度为6的Eigen向量代表6个关节的角度
#include Eigen/Dense
#include cmathEigen::Matrixdouble, 6, 1 inverseKinematics(const Eigen::Matrix4d T_ee, const Eigen::Matrix4d T_0e, const Eigen::Vector3d p_e,const Eigen::Vector3d o_x,const Eigen::Vector3d o_y,const Eigen::Vector3d o_z)
{Eigen::Matrixdouble, 6, 1 joint_angles;Eigen::Vector3d p_0e T_0e.block3,3(0,0).transpose() * (p_e - T_0e.col(3).head3());double c5 T_ee(2,2);double s5 sqrt(1 - c5*c5);joint_angles(4) atan2(s5, c5);joint_angles(5) atan2(-T_ee(0,2), T_ee(1,2));joint_angles(3) atan2(T_ee(2,1)/s5, T_ee(2,0)/s5);double s3 sin(joint_angles(3));double c3 cos(joint_angles(3));joint_angles(0) atan2((p_0e(1)*s3 - p_0e(2)*c3) / s5, p_0e(0) - (p_0e(1)*c3 p_0e(2)*s3) * c5);joint_angles(2) atan2((p_0e(1)*c3 p_0e(2)*s3) / c5, p_0e(0) - p_0e(1)*s3 p_0e(2)*c3);joint_angles(1) atan2(o_y(0), o_x(0));return joint_angles;
}根据这个计算流程将步骤 2 中省略的逆运动学方程具体实现的代码补充上
Eigen::Matrixdouble,6,1 inverseKinematics(const Eigen::Matrix4d T_ee, const Eigen::Matrix4d T_0e, const Eigen::Vector3d p_e,const Eigen::Vector3d o_x,const Eigen::Vector3d o_y,const Eigen::Vector3d o_z) {Eigen::Matrixdouble,6,1 theta;Eigen::Matrix4d T_0e_inv invT(T_0e);Eigen::Matrix4d T_ee_0 T_ee * T_0e_inv;Eigen::Vector3d p_0 T_ee_0.block3,1(0,3);Eigen::Vector3d o_z_0 T_0e_inv.block3,3(0,0) * o_z;Eigen::Vector3d o_y_0 T_0e_inv.block3,3(0,0) * o_y;Eigen::Vector3d o_x_0 T_0e_inv.block3,3(0,0) * o_x;
// 逆运动学方程的具体实现double q1, q2, q3, q4, q5, q6;double d p_e(2) - p_0(2);q1 atan2(p_0(1), p_0(0));double c2 (pow(p_0(0), 2) pow(p_0(1), 2) - pow(d, 2) - pow(o_x_0(2), 2)) / (2 * o_x_0(2) * sqrt(pow(p_0(0), 2) pow(p_0(1), 2) - pow(d, 2)));q2 atan2(sqrt(1-pow(c2, 2)), c2);q3 atan2(o_z_0(2), -o_x_0(0) * sin(q2) o_x_0(2) * cos(q2));double s4 -o_y_0(2) * cos(q2) - o_y_0(0) * sin(q2) * sin(q3) o_y_0(1) * sin(q2) * cos(q3);double c4 o_x_0(0) * cos(q3) o_x_0(1) * sin(q3) o_x_0(2) * sin(q2);q4 atan2(s4, c4);double s5 o_x_0(0) * cos(q3) * sin(q4) o_x_0(1) * sin(q3) * sin(q4) o_x_0(2) * cos(q4);double c5 o_y_0(0) * cos(q3) * cos(q4) o_y_0(1) * sin(q3) * cos(q4) - o_y_0(2) * sin(q4);q5 atan2(-s5, c5);double s6 -o_x_0(0) * sin(q3) o_x_0(1) * cos(q3);double c6 o_y_0(0) * cos(q3) * cos(q5) o_y_0(1) * sin(q3) * cos(q5) - o_y_0(2) * sin(q5);q6 atan2(s6, c6);theta q1, q2, q3, q4, q5, q6;return theta;
} 上面代码中的逆运动学方程的返回值 theta 可能会有多组解UR机械臂通常为8组解但是通常情况下仅返回一组最合适的解因为它对应的正运动学方程只能够求出一组解。如果机器人的关节范围限制了某些解的取值范围则需要在代码中加入关节范围限制的判断以保证返回的解在关节范围内。 在当前代码中并没有对多组解进行选取的部分所以该代码中直接返回的是求得的一组解。因为选取某一组解的方式取决于你所实现的逆运动学算法以及实际的应用需求对于不同的需求还需要对代码进行进一步的修改以实现选取一组合法的解的功能。 对于选解我在这里举一个例子机械臂六个关节角度均有最大和最小的限制。 那么选解的代码可以写为
if (q1 q1_min) {q1 q1 2 * M_PI;}if (q1 q1_max) {q1 q1 - 2 * M_PI;}// 根据需求确定q2的取值范围if (q2 q2_min) {q2 q2_min;}if (q2 q2_max) {q2 q2_max;}// 根据需求确定q3的取值范围if (q3 q3_min) {q3 q3_min;}if (q3 q3_max) {q3 q3_max;}// 根据需求确定q4的取值范围if (q4 q4_min) {q4 q4_min;}if (q4 q4_max) {q4 q4_max;}// 根据需求确定q5的取值范围if (q5 q5_min) {q5 q5_min;}if (q5 q5_max) {q5 q5_max;}// 根据需求确定q6的取值范围if (q6 q6_min) {q6 q6_min;}if (q6 q6_max) {q6 q6_max;}theta q1, q2, q3, q4, q5, q6;return theta;
