三轴机械臂逆运动学解算（附代码）

凛冬未尽 2023-04-18 原文

三轴机械臂逆运动学解算（附代码）

机械臂运动位姿的求解有两种方式

一、正运动学

通过控制已知的连轴（舵机或电机）的旋转角度，求出机械臂终端的空间坐标

二、逆运动学

通过已知的抓取点的空间坐标，求解出三个舵机所需要转动的角度，这里主要讲解逆运动学解法

此处θ1 ，θ2， θ3是三个舵机所需转动的角度，γ是杆3相对于x轴的夹角，根据刚体旋转，逆运动学求解，会得到两个解，即有两种姿态，相对于前一个杆逆时针旋转的夹角为正，顺时针为负。如上图第一种姿态（上折线），θ1＜0，θ2<0，θ3＞0

三、求解

（这里就第一种姿态求解）

①求B点坐标

已知空间点A坐标x,y。（因为三杆必然处于同于平面，若涉及z坐标，则机械臂下方云台的解算应考虑在内。

Bx = x - L3*cos(γ);

By = y - L3*sin(γ);

②求出θ1

cosβ = (Bx^2+ By^2 + L1^2 -L2^2) / (2* L1 *sqrt(Bx^2 +By^2));

β = acos(cosβ)*180/pi;

α = atan2(By,Bx)*180/pi;

θ1=－（pi/2-α-β）; //数学上θ1为正，但应用上应为负值

③求出θ2

cosθ2 = (Bx^2 + By^2 - L1^2 - L2^2)/(2* L1 *L2);

θ2 = - acos(cosθ2 )*180/pi； //同理θ2改为负值

④求出θ3（这里的γ需预先定好）

通过γ = θ1 + θ2 + θ3

求出θ3 = γ- θ1- θ2

至此三个角度结算完毕

四、代码实现

#include “stdio.h”
#include “math.h”
float theta[3];
#define pi 3.1415
void caculate(float x, float y, float z, float * ptheta)
{
float L1 = 0.14; //杆长
float L2 = 0.11;
float L3 = 0.04;
Bx=x-L3; //这里设置γ为-90
By=y;
lp=Bx* Bx+By* By;
if (sqrt(lp)>=L1+L2 || sqrt(lp)<=fabs(L1-L2))
printf(“unreached”);
alpha = atan2(By,Bx);
beta = acos((L1* L1+lp-L2* L2)/(2* L1* sqrt(lp))); //这里使用弧度制
ptheta[0] = -(pi/2.0-alpha-beta);
ptheta[1] = acos((L1* L1+L2* L2-lp)/(2* L1 *L2))-pi;
ptheta[2] = -ptheta[0] -ptheta[1]- pi/2;
}

caculate(0.14, 0.09, 0.1, theta);

ROS moveit仿真效果

