正逆运动学解（三维）

我又来了！之前写了一个在二维直角坐标下的正逆运动学解今天写一下在三维坐标下如何进行正逆运动学的计算及程序实现。
一、正运动学
首先还是要建立一个坐标系，依然以机械臂举例，这里放一个正视图和一个俯视图。

正运动学：已知α、β、γ求C点坐标
咱们废话不多说了直接上公式
先求出AEAE=L2cos(α)+L3cos(β)
然后是X坐标X=AEcos(γ)
Y轴Y=AEsin(γ)
最后Z轴Z=L2sin(α)-L3sin(β)
转换成程序实现

#include<math.h>
#include<stdio.h>
float Alpha,Beta,Gamma,AE,L2,L3;
struct ForwardKinematics
{float x;float y;float z;
}h;
void main()
{AE=L2*cos(Alpha)+L3*cos(Beta);h.x=AE*cos(Gamma);h.y=AE*sin(Gamma);h.z=L2*sin(Alpha)-L3*sin(Beta);
}

正运动学解在运算上还是很简单的
二、逆运动学
还是用正运动学建立的坐标系，算法就比较复杂了

采用程序实现：

#include<math.h>
#include<stdio.h>
float x,y,z,AE,L2,L3;
struct ForwardKinematics
{float Alpha;float Beta;float Gamma;
}h;
void main()
{AE=sqrt(x*x+y*y);A=atan(z/(x*x+y*y))*180/PI+acos((L2*L2+z*z+(x*x+y*y)*(x*x+y*y)-L3*L3)/(2*L2*sqrt(z*z+(x*x+y*y)*(x*x+y*y)*(x*x+y*y)*(x*x+y*y))))*180/PI;h.Alpha=A;h.Beta=180-acos((L2*L2+L3*L3-z*z-(x*x+y*y)*(x*x+y*y)*(x*x+y*y)*(x*x+y*y))/(2*L2*L3))*180/PI-A;h.Gamma=atan(y/x)*180/PI;

修正：三维计算公式里边的AE应该=aqrt（x^2 + y^2），感谢纠正。

正逆运动学解（三维）相关推荐

机械臂学习——舵机的速度控制、坐标变换、DH模型、正运动学解、逆运动学解
机械臂学习文章目录机械臂学习前言一.舵机的速度控制 1.舵机概述 2.代码实现二.机械臂的坐标变换 1.坐标变换 2.坐标的几何概述三.DH模型和正运动学解四.逆运动学解 1.逆运动学几 ...
修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
V-rep学习笔记：机器人逆运动学解算
IK groups and IK elements VREP中使用IK groups和IK elements来进行正/逆运动学计算,一个IK group可以包含一个或者多个IK elements: I ...
数值法求六轴机械臂逆运动学解
问题描述:已知工具坐标系相对于固定坐标系的期望位置和姿态,如何求解满足期望位姿的关节角? 本篇将通过基于雅克比矩阵的牛顿-拉夫森迭代法求运动学逆解. 牛顿 - 拉夫森法(Newton - Raphso ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
三轴机械臂/三自由度四足单腿DH正逆运动学及matlab验证
实物模型 DH建立坐标系以及正逆运动学推导 Matlab验证 clear; clc; a1=-9.57*0.001;alpha1=pi/2; a2=-59.2*0.001; a3=-77*0.001; ...
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵
MATLAB机器人工具箱[1]-- 机械臂建模+正逆运动学+雅克比矩阵 1. 二维空间位姿描述 2. 三维空间位姿描述 3. 建立机器人模型 3.1 Link 类 3.2 SerialLink 类 3 ...
六自由度机器人正逆运动学
简介本文主要是对传统六自由度机器人进行正逆运动学求解,选取大族机器人Elfin05 为分析的对象,开发语言是C++.(完善中) 机器人正运动学机器人正运动学推导过程各关节坐标系确定的通用方法: ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...

正逆运动学解（三维）

正逆运动学解（三维）相关推荐

最新文章

热门文章