机械臂协同搬运中的内力建模

写在前面

上一篇文章(机械臂协同搬运中的阻抗控制)有一点遗留部分没有讲，即内力$F_{int}$应该如何建模。我们知道，$F_{int}$在抓持矩阵$G$的零空间里，即$G{F}_{int}=0$。如何选取一个有实际物理意义的模型，同时能够满足前述等式约束呢？这篇文章就会介绍一个virtual linkage模型¹来表示搬运物体的内力。

virtual linkage模型

考虑如图1所示的多机械臂搬运场景。

图1. 多机械臂搬运场景

我们定义virtual linkage的运动学结构为接触点相互连结的3个主动(actuated)的棱柱关节，其中接触点处为被动(passive)的回转关节，它们形成一个闭链(closed-chain)，如图2所示。

图2. virtual linkage结构

内力可以表示为棱柱关节上所作用的力。一般的，对任意virtual linkage，如果它的接触点有$n$个，那么需要$3(n-2)$个棱柱关节。

内力的计算

假设接触点仅受到力作用。令$e_{ij}$为沿着某一个link从接触点$i$到接触点$j$的单位向量，令$r_i$为物体上某一参考点到接触点$i$的向量，如图3所示。

图3. virtual linkage力分析

定义$f_i$为施加到接触点$i$上的力，则$\mathbf{f}=[f_1^T,f_2^T,f_3^T{]}^T$。定义$\mathbf{t}=[t_1^T,t_2^T,t_3^T{]}^T$，满足
$$\mathbf{f}=-E\mathbf{t}+{\mathbf{f}}_e，$$
其中
$$E=\left[\begin{array}{ccc}-e_{12}& 0& e_{31}\\ e_{12}& -e_{23}& 0\\ 0& e_{23}& -e_{31}\end{array}\right]。$$
定义上外力${\mathbf{f}}_e$不产生内力，故$\mathbf{t}=-\bar{E}\mathbf{f}$，$\bar{E}{\mathbf{f}}_e=0$，其中$\bar{E}=(E^{T}E{)}^{-1}{E}^T$为$E$的左逆。

物体的合力$f_r$和合力矩$m_r$为
$$\left[\begin{array}{c}{f}_r\\ m_r\end{array}\right]=G\mathbf{f}，$$
其中
$$G=\left[\begin{array}{ccc}{I}_3& I_3& I_3\\ {\hat{r}}_1& {\hat{r}}_2& {\hat{r}}_3\end{array}\right]。$$
注意：

• 我在$E\mathbf{t}$前加了负号，因为按图3的定义$E\mathbf{t}$是由接触点作用于机械臂末端的力，而$\mathbf{f}$是机械臂末端作用于接触点的力，二者方向相反。
• 按照上面定义的方向，当$\mathbf{t}>0$时，物体受到压力，此时反作用力的方向与$E$中的向量同向。
• 这里的$r_i$和$e_{ij}$均为时变的，所以每次计算都要更新$E$和$G$。
• 这里假设抓持点固定，所以$E$和$G$虽然时变，但是仍满足刚体运动性质(向量模不变)。
• $E$位于$G$的零空间，即$G{F}_{int}=GEt=0$。

两个机械臂搬运实例

下面我们将virtual linkage应用到两个机械臂搬运任务中，如图4所示。

图4. 两个机械臂搬运任务

我们可以写出施加的力和内力之间的关系
$$\left[\begin{array}{c}{f}_1\\ f_2\end{array}\right]=-E{t}_{12}，$$
其中$E=[-1,0,0,1,0,0{]}^T$。

这里将物体对接触点产生的反作用力$t_{12}$建模为弹簧模型。令实际接触点位置为$x_1$、$x_2$，有如下关系
$$t_{12}=-k_t(\Vert x_1-x_2\Vert -\Vert r_1-r_2\Vert )。$$

反过来已知$f_1$、$f_2$，可由$\bar{E}=\frac{1}{2}[-1,0,0,1,0,0{]}^T$计算得到
$$t_{12}=-\bar{E}\left[\begin{array}{c}{f}_1\\ f_2\end{array}\right]。$$
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。

---

1. 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 ↩︎