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

左式 = 右式，372c1- 227s1 = 0 ,求出 θ1= 59°左式: T01逆矩阵 * T06 * T56逆矩阵：右式 = T12 连乘至 T45：求得θ1=59°。

Vodka~

724人浏览 · 2024-09-01 14:05:13

Vodka~ · 2024-09-01 14:05:13 发布

栗子

在这里插入图片描述
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;
}