写在前面
上一篇文章(机械臂协同搬运中的阻抗控制)有一点遗留部分没有讲,即内力 F i n t F_{int} Fint应该如何建模。我们知道, F i n t F_{int} Fint在抓持矩阵 G G G的零空间里,即 G F i n t = 0 GF_{int}=0 GFint=0。如何选取一个有实际物理意义的模型,同时能够满足前述等式约束呢?这篇文章就会介绍一个virtual linkage模型1来表示搬运物体的内力。
virtual linkage模型
考虑如图1所示的多机械臂搬运场景。
图1. 多机械臂搬运场景
我们定义virtual linkage的运动学结构为接触点相互连结的3个主动(actuated)的棱柱关节,其中接触点处为被动(passive)的回转关节,它们形成一个闭链(closed-chain),如图2所示。
图2. virtual linkage结构
内力可以表示为棱柱关节上所作用的力。一般的,对任意virtual linkage,如果它的接触点有 n n n个,那么需要 3 ( n − 2 ) 3(n-2) 3(n−2)个棱柱关节。
内力的计算
假设接触点仅受到力作用。令 e i j e_{ij} eij为沿着某一个link从接触点 i i i到接触点 j j j的单位向量,令 r i r_i ri为物体上某一参考点到接触点 i i i的向量,如图3所示。
图3. virtual linkage力分析
定义
f
i
f_i
fi为施加到接触点
i
i
i上的力,则
f
=
[
f
1
T
,
f
2
T
,
f
3
T
]
T
\boldsymbol f=[f_1^T,f_2^T,f_3^T]^T
f=[f1T,f2T,f3T]T。定义
t
=
[
t
1
T
,
t
2
T
,
t
3
T
]
T
\boldsymbol t=[t_1^T,t_2^T,t_3^T]^T
t=[t1T,t2T,t3T]T,满足
f
=
−
E
t
+
f
e
,
\boldsymbol f=-E\boldsymbol t+\boldsymbol f_e,
f=−Et+fe,
其中
E
=
[
−
e
12
0
e
31
e
12
−
e
23
0
0
e
23
−
e
31
]
。
E=\begin{bmatrix} -e_{12}&0&e_{31}\\ e_{12}&-e_{23}&0\\ 0&e_{23}&-e_{31} \end{bmatrix}。
E=⎣⎡−e12e1200−e23e23e310−e31⎦⎤。
定义上外力
f
e
\boldsymbol f_e
fe不产生内力,故
t
=
−
E
ˉ
f
\boldsymbol t=-\bar E\boldsymbol f
t=−Eˉf,
E
ˉ
f
e
=
0
\bar E \boldsymbol f_e=0
Eˉfe=0,其中
E
ˉ
=
(
E
T
E
)
−
1
E
T
\bar E=(E^TE)^{-1}E^T
Eˉ=(ETE)−1ET为
E
E
E的左逆。
物体的合力
f
r
f_r
fr和合力矩
m
r
m_r
mr为
[
f
r
m
r
]
=
G
f
,
\begin{bmatrix} f_r\\ m_r \end{bmatrix}=G\boldsymbol f,
[frmr]=Gf,
其中
G
=
[
I
3
I
3
I
3
r
^
1
r
^
2
r
^
3
]
。
G=\begin{bmatrix} I_3&I_3&I_3\\ \hat r_1&\hat r_2&\hat r_3 \end{bmatrix}。
G=[I3r^1I3r^2I3r^3]。
注意:
- 我在 E t E\boldsymbol t Et前加了负号,因为按图3的定义 E t E\boldsymbol t Et是由接触点作用于机械臂末端的力,而 f \boldsymbol{f} f是机械臂末端作用于接触点的力,二者方向相反。
- 按照上面定义的方向,当 t > 0 \boldsymbol t>0 t>0时,物体受到压力,此时反作用力的方向与 E E E中的向量同向。
- 这里的 r i r_i ri和 e i j e_{ij} eij均为时变的,所以每次计算都要更新 E E E和 G G G。
- 这里假设抓持点固定,所以 E E E和 G G G虽然时变,但是仍满足刚体运动性质(向量模不变)。
- E E E位于 G G G的零空间,即 G F i n t = G E t = 0 GF_{int}=GEt=0 GFint=GEt=0。
两个机械臂搬运实例
下面我们将virtual linkage应用到两个机械臂搬运任务中,如图4所示。
图4. 两个机械臂搬运任务
我们可以写出施加的力和内力之间的关系
[
f
1
f
2
]
=
−
E
t
12
,
\begin{bmatrix} f_1\\ f_2 \end{bmatrix}=-Et_{12},
[f1f2]=−Et12,
其中
E
=
[
−
1
,
0
,
0
,
1
,
0
,
0
]
T
E=[-1,0,0,1,0,0]^T
E=[−1,0,0,1,0,0]T。
这里将物体对接触点产生的反作用力
t
12
t_{12}
t12建模为弹簧模型。令实际接触点位置为
x
1
x_1
x1、
x
2
x_2
x2,有如下关系
t
12
=
−
k
t
(
∥
x
1
−
x
2
∥
−
∥
r
1
−
r
2
∥
)
。
t_{12}= -k_t(\|x_1-x_2\|-\|r_1-r_2\|) 。
t12=−kt(∥x1−x2∥−∥r1−r2∥)。
反过来已知
f
1
f_1
f1、
f
2
f_2
f2,可由
E
ˉ
=
1
2
[
−
1
,
0
,
0
,
1
,
0
,
0
]
T
\bar E=\frac{1}{2}[-1,0,0,1,0,0]^T
Eˉ=21[−1,0,0,1,0,0]T计算得到
t
12
=
−
E
ˉ
[
f
1
f
2
]
。
t_{12}=-\bar E\begin{bmatrix} f_1\\ f_2 \end{bmatrix}。
t12=−Eˉ[f1f2]。
Matlab代码如下:
%% contact positions
x0 = [-1,1.5,0;
0,1.5,0];
%% object position
Xo = [sum(x0)/size(x0,1),0,0,0];
%% displacements
ro= x0-Xo(1:3);
r = @(Xo,i) (SO3.rpy(Xo(4:6))*ro(i,:)')';
%% object impedance
Mo = blkdiag(obj.mass*eye(3),obj.inertia);
Co = @(dXo) [-obj.mass*g';skew(dXo(4:6))*obj.inertia*dXo(4:6)'];
G = @(Xo) [eye(3),eye(3);
skew(r(Xo,1)),skew(r(Xo,2))];
%% virtual linkage
e12 = @(Xo) (r(Xo,2)-r(Xo,1))/norm(r(Xo,1)-r(Xo,2));
E = @(Xo) [-e12(Xo),e12(Xo)]';
% modeled as a spring
Kpe = 1;
f12 = @(x) Kpe*(norm(x(1,:)-x(2,:))-norm(ro(1,:)-ro(2,:)));
Fint = @(Xo) (E(Xo)*f12(x))';
%% command force
Fcmd = @(Xo,dXo,Xd) (pinv(G(Xo))*(Co(dXo)+Mo*md^-1*Fimp(Xo,dXo,Xd)')+Fint(Xo)')';
结果下图所示。

源代码
本文所需全部源代码已上传至我的GitHub,点击这里下载。与上一篇文章(机械臂协同搬运中的阻抗控制)相同,运行two_link_object.m
和two_link_distributed.m
即可。使用前请确认RTB已经正确安装,下载和安装说明点击这里。
如果喜欢,欢迎点赞和fork。
Williams, D., & Khatib, O. (1993). Virtual linkage: A model for internal forces in multi-grasp manipulation. In Proceedings - IEEE International Conference on Robotics and Automation (Vol. 1, pp. 1025–1030). Published by IEEE. https://doi.org/10.1109/robot.1993.292110 ↩︎