机器人基础－－雅克比矩阵
1.雅克比矩阵的理论部分
2.机器人逆运动学
材料来源于：https://www.cnblogs.com/21207-iHome/p/5948659.html
引用请备注上材料的来源者：https://www.cnblogs.com/21207-iHome/p/5948659.html
2.1机器人运动学公式
参考挖掘机VOLVO walker excavator：
机器人末端有４个自由度x,y,z和shovel的翻斗, P x , P y , P z , θ s h o v e l P_x,P_y,P_z,\theta_{shovel} Px,Py,Pz,θshovel；
关节有５个自由度 ψ , θ 1 , θ 2 , x t e l e , θ 3 \psi,\theta_1,\theta_2,x_{tele},\theta_3 ψ,θ1,θ2,xtele,θ3；
[ P x P y P z θ s h o v e l ] = [ f 1 ( ψ , θ 1 , θ 2 , x t e l e , θ 3 ) f 2 ( ψ , θ 1 , θ 2 , x t e l e , θ 3 ) f 3 ( ψ , θ 1 , θ 2 , x t e l e , θ 3 ) f 4 ( ψ , θ 1 , θ 2 , x t e l e , θ 3 ) ] \left[\begin{matrix}P_x\\P_y\\P_z\\ \theta_{shovel} \end{matrix} \right]= \left[\begin{matrix}f_1(\psi,\theta_1,\theta_2,x_{tele},\theta_3) \\ f_2(\psi,\theta_1,\theta_2,x_{tele},\theta_3) \\ f_3(\psi,\theta_1,\theta_2,x_{tele},\theta_3) \\ f_4(\psi,\theta_1,\theta_2,x_{tele},\theta_3) \end{matrix}\right] ⎣⎢⎢⎡PxPyPzθshovel⎦⎥⎥⎤=⎣⎢⎢⎡f1(ψ,θ1,θ2,xtele,θ3)f2(ψ,θ1,θ2,xtele,θ3)f3(ψ,θ1,θ2,xtele,θ3)f4(ψ,θ1,θ2,xtele,θ3)⎦⎥⎥⎤
对上述公式进行微分：
[ δ P x δ P y δ P z δ θ s h o v e l ] = [ δ f 1 ψ ψ + δ f 1 θ 1 θ 1 + δ f 1 θ 2 θ 2 + δ f 1 x t e l e x t e l e + δ f 1 θ 3 θ 3 δ f 2 ψ ψ + δ f 2 θ 1 θ 1 + δ f 2 θ 2 θ 2 + δ f 2 x t e l e x t e l e + δ f 2 θ 3 θ 3 δ f 3 ψ ψ + δ f 3 θ 1 θ 1 + δ f 3 θ 2 θ 2 + δ f ３ x t e l e x t e l e + δ f ３ θ 3 θ 3 δ f ４ ψ ψ + δ f ４ θ 1 θ 1 + δ f ４ θ 2 θ 2 + δ f ４ x t e l e x t e l e + δ f ４ θ 3 θ 3 ] \left[\begin{matrix}\delta P_x\\ \delta P_y\\ \delta P_z\\ \delta \theta_{shovel} \end{matrix} \right]= \left[\begin{matrix} \delta \frac{f_1}{\psi}\psi+\delta \frac{f_1}{\theta_1}\theta_1 + \delta \frac{f_1}{\theta_2}\theta_2 + \delta \frac{f_1}{x_{tele}}x_{tele} + \delta \frac{f_1}{\theta_3}\theta_3\\ \delta \frac{f_2}{\psi}\psi+\delta \frac{f_2}{\theta_1}\theta_1 + \delta \frac{f_2}{\theta_2}\theta_2 + \delta \frac{f_2}{x_{tele}}x_{tele} + \delta \frac{f_2}{\theta_3}\theta_3 \\ \delta \frac{f_3}{\psi}\psi+\delta \frac{f_3}{\theta_1}\theta_1 + \delta \frac{f_3}{\theta_2}\theta_2 + \delta \frac{f_３}{x_{tele}}x_{tele} + \delta \frac{f_３}{\theta_3}\theta_3 \\ \delta \frac{f_４}{\psi}\psi+\delta \frac{f_４}{\theta_1}\theta_1 + \delta \frac{f_４}{\theta_2}\theta_2 + \delta \frac{f_４}{x_{tele}}x_{tele} + \delta \frac{f_４}{\theta_3}\theta_3 \end{matrix}\right] ⎣⎢⎢⎡δPxδPyδPzδθshovel⎦⎥⎥⎤=⎣⎢⎢⎢⎡δψf1ψ+δθ1f1θ1+δθ2f1θ2+δxtelef1xtele+δθ3f1θ3δψf2ψ+δθ1f2θ1+δθ2f2θ2+δxtelef2xtele+δθ3f2θ3δψf3ψ+δθ1f3θ1+δθ2f3θ2+δxtelef３xtele+δθ3f３θ3δψf４ψ+δθ1f４θ1+δθ2f４θ2+δxtelef４xtele+δθ3f４θ3⎦⎥⎥⎥⎤
通用化表示为：
Y ˙ = J ( X ) X ˙ \dot{Y}=J(X)\dot{X} Y˙=J(X)X˙
J(X)是时变转换矩阵,这个公式描述关节（actuator，自变量）变化对系统（末端，因变量）的影响
2.2雅克比矩阵的求解
公式比较复杂，如果展开计算，非常要命，尤其是在５，６个自由度的工业机器人上。
雅克比矩阵可以表达成：
Y ˙ = [ J 1 J 2 J 3 J 4 J 5 ] X ˙ \dot{Y}=\left[\begin{matrix}J_1 & J_2 & J_3 & J_4 & J_5 \end{matrix}\right]\dot{X} Y˙=[J1J2J3J4J5]X˙
J i J_i Ji是驱动关节i运动对末端的影响
e.g. 一个二自由度的平面连杆机器人
分析：末端只有x,y两个自由度，如果关节L1角度发生变化，那么末端的位置变化为：
Y ˙ = w 1 ∗ ( s e → − s 1 → ) \dot{Y} = w_1* (\stackrel{\rightarrow}{s_e}-\stackrel{\rightarrow}{s_1}) Y˙=w1∗(se→−s1→)
2.3 DH方程的jacobian矩阵的求解
如果是平面连杆机械臂，用上面2.2节就可以很方便的求出来。对于空间的连杆机械臂，如果是用标准DH方程建立的，jacobain矩阵为：