【基于机械臂触觉伺服的物体操控研究】UR5e动力学建模及代码实现

动力学建模及代码实现"/>

【基于机械臂触觉伺服的物体操控研究】UR5e动力学建模及代码实现

我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。

前言：UR系列是优傲公司的代表产品，也是目前比较通用的产品级机械臂。所以我打算用该机械臂进行毕设的仿真实现。关于其动力学建模，网上有很多参考的文献，但由于六自由度的完整动力学模型很复杂，验证和移植都比较麻烦，所以我参考了以下的论文，对动力学模型进行了简化，并且在仿真环境中进行了验证。
基于UR5机械臂的轨迹跟踪控制算法研究

1. 模型简化

UR5e一共有6个驱动关节，其中前三个关节主要用于控制末端的位置，后三个关节主要用于控制末端的姿态。
因为后三个连杆较短，并且质量较小，动力学参数值较小，因此将其简化为质点，位置在第三个驱动关节对应连杆的末端。
对于前三个关节，认为其质量沿杆均匀分布，并将其简化为两个模型，一个是只有关节1的单连杆模型，一个是有关节2和关节3的双连杆模型。
注：以下模型中的 c 1 c_1 c1​表示 c o s ( θ 1 ) cos(\theta_1) cos(θ1​)， s 12 s_{12} s12​表示 s i n ( θ 1 + θ 2 ) sin(\theta_1 + \theta_2) sin(θ1​+θ2​)

关节1的单连杆模型

关节2和关节3的双连杆模型

其中， m e m_e me​为后三个连杆的质量之和。
m e = m 4 + m 5 + m 6 m_e = m_4 + m_5 + m_6 me​=m4​+m5​+m6​

2. 动力学建模

因为对于简化后的模型，连杆的动能和势能都比较容易求取，所以我采用了拉格朗日法进行动力学建模。
拉格朗日公式
L ( Θ , Θ ˙ ) = k ( Θ , Θ ˙ ) − u ( Θ ) d d t ∂ L ∂ Θ ˙ − ∂ L ∂ Θ = τ \begin{gathered} \mathcal{L}(\Theta, \dot{\Theta})=k(\Theta, \dot{\Theta})-u(\Theta) \\ \frac{\mathrm{d}}{\mathrm{d} t} \frac{\partial \mathcal{L}}{\partial \dot{\Theta}}-\frac{\partial \mathcal{L}}{\partial \Theta}=\tau \end{gathered} L(Θ,Θ˙)=k(Θ,Θ˙)−u(Θ)dtd​∂Θ˙∂L​−∂Θ∂L​=τ​
动力学模型的状态空间方程
τ = M ( Θ ) Θ ¨ + V ( Θ , Θ ˙ ) + G ( Θ ) \tau = M(\Theta) \ddot{\Theta} + V(\Theta, \dot{\Theta}) + G(\Theta) τ=M(Θ)Θ¨+V(Θ,Θ˙)+G(Θ)
M ( Θ ) M(\Theta) M(Θ)： n × n n \times n n×n质量矩阵
V ( Θ , Θ ˙ ) V(\Theta, \dot{\Theta}) V(Θ,Θ˙)： n × 1 n \times 1 n×1离心力和哥氏力矢量
G ( Θ ) G(\Theta) G(Θ)： n × 1 n \times 1 n×1阶重力矢量

动力学模型的位形空间方程
τ = ( Θ ) Θ ¨ + B ( Θ ) ( Θ ˙ Θ ˙ ) + C ( Θ ) ( Θ ˙ 2 ) + G ( Θ ) \tau = (\Theta)\ddot{\Theta}+B(\Theta)(\dot{\Theta}\dot{\Theta})+ C(\Theta) (\dot{\Theta}^2)+G(\Theta) τ=(Θ)Θ¨+B(Θ)(Θ˙Θ˙)+C(Θ)(Θ˙2)+G(Θ)
B ( Θ ) B(\Theta) B(Θ)： n × n ( n − 1 ) / 2 n \times n(n-1)/2 n×n(n−1)/2阶哥氏力系数矩阵
C ( Θ ) C(\Theta) C(Θ)： n × n n \times n n×n阶离心力系数矩阵
( Θ ˙ Θ ˙ ) (\dot{\Theta}\dot{\Theta}) (Θ˙Θ˙)： n ( n − 1 ) / 2 × 1 n(n-1)/2 \times 1 n(n−1)/2×1阶关节速度积矢量
( Θ ˙ 2 ) (\dot{\Theta}^2) (Θ˙2)： n × 1 n \times 1 n×1阶关节速度平方矢量

