一、推导步骤

1、方法1

  式(1)联立以下三角恒等式：
$$si{n}^2(\theta )+co{s}^2(\theta )=1\text{\hspace{1em}(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)

  很容易求得两组解：
$$\{\begin{array}{l}sin\theta =(k_1{k}_3\mp k_2\sqrt{k_1^2+k_2^2-k_3^2})/(k_1^2+k_2^2)\\ cos\theta =(k_2{k}_3\pm k_1\sqrt{k_1^2+k_2^2-k_3^2})/(k_1^2+k_2^2)\end{array}\text{\hspace{1em}(3)}$$

  若$k_1^2+k_2^2<k_3^2$，则当前给定位姿区域不可到达；若$k_1=k_2=0$，则当前给定位姿为奇异位姿。这里，采用代数的方法便可方便判断给定位姿是否在机器人可达工作空间里，并判断该位姿是否为奇异位姿！
  若$k_1^2+k_2^2\le k_3^2$，由于$k_1^2+k_2^2>0$，易得：
$$\theta =atan2(k_1{k}_3\mp k_2\sqrt{k_1^2+k_2^2-k_3^2},k_2{k}_3\pm k_1\sqrt{k_1^2+k_2^2-k_3^2})\text{\hspace{1em}(4)}$$
  这里采用双变量反正切函数$atan2$，有几个优点，详见博文:为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

2、方法2

  令$tan(\theta /2)=u$，将三角恒等式$cos\theta =(1-u^2)/(1+u^2),sin\theta =2u/(1+u^2)$代入式(1)，整理得：
$$(k_2+k_3)u^2-2k_{1}u+k_3-k_2=0\text{\hspace{1em}(5)}$$
  当$k_2+k_3=0$时，$\theta =\pm \pi$。
  当$k_2+k_3\ne 0$时，若$k_1^2+k_2^2<k_3^2$，则当前给定位姿区域不可到达；
  若$k_1^2+k_2^2\ge k_3^2$，求解式(5)关于$u$的一元二次方程的两实根，得到三角方程(1)的解：
$$\theta =2atan((k_1\pm \sqrt{k_1^2+k_2^2-k_3^2})/(k_2+k_3))\text{\hspace{1em}(6)}$$

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

  在博文：scara机器人运动学正逆解中，采用几何法推导得到机器人的运动学逆解，本文采用代数法来求运动学逆解。
  SCARA机器人的运动学正解为：
$$\{\begin{array}{c}x=L_{1}cos{\theta }_1+L_{2}cos({\theta }_1+{\theta }_2)\\ y=L_{1}sin{\theta }_1+L_{2}sin({\theta }_1+{\theta }_2)\end{array}\text{\hspace{1em}(7)}$$
  式(5)可消去$sin({\theta }_1+{\theta }_2),cos({\theta }_1+{\theta }_2)$，转化为关于${\theta }_1$的三角方程：
$$2y{L}_{1}sin{\theta }_1+2x{L}_{1}cos{\theta }_1=x^2+y^2+L_1^2-L_2^2\text{\hspace{1em}(8)}$$
  令$k_1=2y{L}_1,k_2=2x{L}_1,k_3=x^2+y^2+L_1^2-L_2^2$,可解得：
$${\theta }_1=atan2(k_1{k}_3\mp k_2\sqrt{k_1^2+k_2^2-k_3^2},k_2{k}_3\pm k_1\sqrt{k_1^2+k_2^2-k_3^2})\text{\hspace{1em}(9)}$$
  由式(5)得：
$$\{\begin{array}{c}sin({\theta }_1+{\theta }_2)=(y-L_{1}sin{\theta }_1)/L_2\\ cos({\theta }_1+{\theta }_2)=(x-L_{1}cos{\theta }_1)/L_2\end{array}\text{\hspace{1em}(10)}$$
  解得：
$${\theta }_2=atan2(y-L_{1}sin{\theta }_1,x-L_{1}cos{\theta }_1)-{\theta }_1\text{\hspace{1em}(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 < -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))])