機器人逆運動學公式推導中常用的方程求解

求方程： s i n ( θ ) = a sin(\theta)=a sin(θ)=a

則有， c o s ( θ ) = ± 1 − a 2 cos(\theta)=\pm \sqrt{1-a^{2}} cos(θ)=±1−a2

​

故， θ = a t a n 2 ( s i n ( θ ) , c o s ( θ ) ) = a t a n 2 ( a , ± 1 − a 2 ) \theta=atan2(sin(\theta), cos(\theta))=atan2(a, \pm \sqrt{1-a^{2}}) θ=atan2(sin(θ),cos(θ))=atan2(a,±1−a2

​)

求方程： c o s ( θ ) = b cos(\theta)=b cos(θ)=b

則有， s i n ( θ ) = ± 1 − b 2 sin(\theta)=\pm \sqrt{1-b^{2}} sin(θ)=±1−b2

​

故， θ = a t a n 2 ( s i n ( θ ) , c o s ( θ ) ) = a t a n 2 ( ± 1 − b 2 , b ) \theta=atan2(sin(\theta), cos(\theta))=atan2(\pm \sqrt{1-b^{2}}, b) θ=atan2(sin(θ),cos(θ))=atan2(±1−b2

​,b)

求方程： a ⋅ c o s ( θ ) + b ⋅ s i n ( θ ) = 0 a \cdot cos(\theta)+b \cdot sin(\theta)=0 a⋅cos(θ)+b⋅sin(θ)=0

令： a = a 2 + b 2 s i n ( α ) ⇒ s i n ( α ) = a a 2 + b 2 , b = a 2 + b 2 c o s ( α ) ⇒ c o s ( α ) = b a 2 + b 2 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}}} a=a2+b2

​sin(α)⇒sin(α)=a2+b2

​a​,b=a2+b2

​cos(α)⇒cos(α)=a2+b2

​b​

則有， a 2 + b 2 s i n ( α ) c o s ( θ ) + a 2 + b 2 c o s ( α ) s i n ( θ ) = 0 \sqrt{a^{2}+b^{2}}sin(\alpha)cos(\theta)+\sqrt{a^{2}+b^{2}}cos(\alpha)sin(\theta)=0 a2+b2

​sin(α)cos(θ)+a2+b2

​cos(α)sin(θ)=0

即， a 2 + b 2 s i n ( α + θ ) = 0 ⇒ s i n ( α + θ ) = 0 a 2 + b 2 = 0 \sqrt{a^{2}+b^{2}}sin(\alpha+\theta)=0 \Rightarrow sin(\alpha+\theta)=\frac{0}{\sqrt{a^{2}+b^{2}}}=0 a2+b2

​sin(α+θ)=0⇒sin(α+θ)=a2+b2

​0​=0

那麼， c o s ( α + θ ) = ± 1 − 0 = ± 1 cos(\alpha + \theta)=\pm \sqrt{1-0}=\pm 1 cos(α+θ)=±1−0

​=±1

故， θ = a t a n 2 ( s i n ( α + θ ) , c o s ( α + θ ) ) − a t a n 2 ( s i n ( α ) , c o s ( α ) ) = 0 − a t a n 2 ( a , b ) = − a t a n 2 ( a , b ) \theta=atan2(sin(\alpha+\theta), cos(\alpha+\theta))-atan2(sin(\alpha), cos(\alpha))=0-atan2(a, b)=-atan2(a, b) θ=atan2(sin(α+θ),cos(α+θ))−atan2(sin(α),cos(α))=0−atan2(a,b)=−atan2(a,b)

求方程： a ⋅ c o s ( θ ) + b ⋅ s i n ( θ ) = c a \cdot cos(\theta) + b \cdot sin(\theta) = c a⋅cos(θ)+b⋅sin(θ)=c

令： a = a 2 + b 2 s i n ( α ) ⇒ s i n ( α ) = a a 2 + b 2 , b = a 2 + b 2 c o s ( α ) ⇒ c o s ( α ) = b a 2 + b 2 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}}} a=a2+b2

​sin(α)⇒sin(α)=a2+b2

​a​,b=a2+b2

​cos(α)⇒cos(α)=a2+b2

​b​

則有， a 2 + b 2 s i n ( α ) c o s ( θ ) + a 2 + b 2 c o s ( α ) s i n ( θ ) = c \sqrt{a^{2}+b^{2}}sin(\alpha)cos(\theta)+\sqrt{a^{2}+b^{2}}cos(\alpha)sin(\theta)=c a2+b2

​sin(α)cos(θ)+a2+b2

​cos(α)sin(θ)=c

即， a 2 + b 2 s i n ( α + θ ) = c ⇒ s i n ( α + θ ) = c a 2 + b 2 \sqrt{a^{2}+b^{2}}sin(\alpha+\theta)=c \Rightarrow sin(\alpha+\theta)=\frac{c}{\sqrt{a^{2}+b^{2}}} a2+b2

​sin(α+θ)=c⇒sin(α+θ)=a2+b2

​c​

那麼， c o s ( α + θ ) = ± a 2 + b 2 − c 2 a 2 + b 2 cos(\alpha+\theta)=\pm \frac{\sqrt{a^{2}+b^{2}-c^{2}}}{\sqrt{a^{2}+b^{2}}} cos(α+θ)=±a2+b2

​a2+b2−c2

​​

故， θ = a t a n 2 ( s i n ( α + θ ) , c o s ( α + θ ) ) − a t a n 2 ( s i n ( α ) , c o s ( α ) ) = a t a n 2 ( c , ± a 2 + b 2 − c 2 ) − a t a n 2 ( a , b ) \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) θ=atan2(sin(α+θ),cos(α+θ))−atan2(sin(α),cos(α))=atan2(c,±a2+b2−c2

​)−atan2(a,b)