单连杆模型

参数定义
l 2 c 2 = l 2 ′ l 3 c 23 = l 3 ′ l_2c_2 = l_2'\\ l_3c_{23} = l_3' l2​c2​=l2′​l3​c23​=l3′​
速度微分
v 2 = r θ 1 ˙ v 3 = ( l 1 ′ + r ) θ 1 ˙ v e = ( l 1 ′ + l 2 ′ ) θ 1 ˙ v_2 = r\dot{\theta_1}\\ v_3 = (l_1'+r)\dot{\theta_1}\\ v_e = (l_1'+l_2')\dot{\theta_1} v2​=rθ1​˙​v3​=(l1′​+r)θ1​˙​ve​=(l1′​+l2′​)θ1​˙​
动能求取
E 2 = 1 2 m 2 l 2 ′ ∫ 0 l 2 ′ v 2 2 d r = 1 6 m 2 l 2 2 c 2 2 θ 1 ˙ 2 E 3 = 1 2 m 3 l 3 ′ ∫ 0 l 3 ′ v 3 2 d r = 1 2 m 3 l 2 2 c 2 2 θ 1 ˙ 2 + 1 2 m 3 l 2 l 3 c 2 c 3 θ 1 ˙ 2 + 1 6 m 3 l 3 2 c 3 2 θ 1 ˙ 2 E e = 1 2 m e ( l 2 ′ + l 3 ′ ) 2 θ 1 ˙ 2 E = E 2 + E 3 + E e E_2 = \frac{1}{2}\frac{m_2}{l_2'}\int_{0}^{l_2'} v_2^2\mathrm{d}r = \frac{1}{6}m_2l_2^2c_2^2\dot{\theta_1}^2\\~\\ E_3 = \frac{1}{2}\frac{m_3}{l_3'}\int_{0}^{l_3'} v_3^2\mathrm{d}r = \frac{1}{2}m_3l_2^2c_2^2\dot{\theta_1}^2 + \frac{1}{2}m_3l_2l_3c_2c_3\dot{\theta_1}^2 + \frac{1}{6}m_3l_3^2c_3^2\dot{\theta_1}^2\\~\\ E_e = \frac{1}{2}m_e(l_2'+l_3')^2\dot{\theta_1}^2\\~\\ E = E_2 + E_3 + E_e E2​=21​l2′​m2​​∫0l2′​​v22​dr=61​m2​l22​c22​θ1​˙​2 E3​=21​l3′​m3​​∫0l3′​​v32​dr=21​m3​l22​c22​θ1​˙​2+21​m3​l2​l3​c2​c3​θ1​˙​2+61​m3​l32​c32​θ1​˙​2 Ee​=21​me​(l2′​+l3′​)2θ1​˙​2 E=E2​+E3​+Ee​
势能求取
因为关节1的旋转方向与重力方向垂直，所以不受重力影响，即
G = 0 G = 0 G=0
创建动力学模型
将上述式子代入到拉格朗日公式中，即可求得
M ( Θ ) = 1 3 m 2 l 2 ′ 2 + 1 3 m 3 l 3 ′ 2 + m 3 l 2 ′ 2 + m 3 l 2 ′ l 3 ′ V ( Θ , Θ ˙ ) = 0 G ( Θ ) = 0 τ 1 = M ( Θ ) θ 1 ¨ M(\Theta) = \frac{1}{3}m_2l_2'^2 + \frac{1}{3}m_3l_3'^2 + m_3l_2'^2 + m_3l_2'l_3'\\~\\ V(\Theta, \dot{\Theta}) = 0\\~\\ G(\Theta) = 0\\~\\ \tau_1 = M(\Theta) \ddot{\theta_1} M(Θ)=31​m2​l2′2​+31​m3​l3′2​+m3​l2′2​+m3​l2′​l3′​ V(Θ,Θ˙)=0 G(Θ)=0 τ1​=M(Θ)θ1​¨​

双连杆模型

速度微分（采用极坐标的方式）
对于连杆2
{ x = r c 2 y = r s 2 ⇒ { x ˙ = − θ 2 ˙ r s 2 y ˙ = θ 2 ˙ r c 2 v 2 2 = x 2 + y 2 = r 2 θ 2 ˙ 2 \begin{cases} x =rc_2 \\y=rs_2 \end{cases}\Rightarrow \begin{cases} \dot{x} =-\dot{\theta_2}rs_2 \\\dot{y}=\dot{\theta_2}rc_2 \end{cases}\\~\\ v_2^2 = x^2 + y^2 = r^2\dot{\theta_2}^2 {x=rc2​y=rs2​​⇒{x˙=−θ2​˙​rs2​y˙​=θ2​˙​rc2​​ v22​=x2+y2=r2θ2​˙​2
对于连杆3
{ x = l 2 c 2 + r c 23 y = l 2 s 2 + r c s 23 ⇒ { x ˙ = − θ 2 ˙ l 2 s 2 − r ( θ 2 ˙ + θ 3 ˙ ) s 23 y ˙ = θ 2 ˙ l 2 c 2 + r ( θ 2 ˙ + θ 3 ˙ ) c 23 v 3 2 = x 2 + y 2 = l 2 2 θ 2 ˙ 2 + r 2 ( θ 2 ˙ + θ 3 ˙ ) 2 + 2 l 2 r c 3 θ 2 ˙ ( θ 2 ˙ + θ 3 ˙ ) \begin{cases} x =l_2c_2 + rc_{23} \\y=l_2s_2 + rcs_{23} \end{cases}\Rightarrow \begin{cases} \dot{x} =-\dot{\theta_2}l_2s_2 - r(\dot{\theta_2}+\dot{\theta_3})s_{23} \\ \dot{y} =\dot{\theta_2}l_2c_2 + r(\dot{\theta_2}+\dot{\theta_3})c_{23} \end{cases}\\~\\ v_3^2 = x^2 + y^2 = l_2^2\dot{\theta_2}^2 + r^2(\dot{\theta_2}+\dot{\theta_3})^2 + 2l_2rc_3\dot{\theta_2}(\dot{\theta_2}+\dot{\theta_3}) {x=l2​c2​+rc23​y=l2​s2​+rcs23​​⇒{x˙=−θ2​˙​l2​s2​−r(θ2​˙​+θ3​˙​)s23​y˙​=θ2​˙​l2​c2​+r(θ2​˙​+θ3​˙​)c23​​ v32​=x2+y2=l22​θ2​˙​2+r2(θ2​˙​+θ3​˙​)2+2l2​rc3​θ2​˙​(θ2​˙​+θ3​˙​)
对于末端质点
v e 2 = l 2 2 θ 2 ˙ 2 + l 3 2 ( θ 2 ˙ + θ 3 ˙ ) 2 + 2 l 2 l 3 c 3 θ 2 ˙ ( θ 2 ˙ + θ 3 ˙ ) v_e^2 = l_2^2\dot{\theta_2}^2 + l_3^2(\dot{\theta_2}+\dot{\theta_3})^2 + 2l_2l_3c_3\dot{\theta_2}(\dot{\theta_2}+\dot{\theta_3}) ve2​=l22​θ2​˙​2+l32​(θ2​˙​+θ3​˙​)2+2l2​l3​c3​θ2​˙​(θ2​˙​+θ3​˙​)

动能求取
E 2 = 1 2 m 2 l 2 ∫ 0 l 2 v 2 2 d r E 3 = 1 2 m 3 l 3 ∫ 0 l 3 v 3 2 d r E e = 1 2 m e v e 2 E = E 2 + E 3 + E e E_2 = \frac{1}{2}\frac{m_2}{l_2}\int_{0}^{l_2} v_2^2\mathrm{d}r \\~\\ E_3 = \frac{1}{2}\frac{m_3}{l_3}\int_{0}^{l_3} v_3^2\mathrm{d}r \\~\\ E_e = \frac{1}{2}m_ev_e^2\\~\\ E = E_2 + E_3 + E_e E2​=21​l2​m2​​∫0l2​​v22​dr E3​=21​l3​m3​​∫0l3​​v32​dr Ee​=21​me​ve2​ E=E2​+E3​+Ee​
势能求取
G 2 = 1 2 m 2 g l 2 s 2 G 3 = m 3 g l 2 s 2 + 1 2 m 3 g l 3 s 23 G e = m e g l 2 s 2 + m e g l 3 s 23 G_2 = \frac{1}{2}m_2gl_2s_2\\~\\ G_3 = m_3gl_2s_2 + \frac{1}{2}m_3gl_3s_{23}\\~\\ G_e = m_egl_2s_2 + m_egl_3s_{23} G2​=21​m2​gl2​s2​ G3​=m3​gl2​s2​+21​m3​gl3​s23​ Ge​=me​gl2​s2​+me​gl3​s23​
创建动力学模型
将上述式子代入到拉格朗日公式中，即可求得
M 23 = [ m 3 l 2 2 + 1 3 m 2 l 2 2 + 1 3 m 3 l 3 2 + m 3 l 2 l 3 c 3 1 3 m 3 l 3 2 + 1 2 m 3 l 2 l 3 c 3 1 3 m 3 l 3 2 + 1 2 m 3 l 2 l 3 c 3 1 3 m 3 l 3 2 ] M e = [ m e l 2 2 + m e l 3 2 + 2 m e l 2 l 3 c 3 m e l 3 2 + m e l 2 l 3 c 3 m e l 3 2 + m e l 2 l 3 c 3 m e l 3 2 ] B 23 = [ − m 3 l 2 l 3 s 3 0 0 0 ] B e = [ − 2 m e l 2 l 3 s 3 0 0 0 ] C 23 = [ 0 − 1 2 m 3 l 2 l 3 s 3 1 2 m 3 l 2 l 3 s 3 0 ] C e = [ 0 − m e l 2 l 3 s 3 m e l 2 l 3 s 3 0 ] G 23 = [ ( 1 2 m 2 + m 3 ) g l 2 c 2 + 1 2 m 3 g l 3 c 23 1 2 m 3 g l 3 c 23 ] G e = [ m e g l 2 c 2 + m e g l 3 c 23 m e g l 3 c 23 ] M_{23} = \begin{bmatrix} m_3l_2^2+ \frac{1}{3}m_2l_2^2 + \frac{1}{3}m_3l_3^2 + m_3l_2l_3c_3& \frac{1}{3}m_3l_3^2 + \frac{1}{2}m_3l_2l_3c_3\\ \frac{1}{3}m_3l_3^2 + \frac{1}{2}m_3l_2l_3c_3& \frac{1}{3}m_3l_3^2 \end{bmatrix}\\~\\ M_e= \begin{bmatrix} m_el_2^2+ m_el_3^2 + 2m_el_2l_3c_3& m_el_3^2 + m_el_2l_3c_3\\ m_el_3^2 + m_el_2l_3c_3& m_el_3^2 \end{bmatrix}\\~\\ B_{23} = \begin{bmatrix} -m_3l_2l_3s_3& 0\\ 0& 0 \end{bmatrix}\\~\\ B_e = \begin{bmatrix} -2m_el_2l_3s_3& 0\\ 0& 0 \end{bmatrix}\\~\\ C_{23} = \begin{bmatrix} 0& -\frac{1}{2}m_3l_2l_3s_3\\ \frac{1}{2}m_3l_2l_3s_3& 0 \end{bmatrix}\\~\\ C_e = \begin{bmatrix} 0& -m_el_2l_3s_3\\ m_el_2l_3s_3& 0 \end{bmatrix}\\~\\ G_{23} = \begin{bmatrix} (\frac{1}{2}m_2 + m_3)gl_2c_2 + \frac{1}{2}m_3gl_3c_{23}\\ \frac{1}{2}m_3gl_3c_{23} \end{bmatrix}\\~\\ G_e = \begin{bmatrix} m_egl_2c_2 + m_egl_3c_{23}\\ m_egl_3c_{23}\ \end{bmatrix}\\~\\ M23​=[m3​l22​+31​m2​l22​+31​m3​l32​+m3​l2​l3​c3​31​m3​l32​+21​m3​l2​l3​c3​​31​m3​l32​+21​m3​l2​l3​c3​31​m3​l32​​] Me​=[me​l22​+me​l32​+2me​l2​l3​c3​me​l32​+me​l2​l3​c3​​me​l32​+me​l2​l3​c3​me​l32​​] B23​=[−m3​l2​l3​s3​0​00​] Be​=[−2me​l2​l3​s3​0​00​] C23​=[021​m3​l2​l3​s3​​−21​m3​l2​l3​s3​0​] Ce​=[0me​l2​l3​s3​​−me​l2​l3​s3​0​] G23​=[(21​m2​+m3​)gl2​c2​+21​m3​gl3​c23​21​m3​gl3​c23​​] Ge​=[me​gl2​c2​+me​gl3​c23​me​gl3​c23​ ​] 
合并结果
M ( Θ ) = M 23 + M e B ( Θ ) = B 23 + B e C ( Θ ) = C 23 + C e G ( Θ ) = G 23 + G e ( Θ ˙ Θ ˙ ) = θ 2 ˙ θ 3 ˙ ( Θ ˙ 2 ) = [ θ 2 ˙ 2 θ 3 ˙ 2 ] τ 23 = [ τ 2 τ 3 ] = M ( Θ ) Θ ¨ + B ( Θ ) ( Θ ˙ Θ ˙ ) + C ( Θ ) ( Θ ˙ 2 ) + G ( Θ ) M(\Theta) = M_{23} + M_e \\ B(\Theta) = B_{23} + B_e\\ C(\Theta) = C_{23} + C_e\\ G(\Theta) = G_{23} + G_e\\\\~\\ (\dot{\Theta}\dot{\Theta}) = \dot{\theta_2}\dot{\theta_3}\\\\~\\ (\dot{\Theta}^2) = \begin{bmatrix} \dot{\theta_2}^2\\ \dot{\theta_3}^2 \end{bmatrix}\\~\\ \tau_{23} = \begin{bmatrix} \tau_2\\ \tau_3 \end{bmatrix} = M(\Theta)\ddot{\Theta}+B(\Theta)(\dot{\Theta}\dot{\Theta})+C(\Theta) (\dot{\Theta}^2)+G(\Theta) M(Θ)=M23​+Me​B(Θ)=B23​+Be​C(Θ)=C23​+Ce​G(Θ)=G23​+Ge​ (Θ˙Θ˙)=θ2​˙​θ3​˙​ (Θ˙2)=[θ2​˙​2θ3​˙​2​] τ23​=[τ2​τ3​​]=M(Θ)Θ¨+B(Θ)(Θ˙Θ˙)+C(Θ)(Θ˙2)+G(Θ)
合并两个模型的结果，得到完整模型（模型矩阵也可以进行合并，这里就不给出结果了，读者可以自行计算）
τ = [ τ 1 τ 23 ] T = [ τ 1 τ 2 τ 3 ] \tau = \begin{bmatrix} \tau_1& \tau_{23} \end{bmatrix}^T = \begin{bmatrix} \tau_1\\ \tau_2\\ \tau_3 \end{bmatrix} τ=[τ1​​τ23​​]T= ​τ1​τ2​τ3​​ ​

代码实现

template <typename T>
void Dynamics(const Vec6<T>& q, const Vec6<T>& qd, const Vec6<T>& qdd, Vec6<T>* tau){/* define params */T m1 = _upperarmLinkMass;T m2 = _forearmLinkMass;T l1 = _upperarmLinkLength;T l2 = _forearmLinkLength;T t1 = M_PI_2 - q(1);T t2 = -q(2);/* ------------------------------------ double link model -------------------------------------------- */Mat2<T> M;M(0,0) = m2*pow(l1,2) + (1./3.)*m1*pow(l1,2) + (1./3.)*m2*pow(l2,2) + m2*l1*l2*cos(t2);M(0,1) = (1./3.)*m2*pow(l2,2) + (1./2.)*m2*l1*l2*cos(t2);M(1,0) = (1./3.)*m2*pow(l2,2) + (1./2.)*m2*l1*l2*cos(t2);M(1,1) = (1./3.)*m2*pow(l2,2);Mat2<T> A = Mat2<T>::Zero();A(0,1) = -(1./2.)*m2*l1*l2*sin(t2);A(1,0) = (1./2.)*m2*l1*l2*sin(t2);Mat2<T> B = Mat2<T>::Zero();B(0,0) = -m2*l1*l2*sin(t2);Vec2<T> G;T g = 9.81;G(0) = ((1./2.)*m1 + m2)*g*l1*cos(t1) + (1./2.)*m2*g*l2*cos(t1+t2);G(1) = (1./2.)*m2*g*l2*cos(t1+t2);/* consider tail mass */T me = *links_mass[3] + *links_mass[4] + *links_mass[5];/* adjustment coefficient */M(0,0) += me*pow(l1,2) + me*pow(l2,2) + 2*l1*l2*me*cos(t2);M(0,1) += me*pow(l2,2) + l1*l2*me*cos(t2);M(1,0) += me*pow(l2,2) + l1*l2*me*cos(t2);M(1,1) += me*pow(l2,2);A(0,1) += -me*l1*l2*sin(t2);A(1,0) += me*l1*l2*sin(t2);B(0,0) += -2*me*l1*l2*sin(t2);G(0) += me*g*l1*cos(t1) + me*g*l2*cos(t1+t2);G(1) += me*g*l2*cos(t1+t2);/* compute tau */Vec2<T> qd_2(pow(qd[1],2), pow(qd[2],2));Vec2<T> qd_mix(qd[1]*qd[2], qd[1]*qd[2]);Vec2<T> _qdd(qdd[1], qdd[2]);Vec2<T> V = A*qd_2 + B*qd_mix;Vec2<T> _tau = M*_qdd + V + G;tau->operator()(1) = -_tau(0);tau->operator()(2) = -_tau(1);/* ------------------------------------ sigle link model -------------------------------------------- */T _l1 = l1*cos(t1);T _l2 = l2*cos(t2);T A_0 = (1./3.)*m1*pow(_l1,2) + (1./3.)*m2*pow(_l2,2) + m2*pow(_l1,2) + m2*_l1*_l2;/* consider tail mass */A_0 += me*pow(_l1+_l2,2);/* compute tau */T tau_0 = A_0*qdd[0];tau->operator()(0) = tau_0;/* no control data for other three joint */tau->operator()(3) = 0;tau->operator()(4) = 0;tau->operator()(5) = 0;
}

3. 计算雅克比矩阵

雅克比矩阵同样是针对于上面简化后的模型进行求取。
运动学关系
x = ( l 2 c 2 + l 3 c 23 ) c 1 y = ( l 2 c 2 + l 3 c 23 ) s 1 z = l 2 s 2 + l 3 s 23 x = (l_2c_2 + l_3c_{23})c_1\\ y = (l_2c_2 + l_3c_{23})s_1\\ z = l_2s_2 + l_3s_{23} x=(l2​c2​+l3​c23​)c1​y=(l2​c2​+l3​c23​)s1​z=l2​s2​+l3​s23​
微分得雅克比矩阵
v = [ x ˙ y ˙ z ˙ ] = [ − l 2 s 1 c 2 − l 3 s 1 c 23 − l 2 c 1 s 2 − l 3 c 1 s 23 − l 3 c 1 s 23 l 2 c 1 c 2 + l 3 c 1 c 23 − l 2 s 1 s 2 − l 3 s 1 s 23 − l 3 s 1 s 23 0 l 2 c 2 + l 3 c 23 l 3 c 23 ] [ θ 1 ˙ θ 2 ˙ θ 3 ˙ ] J = [ − l 2 s 1 c 2 − l 3 s 1 c 23 − l 2 c 1 s 2 − l 3 c 1 s 23 − l 3 c 1 s 23 l 2 c 1 c 2 + l 3 c 1 c 23 − l 2 s 1 s 2 − l 3 s 1 s 23 − l 3 s 1 s 23 0 l 2 c 2 + l 3 c 23 l 3 c 23 ] v = \begin{bmatrix}\dot{x} \\ \dot{y} \\ \dot{z} \end{bmatrix} = \begin{bmatrix} -l_2s_1c_2-l_3s_1c_{23} & -l_2c_1s_2-l_3c_1s_{23} & -l_3c_1s_{23} \\ l_2c_1c_2+l_3c_1c_{23} & -l_2s_1s_2-l_3s_1s_{23} & -l_3s_1s_{23} \\ 0 & l_2c_2+l_3c_{23} & l_3c_{23} \\ \end{bmatrix} \begin{bmatrix}\dot{\theta_1} \\ \dot{\theta_2} \\ \dot{\theta_3} \end{bmatrix} \\~\\ J = \begin{bmatrix} -l_2s_1c_2-l_3s_1c_{23} & -l_2c_1s_2-l_3c_1s_{23} & -l_3c_1s_{23} \\ l_2c_1c_2+l_3c_1c_{23} & -l_2s_1s_2-l_3s_1s_{23} & -l_3s_1s_{23} \\ 0 & l_2c_2+l_3c_{23} & l_3c_{23} \\ \end{bmatrix} v= ​x˙y˙​z˙​ ​= ​−l2​s1​c2​−l3​s1​c23​l2​c1​c2​+l3​c1​c23​0​−l2​c1​s2​−l3​c1​s23​−l2​s1​s2​−l3​s1​s23​l2​c2​+l3​c23​​−l3​c1​s23​−l3​s1​s23​l3​c23​​ ​ ​θ1​˙​θ2​˙​θ3​˙​​ ​ J= ​−l2​s1​c2​−l3​s1​c23​l2​c1​c2​+l3​c1​c23​0​−l2​c1​s2​−l3​c1​s23​−l2​s1​s2​−l3​s1​s23​l2​c2​+l3​c23​​−l3​c1​s23​−l3​s1​s23​l3​c23​​ ​

代码实现

template <typename T>
void computeJacobian(const Vec6<T>& q, Mat6<T>* J){T l2 = _upperarmLinkLength;T l3 = _forearmLinkLength;T t1 = q(0);T t2 = M_PI_2 - q(1);T t3 = -q(2);Mat6<T> J_ = Mat6<T>::Zero();Mat3<T> J_block;J_block(0, 0) = -l2*sin(t1)*cos(t2) - l3*sin(t1)*cos(t2+t3);J_block(0, 1) = -l2*cos(t1)*sin(t2) - l3*cos(t1)*sin(t2+t3);J_block(0, 2) = -l3*cos(t1)*sin(t2+t3);J_block(1, 0) = l2*cos(t1)*cos(t2) + l3*cos(t1)*cos(t2+t3);J_block(1, 1) = -l2*sin(t1)*sin(t2)  - l3*sin(t1)*sin(t2+t3);;J_block(1, 2) = -l3*sin(t1)*sin(t2+t3);    J_block(2, 0) = 0;J_block(2, 1) = l2*cos(t2) + l3*cos(t2+t3);J_block(2, 2) = l3*cos(t2+t3);     /* convert to model */Mat3<T> model_convert = Mat3<T>::Identity();model_convert(1,1) = -1;model_convert(2,2) = -1;J_block = J_block * model_convert;J_.block(0, 0, 3, 3) = J_block;*J = J_;
}

4. 优化

添加控制器
因为上述模型将末端等效为了一个节点，在仿真的时候，控制轨迹与理想轨迹是会存在有一定误差的，所以可以添加一个笛卡尔坐标系下的PID控制器，减小误差。
f = K p ( p d e s − p ) + K d ( v d e s − v ) τ o u t = J T f + τ f = K_p(p_{des} - p) + K_d(v_{des} - v)\\~\\ \tau_{out} = J^Tf + \tau f=Kp​(pdes​−p)+Kd​(vdes​−v) τout​=JTf+τ
K p K_p Kp​： 3 × 3 3 \times 3 3×3阶对角矩阵，对角元素为各个轴的PID比例系数。
K d K_d Kd​： 3 × 3 3 \times 3 3×3阶对角矩阵，对角元素为各个轴的PID微分系数。
p d e s p_{des} pdes​： 3 × 1 3 \times 1 3×1阶矩阵，末端目标位置。
v d e s v_{des} vdes​： 3 × 1 3 \times 1 3×1阶矩阵，末端目标速度。
p p p： 3 × 1 3 \times 1 3×1阶矩阵，末端当前位置。
v v v： 3 × 1 3 \times 1 3×1阶矩阵，末端当前速度。
τ \tau τ： 3 × 1 3 \times 1 3×1阶矩阵，上述动力学输出。

修改建模方向
在本文章建立的动力学模型中，关节2和关节3的转动的方向与上一篇运动学模型的方向相反。
【基于机械臂触觉伺服的物体操控研究】UR5e运动学建模及代码实现
这样子会导致实际应用时，需要修改部分数据的正负以适配模型。
所以读者在建立自己的动力学模型时，可以注意这个点，保证动力学，运动学模型，以及实际物理模型的坐标系方向都一一对应。