机器人逆运动学公式推导中常用的方程求解

求方程：$sin(\theta)=a$

则有，$cos(\theta)=\pm \sqrt{1-a^{2}}$

故，$\theta=atan2(sin(\theta), cos(\theta))=atan2(a, \pm \sqrt{1-a^{2}})$

求方程：$cos(\theta)=b$

则有，$sin(\theta)=\pm \sqrt{1-b^{2}}$

故，$\theta=atan2(sin(\theta), cos(\theta))=atan2(\pm \sqrt{1-b^{2}}, b)$

求方程：$a \cdot cos(\theta)+b \cdot sin(\theta)=0$

令：$a=\sqrt{a^{2}+b^{2}}sin(\alpha) \Rightarrow sin(\alpha)=\frac{a}{\sqrt{a^{2}+b^{2}}}, b=\sqrt{a^{2}+b^{2}}cos(\alpha) \Rightarrow cos(\alpha)=\frac{b}{\sqrt{a^{2}+b^{2}}}$

则有，$\sqrt{a^{2}+b^{2}}sin(\alpha)cos(\theta)+\sqrt{a^{2}+b^{2}}cos(\alpha)sin(\theta)=0$

即，$\sqrt{a^{2}+b^{2}}sin(\alpha+\theta)=0 \Rightarrow sin(\alpha+\theta)=\frac{0}{\sqrt{a^{2}+b^{2}}}=0$

那么，$cos(\alpha + \theta)=\pm \sqrt{1-0}=\pm 1$

故，$\theta=atan2(sin(\alpha+\theta), cos(\alpha+\theta))-atan2(sin(\alpha), cos(\alpha))=0-atan2(a, b)=-atan2(a, b)$

求方程：$a \cdot cos(\theta) + b \cdot sin(\theta) = c$

令：$a=\sqrt{a^{2}+b^{2}}sin(\alpha) \Rightarrow sin(\alpha)=\frac{a}{\sqrt{a^{2}+b^{2}}}, b=\sqrt{a^{2}+b^{2}}cos(\alpha) \Rightarrow cos(\alpha)=\frac{b}{\sqrt{a^{2}+b^{2}}}$

则有，$\sqrt{a^{2}+b^{2}}sin(\alpha)cos(\theta)+\sqrt{a^{2}+b^{2}}cos(\alpha)sin(\theta)=c$

即，$\sqrt{a^{2}+b^{2}}sin(\alpha+\theta)=c \Rightarrow sin(\alpha+\theta)=\frac{c}{\sqrt{a^{2}+b^{2}}}$

那么，$cos(\alpha+\theta)=\pm \frac{\sqrt{a^{2}+b^{2}-c^{2}}}{\sqrt{a^{2}+b^{2}}}$

故，$\theta= atan2(sin(\alpha+\theta), cos(\alpha+\theta)) - atan2(sin(\alpha), cos(\alpha))= atan2(c, \pm \sqrt{a^{2}+b^{2}-c^{2}}) - atan2(a, b)$