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

Vodka~

于 2024-09-01 14:05:13 发布

阅读量280

点赞数 7

分类专栏：机器人数学文章标签：矩阵线性代数机器人

本文链接：https://blog.csdn.net/Vodka688/article/details/141684887

版权

机器人同时被 2 个专栏收录

18 篇文章 2 订阅

订阅专栏

数学

8 篇文章 0 订阅

订阅专栏

栗子

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