参考博客
(https://blog.csdn.net/xinglucao/article/details/86534402?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522164432010116780261924975%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fblog.%2522%257D&request_id=164432010116780261924975&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_blogfirst_rank_ecpm_v1~hot_rank-1-86534402.nonecase&utm_term=%E4%B8%89%E8%BD%B4%E6%9C%BA%E6%A2%B0%E8%87%82%E6%8E%A7%E5%88%B6%E7%AE%97%E6%B3%95&spm=1018.2226.3001.4450)

修改：代码中部分 * 没有打出来

void caculate(float x, float y, float z, float *ptheta)
{
	float L1 = 0.14; //杆长
	float L2 = 0.11;
	float L3 = 0.04;
	float alpha, beta, lp, Bx, By;
	Bx=x-L3; //这里设置γ为-90
	By=y;
	lp=Bx*Bx+By*By;
	if (sqrt(lp)>=L1+L2 || sqrt(lp)<=fabs(L1-L2))
		return 1;
	alpha = atan2(By,Bx);
	beta = acos((L1*L1+lp-L2*L2)/(2*L1*sqrt(lp))); //这里使用弧度制
	ptheta[0] = -(pi/2.0-alpha-beta);
	ptheta[1] = acos((L1*L1+L2*L2-lp)/(2*L1*L2))-pi;
	ptheta[2] = -ptheta[0] -ptheta[1]- pi/2.0;
	return 0;
}

机械动学 span class token 几何学算法线性代数 stm32

有关三轴机械臂逆运动学解算（附代码）的更多相关文章

ruby - 如何在 buildr 项目中使用 Ruby 代码？ - 2
如何在buildr项目中使用Ruby？我在很多不同的项目中使用过Ruby、JRuby、Java和Clojure。我目前正在使用我的标准Ruby开发一个模拟应用程序，我想尝试使用Clojure后端(我确实喜欢功能代码)以及JRubygui和测试套件。我还可以看到在未来的不同项目中使用Scala作为后端。我想我要为我的项目尝试一下buildr(http://buildr.apache.org/)，但我注意到buildr似乎没有设置为在项目中使用JRuby代码本身!这看起来有点傻，因为该工具旨在统一通用的JVM语言并且是在ruby中构建的。除了将输出的jar包含在一个独特的、仅限ruby
ruby-on-rails - Rails 源代码 : initialize hash in a weird way? - 2
在rails源中:https://github.com/rails/rails/blob/master/activesupport/lib/active_support/lazy_load_hooks.rb可以看到以下内容@load_hooks=Hash.new{|h,k|h[k]=[]}在IRB中，它只是初始化一个空哈希。和做有什么区别@load_hooks=Hash.new 最佳答案查看rubydocumentationforHashnew→new_hashclicktotogglesourcenew(obj)→new_has
ruby-on-rails - 浏览 Ruby 源代码 - 2
我的主要目标是能够完全理解我正在使用的库/gem。我尝试在Github上从头到尾阅读源代码，但这真的很难。我认为更有趣、更温和的踏脚石就是在使用时阅读每个库/gem方法的源代码。例如，我想知道RubyonRails中的redirect_to方法是如何工作的:如何查找redirect_to方法的源代码？我知道在pry中我可以执行类似show-methodmethod的操作，但我如何才能对Rails框架中的方法执行此操作？您对我如何更好地理解Gem及其API有什么建议吗？仅仅阅读源代码似乎真的很难，尤其是对于框架。谢谢! 最佳答案 Ru
ruby - 模块嵌套代码风格偏好 - 2
我的假设是moduleAmoduleBendend和moduleA::Bend是一样的。我能够从thisblog找到解决方案,thisSOthread和andthisSOthread.为什么以及什么时候应该更喜欢紧凑语法A::B而不是另一个，因为它显然有一个缺点？我有一种直觉，它可能与性能有关，因为在更多命名空间中查找常量需要更多计算。但是我无法通过对普通类进行基准测试来验证这一点。最佳答案这两种写作方法经常被混淆。首先要说的是，据我所知，没有可衡量的性能差异。(在下面的书面示例中不断查找)最明显的区别，可能也是最著名的，是你的
ruby - 寻找通过阅读代码确定编程语言的ruby gem？ - 2
几个月前，我读了一篇关于rubygem的博客文章，它可以通过阅读代码本身来确定编程语言。对于我的生活，我不记得博客或gem的名称。谷歌搜索“ruby编程语言猜测”及其变体也无济于事。有人碰巧知道相关gem的名称吗？最佳答案是这个吗:http://github.com/chrislo/sourceclassifier/tree/master 关于ruby-寻找通过阅读代码确定编程语言的rubygem？，我们在StackOverflow上找到一个类似的问题：
ruby - Net::HTTP 获取源代码和状态 - 2
我目前正在使用以下方法获取页面的源代码:Net::HTTP.get(URI.parse(page.url))我还想获取HTTP状态，而无需发出第二个请求。有没有办法用另一种方法做到这一点？我一直在查看文档，但似乎找不到我要找的东西。最佳答案在我看来，除非您需要一些真正的低级访问或控制，否则最好使用Ruby的内置Open::URI模块:require'open-uri'io=open('http://www.example.org/')#=>#body=io.read[0,50]#=>"["200","OK"]io.base_ur
程序员如何提高代码能力？ - 2
前言作为一名程序员，自己的本质工作就是做程序开发，那么程序开发的时候最直接的体现就是代码，检验一个程序员技术水平的一个核心环节就是开发时候的代码能力。众所周知，程序开发的水平提升是一个循序渐进的过程，每一位程序员都是从“菜鸟”变成“大神”的，所以程序员在程序开发过程中的代码能力也是根据平时开发中的业务实践来积累和提升的。提高代码能力核心要素程序员要想提高自身代码能力，尤其是新晋程序员的代码能力有很大的提升空间的时候，需要针对性的去提高自己的代码能力。提高代码能力其实有几个比较关键的点，只要把握住这些方面，就能很好的、快速的提高自己的一部分代码能力。1、多去阅读开源项目，如有机会可以亲自参与开源
FOHEART H1数据手套驱动Optitrack光学动捕双手运动(Unity3D) - 2
本教程将在Unity3D中混合Optitrack与数据手套的数据流，在人体运动的基础上，添加双手手指部分的运动。双手手背的角度仍由Optitrack提供，数据手套提供双手手指的角度。 01 客户端软件分别安装MotiveBody与MotionVenus并校准人体与数据手套。MotiveBodyMotionVenus数据手套使用、校准流程参照：https://gitee.com/foheart_1/foheart-h1-data-summary.git02 数据转发打开MotiveBody软件的Streaming，开始向Unity3D广播数据；MotionVenus中设置->选项选择Unit
7个大一C语言必学的程序 / C语言经典代码大全 - 2
嗨~大家好，这里是可莉！今天给大家带来的是7个C语言的经典基础代码~那一起往下看下去把【程序一】打印100到200之间的素数#includeintmain(){ inti; for(i=100;i 【程序二】输出乘法口诀表#includeintmain(){inti;for(i=1;i 【程序三】判断1000年---2000年之间的闰年#includeintmain(){intyear;for(year=1000;year 【程序四】给定两个整形变量的值，将两个值的内容进行交换。这里提供两种方法来进行交换，第一种为创建临时变量来进行交换，第二种是不创建临时变量而直接进行交换。1.创建临时变量来
git使用常见问题（提交代码，合并冲突） - 2
文章目录git常用命令（简介，详细参数往下看）Git提交代码步骤gitpullgitstatusgitaddgitcommitgitpushgit代码冲突合并问题方法一：放弃本地代码方法二：合并代码常用命令以及详细参数gitadd将文件添加到仓库：gitdiff比较文件异同gitlog查看历史记录gitreset代码回滚版本库相关操作远程仓库相关操作分支相关操作创建分支查看分支：gitbranch合并分支：gitmerge删除分支：gitbranch-ddev查看分支合并图：gitlog–graph–pretty=oneline–abbrev-commit撤消某次提交git用户名密码相关配置g

三轴机械臂逆运动学解算（附代码）