机器人运动学逆解中最常用的三角方程(附代码)

2024-06-13 04:49| 来源: 网络整理| 查看: 265

Marc Pony 分类：机器人学发布时间 2021.06.21阅读数 6386 评论数 1 文章目录一、推导步骤 1、方法1 2、方法2 二、实例(以SCARA机器人逆解为例)

在推导机器人运动学逆解的解析解时，经常会遇到以下三角方程：

求解以上三角方程的解析解对运动学逆解的推导过程至关重要。下面采用两种方法进行推导。

一、推导步骤 1、方法1

式(1)联立以下三角恒等式：

利用MATLAB解符号方程组(1)(2)，代码如下：

clc; clear; syms k1 k2 k3 theta sinTheta cosTheta real result = solve(k1 * sinTheta + k2 * cosTheta == k3, sinTheta^2 + cosTheta^2 == 1, [sinTheta, cosTheta]); sinTheta = simplify(result.sinTheta) cosTheta = simplify(result.cosTheta)

很容易求得两组解：

若，则当前给定位姿区域不可到达，若则当前给定位姿为奇异位姿。这里，采用代数的方法便可方便判断给定位姿是否在机器人可达工作空间里，并判断该位姿是否为奇异位姿！若，由于，易得：

这里采用双变量反正切函数atan2，有几个优点，详见博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

2、方法2

二、实例(以SCARA机器人逆解为例)

在博文：scara机器人运动学正逆解中，采用几何法推导得到机器人的运动学逆解，本文采用代数法来求运动学逆解。 SCARA机器人的运动学正解为：

MATLAB代码：

clc; clear; L1 = 200.0; %mm L2 = 300.0; %mm theta1 = 1.0; %rad theta2 = 3.0; %rad x = L1 * cos(theta1) + L2 * cos(theta1 + theta2); y = L1 * sin(theta1) + L2 * sin(theta1 + theta2); disp(['输入角度(rad):',num2str(theta1), ',', num2str(theta2)]) disp(['输入x，y(mm):',num2str(x), ',', num2str(y)]) k1 = 2.0 * y * L1; k2 = 2.0 * x * L1; k3 = x^2 + y^2 + L1^2 - L2^2; temp = k1^2 + k2^2 - k3^2; if temp < -eps disp('区域不可到达'); return; elseif temp < eps disp('奇异点'); else %do nothing end theta1 = zeros(2, 1); theta1(1) = atan2(k1 * k3 - k2 * sqrt(temp), k2 * k3 + k1 * sqrt(temp)); theta1(2) = atan2(k1 * k3 + k2 * sqrt(temp), k2 * k3 - k1 * sqrt(temp)); s1 = sin(theta1); c1 = cos(theta1); theta2 = atan2(y - L1 * s1, x - L1 * c1) - theta1; for i = 1 : 2 if theta2(i) > pi theta2(i) = theta2(i) - 2.0 * pi; end if theta2(i) < -pi theta2(i) = theta2(i) + 2.0 * pi; end end xx = L1 * cos(theta1) + L2 * cos(theta1 + theta2); yy = L1 * sin(theta1) + L2 * sin(theta1 + theta2); disp(['输出角度(rad):',num2str(theta1(1)), ',', num2str(theta2(1)), '; ', num2str(theta1(2)), ',', num2str(theta2(2))]) disp(['输出x，y(mm):',num2str(xx(1)), ',', num2str(yy(1)), '; ', num2str(xx(2)), ',', num2str(yy(2))]) 机器人学运动学建模仿真三角方程

打赏 0

微信微博 QQ 图片上一篇：为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？下一篇：一文带你完全掌握机器人DH参数建模(详细步骤+实例+代码)

【本文地址】

公司简介

联系我们