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

写在前面

上一篇文章(机械臂协同搬运中的阻抗控制)有一点遗留部分没有讲,即内力 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(n2)个棱柱关节。

内力的计算

假设接触点仅受到力作用。令 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=e12e1200e23e23e310e31
定义上外力 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(x1x2r1r2)

反过来已知 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.mtwo_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 ↩︎

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值