为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

Marc Pony

已于 2022-03-05 11:46:54 修改

阅读量9.5k

点赞数 32

分类专栏：机器人学文章标签： atan2 机器人逆解atan2 atan2解释双变量反正切函数

于 2020-03-01 17:06:09 首次发布

本文链接：https://blog.csdn.net/maple_2014/article/details/104594261

版权

机器人学专栏收录该内容

4 篇文章 83 订阅

订阅专栏

文章目录

一、采用 $a t a n 2 (y, x)$ 的三点优势
二、采用 $a t a n 2 (y, x)$ 的两个坑
三、参考文献/资料

一、采用 $a t a n 2 (y, x)$ 的三点优势

机器人运动学逆解最好采用双变量反正切函数 $a t a n 2$ 而不用反正/余弦函数 $a s i n$ 和 $a c o s$ 的原因主要有：
$1$ .反正弦函数 $a s i n (x)$ 的值域为 $[-\pi/2,\pi/2]$ ，反余弦函数 $a c o s (x)$ 的值域为 $[0,\pi]$ ，而双变量反正切函数 $a t a n 2 (y, x)$ 的值域为 $[-\pi,\pi]$ 。机器人关节角度范围一般在 $[-\pi,\pi]$ 之间，采用 $a t a n 2 (y, x)$ 更加方便、直接，避免了额外的角度范围判断。
$2$ . $a t a n 2 (y, x)$ 相对于 $a s i n (x)$ 或 $a c o s (x)$ ，对输入变量 $x$ 、 $y$ 具有更好的容错性。这里的容错性，主要是指由于计算精度的影响， $x$ 的实际计算值有可能稍微大于1或小于-1，这时 $a s i n (x)$ 或 $a c o s (x)$ 的值是未定义的，而 $a t a n 2 (y, x)$ 可以得到正确的结果。当然，我们也可以通过判断 $x$ 的值在计算误差范围内是否可以认为是-1或1，强制赋值为-1或1，从而解决这个问题。
$3$ .对于函数 $y = f (x)$ ， $x$ 的误差 $\Delta x$ 引起 $y$ 的误差为 $\Delta y\approx f'(x)\Delta x$ 。
若 $f (x) = a s i n (x)$ ，当 $x\in(-1,1)$ 时， $f'(x)=\frac{1}{\sqrt{1-x^2}}\in[1,+\infty)$
若 $f (x) = a c o s (x)$ ，当 $x\in(-1,1)$ 时， $f'(x)=\frac{-1}{\sqrt{1-x^2}}\in(-\infty,-1]$
若 $f (x) = a t a n (x)$ ，当 $x\in(-\infty,+\infty)$ 时， $f'(x)=\frac{1}{x^2+1}\in(0,1]$
由于实际机器人的臂长、零点、减速比等运动学参数有误差，使用 $a s i n (x)$ 或 $a c o s (x)$ 误差会放大，从保证逆解精度均匀性来看，在求解机器人运动学逆解时宜采用 $a t a n 2 (y, x)$ 。
atan2(y,x)

二、采用 $a t a n 2 (y, x)$ 的两个坑

1.当 $x = 0$ 且 $y = 0$ 时，无定义( 在matlab、lua、vs上测试过，atan2(0,0)=0,其他编译环境可能结果不一样 )。在编写程序时，如果 $x$ 与 $y$ 有同时为0，则一定要做异常处理。

  if (fabs(x - 0.0) < 1.0e-4 && fabs(y - 0.0) < 1.0e-4)
  {
    //异常处理
  }

通常，当六轴机器人手腕中心在基坐标系的z轴上时， $x = 0$ 且 $y = 0$ ，此时机器人处于肩部奇异位置，关节1有无穷多个解。
2.通过观察 $a t a n 2$ 的分段表达式，可以知道：当x与y的取值变化时， $a t a n 2$ 可能会切换表达式，发生跳变，所有的跳变情况都和x或y是否“过零”有关。虽然上述第一点做了异常处理，但是在连续插补运动中，两个插补点可能跳过0值，例如自-0.1跳至0.1，躲过判断条件if (fabs(x - 0.0) < 1.0e-4 && fabs(y - 0.0) < 1.0e-4)。
下面把所有x和y的取值列出，分9种情况：
$(1) x < 0 且 y < 0$
$(2) x < 0 且 y = 0$
$(3) x < 0 且 y > 0$
$(4) x = 0 且 y < 0$
$(5) x = 0 且 y = 0$
$(6) x = 0 且 y > 0$
$(7) x > 0 且 y < 0$
$(8) x > 0 且 y = 0$
$(9) x > 0 且 y > 0$
在0附近的两个点，在以上任意两种情况切换，共有 $C_9^2=36$ 种情况。
计算36种情况的 $a t a n 2 (y, x)$ 值matlab代码：

clc
clear
delta = 0.01;
xy = [-delta, -delta    %x < 0 and y < 0
    -delta, 0           %x < 0 and y = 0
    -delta, delta       %x < 0 and y > 0
    0, -delta           %x = 0 and y < 0
    0, 0                %x = 0 and y = 0
    0, delta            %x = 0 and y > 0
    delta, -delta       %x > 0 and y < 0
    delta, 0            %x > 0 and y = 0
    delta, delta        %x > 0 and y > 0
    ];

theta = zeros(nchoosek(9, 2), 2);
n = 1;
for i = 1 : 9
    for j = i + 1 : 9
        theta(n, 1) = atan2(xy(i, 2), xy(i, 1)) * 180 / pi;
        theta(n, 2) = atan2(xy(j, 2), xy(j, 1)) * 180 / pi;
        n = n + 1;
    end
end
theta

在这里插入图片描述
可以得到：36种情况中，只有 $x = 0 且 y = 0$ 切换到 $x > 0 且 y = 0$ 这种情况，不会导致跳变(由于 $x = 0 且 y = 0$ 无定义，所以，可以认为36种情况都会跳变)。因此，在连续插补运动中，若逆解采样 $a t a n 2 (y, x)$ 函数，则连续两次插补的关节角度，一定要判断两次关节角度是否跳变(也即关节速度是否超限)。