机器人学——机械臂运动轨迹规划实例

2024/12/22 17:10:49 来源：https://blog.csdn.net/Vodka688/article/details/141684887 浏览: 次关键词：机器人学——机械臂运动轨迹规划实例

栗子

在这里插入图片描述
Cragi DH

标准DH

逆解

在这里插入图片描述

via points

在这里插入图片描述

条件设定

在这里插入图片描述

求末端的旋转矩阵

在这里插入图片描述

由z-y-x顺序构建的旋转矩阵反推欧拉角

http://t.csdnimg.cn/9tnke
在这里插入图片描述

由z-y-z顺序构建的旋转矩阵反推欧拉角

http://t.csdnimg.cn/GmsBH
在这里插入图片描述

c++代码

#include <iostream>
#include <cmath>
using namespace std;
int main(){//求z(α)y(β)z(γ)构建的旋转矩阵，求三个角度double x,y,pi=3.1415926,alpha=0,Beta=0,Gama=0;double r[4][4];for(int i=1;i<=3;++i){for(int j=1;j<=3;++j)cin >> r[i][j];}Beta = atan2(sqrt(pow(r[3][1],2)+pow(r[3][2],2)),r[3][3]) * 180/pi;//特殊情况if(Beta==0){alpha = 0;Gama = round(atan2(-r[1][2],r[1][1]));}else if(Beta == 180){alpha = 0;Gama = round(atan2(r[1][2],-r[1][1]));}else {alpha = round(atan2(r[2][3]/sin(Beta),r[1][3]/sin(Beta)) * 180/pi);Gama = round(atan2(r[3][2]/sin(Beta),-r[3][1]/sin(Beta)) * 180/pi);}cout << "alpha:" << alpha << endl;cout << "beta:" << round(Beta) << endl;cout << "gama:" << Gama << endl;return 0;
}