培训课程资料 - 百度网盘下载链接 (离线版PDF文件 + 开源代码 + 软件工具包)
链接:https://pan.baidu.com/s/1M5y_Qk6CvFyuNqi1fAERpg 提取码:asvu
机械臂关节定义
机械臂基坐标系
关节编号
关节坐标系
关节1坐标系
关节2坐标系
关节3坐标系
关节4坐标系(腕关节)
关节5坐标系(末端)
连杆长度
数学基础
余弦公式
在几何法逆向运动学的求解中, 一个比较重要的公式就是余弦定理 , 已知三角形的三个边长, 让你求解出来每个夹角, 余弦公式如下:
平面1自由度机械臂
最简单的情况, 是只有一个自由度, 即假设只有底部的舵机可以旋转。
正向运动学
逆向运动学
平面2自由度机械臂
如果去除了底部的关节1, 剩下的关节2, 关节3, 关节4就变成了平面的机械臂。
从简单到复杂, 我们先来看平面二自由度机械臂的情况.
需要注意的是,根据右手螺旋法则可以知道 ,
正向运动学
关节2的坐标值
关节3的坐标值
关节4的坐标值
逆向运动学
求解
与 互补.
使用余弦公式可得
为了方便求解,给 添加约束
求解
根据余弦定理可得
根据右手螺旋定则,可以知道 是小于0的, , 所以
求解
注:Arduino机械臂逆向运动学求解,使用的是解析法求解的 ,最后求得的结果一样.
平面3自由度机械臂
正向运动学
继续前面平面二自由度公式推导
我们定义一个角度为俯仰角
逆向运动学
如果想求解平面三自由度机械臂的逆向运动学,除了指定末端的坐标 之外,还需要指定一个俯仰角
于是,平面三自由度的逆向运动学求解问题, 就变成了平面二自由度机械臂逆向运动学的问题。 后续步骤同上.
空间四自由度机械臂
现在需要将底部的旋转关节考虑进入, 引入 .
正向运动学
再上面平面3自由度机械臂 部分我们推导出来了 在坐标系 坐标系下面的坐标。
注: 左上角的标识,代表参考坐标系
接下来需要先将其转换到关节1坐标系下的坐标.
然后再转换为机械臂基坐标系下的坐标, 引入关节1的旋转
逆向运动学
逆向运动学,需要先将空间四自由度机械臂的问题,转换为平面三自由度机械臂的问题.
计算末端坐标在关节1坐标系下的表示
然后转换为关节2‘坐标系下的坐标
然后再将俯仰角 一起带入平面3自由度机械臂 里面的逆向运动学求解过程.
机械臂运动学(Arduino)
正向运动学
// 机械臂正向运动学
void FSARM_ARM5DoF::forwardKinematics(FSARM_JOINTS_STATE_T thetas, FSARM_POINT3D_T* toolPosi, float* pitch){
FSARM_POINT3D_T wristPosi; //腕关节原点的坐标
// 求解pitch
*pitch = thetas.theta2 + thetas.theta3 + thetas.theta4;
// 角度转弧度
float theta1 = radians(thetas.theta1);
float theta2 = radians(thetas.theta2);
float theta3 = radians(thetas.theta3);
float theta4 = radians(thetas.theta4);
// 计算腕关节的坐标
toolPosi->x = cos(theta1) * (FSARM_LINK2*cos(theta2)+FSARM_LINK3*cos(theta2+theta3) + FSARM_LINK4*cos(theta2+theta3+theta4));
toolPosi->y = sin(theta1) * (FSARM_LINK2*cos(theta2)+FSARM_LINK3*cos(theta2+theta3) + FSARM_LINK4*cos(theta2+theta3+theta4));
toolPosi->z = -FSARM_LINK2*sin(theta2)-FSARM_LINK3*sin(theta2+theta3)-FSARM_LINK4*sin(theta2+theta3+theta4);
}
逆向运动学
// 机械臂逆向运动学
FSARM_STATUS FSARM_ARM5DoF::inverseKinematics(FSARM_POINT3D_T toolPosi, float pitch, FSARM_JOINTS_STATE_T* thetas){
// 关节弧度
float theta1 = 0.0;
float theta2 = 0.0;
float theta3 = 0.0;
float theta4 = 0.0;
FSARM_POINT3D_T wristPosi; // 腕关节坐标
// 根据工具原点距离机械臂基坐标系的直线距离
float disO2Tool = sqrt(pow(toolPosi.x,2) + pow(toolPosi.y, 2) + pow(toolPosi.z, 2));
if (disO2Tool > (FSARM_LINK2+FSARM_LINK3+FSARM_LINK4)){
return FSARM_STATUS_TOOLPOSI_TOO_FAR;
}
// 判断腕关节的原点是否在机械臂坐标系的Z轴上
if (toolPosi.x == 0 && toolPosi.y == 0){
// 让theta1保持跟原来相同
theta1 = radians(this->servos[FSARM_JOINT1].queryRawAngle());
}else{
// 求解theta1
theta1 = atan2(toolPosi.y, toolPosi.x);
thetas->theta1 = degrees(theta1);
// 判断theta1是否合法
if (!servos[FSARM_JOINT1].isAngleLegal(thetas->theta1)){
return FSARM_STATUS_JOINT1_OUTRANGE;
}
}
// 俯仰角, 角度转弧度
float pitch_rad = radians(pitch);
// 计算腕关节的位置
wristPosi.x = toolPosi.x - FSARM_LINK4*cos(pitch_rad)*cos(theta1);
wristPosi.y = toolPosi.y - FSARM_LINK4*cos(pitch_rad)*sin(theta1);
wristPosi.z = toolPosi.z + FSARM_LINK4*sin(pitch_rad);
// 计算theta3
float b;
if(cos(theta1) !=0){
b = wristPosi.x / cos(theta1);
}else{
b = wristPosi.y / sin(theta1);
}
float cos_theta3 = (pow(wristPosi.z, 2)+pow(b,2) - pow(FSARM_LINK2,2) - pow(FSARM_LINK3, 2))/(2*FSARM_LINK2*FSARM_LINK3);
float sin_theta3 = sqrt(1 - pow(cos_theta3, 2));
theta3 = atan2(sin_theta3, cos_theta3);
thetas->theta3 = degrees(theta3);
if(!servos[FSARM_JOINT3].isAngleLegal(thetas->theta3)){
return FSARM_STATUS_JOINT3_OUTRANGE;
}
// 计算theta2
float k1 = FSARM_LINK2 + FSARM_LINK3*cos(theta3);
float k2 = FSARM_LINK3 * sin(theta3);
float r = sqrt(pow(k1, 2) + pow(k2, 2));
theta2 = atan2(-wristPosi.z/r, b/r) - atan2(k2/r, k1/r);
thetas->theta2 = degrees(theta2);
if(!servos[FSARM_JOINT2].isAngleLegal(thetas->theta2)){
return FSARM_STATUS_JOINT2_OUTRANGE;
}
// 计算theta4
theta4 = pitch_rad-(theta2 + theta3);
thetas->theta4 = degrees(theta4);
if(!servos[FSARM_JOINT4].isAngleLegal(thetas->theta4)){
return FSARM_STATUS_JOINT4_OUTRANGE;
}
// 成功完成求解
return FSARM_STATUS_SUCCESS;
}
评论(0)
您还未登录,请登录后发表或查看评论