我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。
前言：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'
l2c2=l2′l3c23=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=21l2′m2∫0l2′v22dr=61m2l22c22θ1˙2 E3=21l3′m3∫0l3′v32dr=21m3l22c22θ1˙2+21m3l2l3c2c3θ1˙2+61m3l32c32θ1˙2 Ee=21me(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(Θ)=31m2l2′2+31m3l3′2+m3l2′2+m3l2′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=rc2y=rs2⇒{x˙=−θ2˙rs2y˙=θ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=l2c2+rc23y=l2s2+rcs23⇒{x˙=−θ2˙l2s2−r(θ2˙+θ3˙)s23y˙=θ2˙l2c2+r(θ2˙+θ3˙)c23 v32=x2+y2=l22θ2˙2+r2(θ2˙+θ3˙)2+2l2rc3θ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+2l2l3c3θ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=21l2m2∫0l2v22dr E3=21l3m3∫0l3v32dr Ee=21meve2 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=21m2gl2s2 G3=m3gl2s2+21m3gl3s23 Ge=megl2s2+megl3s23
创建动力学模型
将上述式子代入到拉格朗日公式中，即可求得
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=[m3l22+31m2l22+31m3l32+m3l2l3c331m3l32+21m3l2l3c331m3l32+21m3l2l3c331m3l32] Me=[mel22+mel32+2mel2l3c3mel32+mel2l3c3mel32+mel2l3c3mel32] B23=[−m3l2l3s3000] Be=[−2mel2l3s3000] C23=[021m3l2l3s3−21m3l2l3s30] Ce=[0mel2l3s3−mel2l3s30] G23=[(21m2+m3)gl2c2+21m3gl3c2321m3gl3c23] Ge=[megl2c2+megl3c23megl3c23 ]
合并结果
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+MeB(Θ)=B23+BeC(Θ)=C23+CeG(Θ)=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=(l2c2+l3c23)c1y=(l2c2+l3c23)s1z=l2s2+l3s23
微分得雅克比矩阵
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˙
=
−l2s1c2−l3s1c23l2c1c2+l3c1c230−l2c1s2−l3c1s23−l2s1s2−l3s1s23l2c2+l3c23−l3c1s23−l3s1s23l3c23
θ1˙θ2˙θ3˙
J=
−l2s1c2−l3s1c23l2c1c2+l3c1c230−l2c1s2−l3c1s23−l2s1s2−l3s1s23l2c2+l3c23−l3c1s23−l3s1s23l3c23
代码实现
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运动学建模及代码实现
这样子会导致实际应用时，需要修改部分数据的正负以适配模型。
所以读者在建立自己的动力学模型时，可以注意这个点，保证动力学，运动学模型，以及实际物理模型的坐标系方向都一一对应。
![[acwing周赛复盘] 第 86 场周赛20230114](https://img-blog.csdnimg.cn/82cf94fe746b4b4ab41e8c01aaed2f46.png)
