文章目录

一、推导步骤
- 1、方法1
- 2、方法2
二、实例(以SCARA机器人逆解为例)

在推导机器人运动学逆解的解析解时，经常会遇到以下三角方程：
k1sin(θ)+k2cos(θ)=k3(1)k_1sin(\theta)+k_2cos(\theta)=k_3 \tag{1} k1sin(θ)+k2cos(θ)=k3(1)
求解以上三角方程的解析解对运动学逆解的推导过程至关重要。下面采用两种方法进行推导。

一、推导步骤

1、方法1

式(1)联立以下三角恒等式：
sin2(θ)+cos2(θ)=1(2)sin^2(\theta)+cos^2(\theta)=1 \tag{2} sin2(θ)+cos2(θ)=1(2)
利用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)

很容易求得两组解：
{sinθ=(k1k3∓k2k12+k22−k32)/(k12+k22)cosθ=(k2k3±k1k12+k22−k32)/(k12+k22)(3)\begin{cases} sin\theta= (k_1k_3 \mp k_2\sqrt{k_1^2 + k_2^2 - k_3^2})/(k_1^2 + k_2^2) \\ cos\theta=(k_2k_3 \pm k_1\sqrt{k_1^2 + k_2^2 - k_3^2})/(k_1^2 + k_2^2) \\ \tag 3 \end{cases} {sinθ=(k1k3∓k2k12+k22−k32)/(k12+k22)cosθ=(k2k3±k1k12+k22−k32)/(k12+k22)(3)

若k12+k22<k32k_1^2+k_2^2<k_3^2k12+k22<k32，则当前给定位姿区域不可到达；若k1=k2=0k_1=k_2=0k1=k2=0，则当前给定位姿为奇异位姿。这里，采用代数的方法便可方便判断给定位姿是否在机器人可达工作空间里，并判断该位姿是否为奇异位姿！
若k12+k22≥k32k_1^2+k_2^2\ge k_3^2k12+k22≥k32，由于k12+k22>0k_1^2+k_2^2>0k12+k22>0，易得：
θ=atan2(k1k3∓k2k12+k22−k32,k2k3±k1k12+k22−k32)(4)\theta=atan2(k_1k_3 \mp k_2\sqrt{k_1^2 + k_2^2 - k_3^2},k_2k_3 \pm k_1\sqrt{k_1^2 + k_2^2 - k_3^2}) \tag{4} θ=atan2(k1k3∓k2k12+k22−k32,k2k3±k1k12+k22−k32)(4)
这里采用双变量反正切函数atan2atan2atan2，有几个优点，详见博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

2、方法2

令tan(θ/2)=utan(\theta/2)=utan(θ/2)=u，将三角恒等式cosθ=(1−u2)/(1+u2),sinθ=2u/(1+u2)cos\theta=(1-u^2)/(1+u^2),sin\theta=2u/(1+u^2)cosθ=(1−u2)/(1+u2),sinθ=2u/(1+u2)代入式(1)，整理得：
(k2+k3)u2−2k1u+k3−k2=0(5)(k_2+k_3)u^2-2k_1u+k_3-k_2=0 \tag{5} (k2+k3)u2−2k1u+k3−k2=0(5)
当k2+k3=0k_2+k_3=0k2+k3=0时，θ=±π\theta=\pm \piθ=±π。
当k2+k3≠0k_2+k_3\ne0k2+k3=0时，若k12+k22<k32k_1^2+k_2^2<k_3^2k12+k22<k32，则当前给定位姿区域不可到达；
若k12+k22≥k32k_1^2+k_2^2\ge k_3^2k12+k22≥k32，求解式(5)关于uuu的一元二次方程的两实根，得到三角方程(1)的解：
θ=2atan((k1±k12+k22−k32)/(k2+k3))(6)\theta= 2atan((k_1 \pm \sqrt{k_1^2+k_2^2-k_3^2})/(k_2+k_3))\tag{6} θ=2atan((k1±k12+k22−k32)/(k2+k3))(6)

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

在博文：scara机器人运动学正逆解中，采用几何法推导得到机器人的运动学逆解，本文采用代数法来求运动学逆解。
SCARA机器人的运动学正解为：
{x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)(7)\left \{ \begin{array}{c} x=L_1cos\theta_1+L_2cos(\theta_1+\theta_2) \\ \tag 7 y=L_1sin\theta_1+L_2sin(\theta_1+\theta_2) \end{array}\right. {x=L1cosθ1+L2cos(θ1+θ2)y=L1sinθ1+L2sin(θ1+θ2)(7)
式(7)可消去sin(θ1+θ2),cos(θ1+θ2)sin(\theta_1+\theta_2),cos(\theta_1+\theta_2)sin(θ1+θ2),cos(θ1+θ2)，转化为关于θ1\theta_1θ1的三角方程：
2yL1sinθ1+2xL1cosθ1=x2+y2+L12−L22(8)2yL_1sin\theta_1+2xL_1cos\theta_1=x^2+y^2+ L_1^2-L_2^2\tag{8} 2yL1sinθ1+2xL1cosθ1=x2+y2+L12−L22(8)
令k1=2yL1,k2=2xL1,k3=x2+y2+L12−L22k_1=2yL_1,k_2=2xL_1,k_3=x^2+y^2+ L_1^2-L_2^2k1=2yL1,k2=2xL1,k3=x2+y2+L12−L22,可解得：
θ1=atan2(k1k3∓k2k12+k22−k32,k2k3±k1k12+k22−k32)(9)\theta_1=atan2(k_1k_3 \mp k_2\sqrt{k_1^2 + k_2^2 - k_3^2},k_2k_3 \pm k_1\sqrt{k_1^2 + k_2^2 - k_3^2}) \tag{9} θ1=atan2(k1k3∓k2k12+k22−k32,k2k3±k1k12+k22−k32)(9)
由式(7)得：
{sin(θ1+θ2)=(y−L1sinθ1)/L2cos(θ1+θ2)=(x−L1cosθ1)/L2(10)\left \{ \begin{array}{c} sin(\theta_1+\theta_2)=(y-L_1sin\theta_1)/L_2 \\ \tag {10} cos(\theta_1+\theta_2)=(x-L_1cos\theta_1)/L_2 \end{array}\right. {sin(θ1+θ2)=(y−L1sinθ1)/L2cos(θ1+θ2)=(x−L1cosθ1)/L2(10)
解得：
θ2=atan2(y−L1sinθ1,x−L1cosθ1)−θ1(11)\theta_2=atan2(y-L_1sin\theta_1,x-L_1cos\theta_1)-\theta_1\tag{11} θ2=atan2(y−L1sinθ1,x−L1cosθ1)−θ1(11)
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 < -epsdisp('区域不可到达');return;
elseif temp < epsdisp('奇异点');
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 : 2if theta2(i) > pitheta2(i) = theta2(i) - 2.0 * pi;endif theta2(i) < -pitheta2(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))])

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

为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？
文章目录一.采用atan2(y,x)atan2(y,x)atan2(y,x)的三点优势二.采用atan2(y,x)atan2(y,x)atan2(y,x)的两个坑三.参考文献/资料一.采用at ...
实例10：四足机器人运动学逆解可视化与实践
实例10: 四足机器人运动学逆解单腿可视化实验目的了解逆运动学的有无解.有无多解情况. 了解运动学逆解的求解. 熟悉逆运动学中求解的几何法和代数法. 熟悉单腿舵机的简单校准. 掌握可视化逆向运动学 ...
斯坦福四足机器人运动学逆解
为什么要运动学逆解运动学逆解即根据工具坐标系相对于基坐标系的目标位姿,求解机器人各关节角. 逆运动学在机器人学中占有非常重要的地位,是机器人轨迹规划和运动控制的基础,直接影响着控制的快速性与准确性. ...
【Stewart并联机器人运动学逆解可视化仿真】
Stewart并联机器人可视化仿真 1. 坐标系建立(SPS构型) 在已知六自由度 Stewart 平台结构参数以及动平台位置的情况下,求其所有连杆伸缩长度就是 Stewart 平台的位置反解问题.提 ...
6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
斯坦福四足机器人运动学逆解(笔记4/作业4)
一.运动学正/逆解概念 1.运动学正解:已知舵机/电机转角,求足端坐标. 2.运动学逆解:已知足端坐标,求舵机/电机转角. 二.足端轨迹规划摆线方程: { x=r*(t-sint) y=r*(1-c ...
【个人简记】基于MDH，6R机器人运动学逆解
[个人理解,有错请指出,谢谢] 求解方法为解析法(满足pieper原则) 以下是我做找逆解公式的时候,matlab的测试代码 clear;%清楚空间(base)变量:clear all清除全局(g ...
(6.1)MATLAB机器人正、逆解中姿态求解的欧拉角的说明
1.欧拉角的类型: 例程中MATLAB机器人工具箱中采用的是世界坐标系的欧拉角(先旋转的在右边).如下: 利用机器人工具箱的.teach()函数进行可是化时的欧拉角如下: 2.正解算法中的欧拉角: 在 ...
四足机器人（二）---运动学逆解和步态规划
四足机器人(二)---运动学逆解和步态规划运动学逆解步态规划 MATLAB仿真运动学逆解其实运动学分为运动学正解和运动学逆解,二者有什么区别呢?因为在四足机器人中用的是12个舵机,所以运动学正 ...

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