阻抗模型

阻抗控制的目的是将原有物体动力学修正为我们期望动力学。假设有一个弹簧，通过阻抗控制，可以使得它的刚度降低，实际推它时有可能感觉像一个海绵。

阻抗模型期望动力学一般为线性二阶系统
M d ( X ¨ o − X ¨ o d ) + K v ( X ˙ o − X ˙ o d ) + K p ( X o − X o d ) = F e x t ， M_d(\ddot X_o-\ddot X_o^d)+K_v(\dot X_o-\dot X_o^d)+K_p(X_o-X_o^d)=F_{ext}， Md(X¨o−X¨od)+Kv(X˙o−X˙od)+Kp(Xo−Xod)=Fext，
其中， F e x t F_{ext} Fext为外力(external force)，系数 M d M_d Md、 K v K_v Kv、 K p K_p Kp针对每个自由度独立指定（对角阵）。

阻抗模型的一般形式
M d X ¨ o = F e x t + F i m p ， M_d\ddot X_o=F_{ext}+F_{imp}， MdX¨o=Fext+Fimp，
其中， F i m p F_{imp} Fimp是虚阻抗力(imaginary impedance force)，既可以模拟动力学，也可以模拟力。仅用于模拟动力学时可以取
F i m p = M d X ¨ o d − K v ( X ˙ o − X ˙ o d ) − K p ( X o − X o d ) 。 F_{imp}=M_d\ddot X_o^d-K_v(\dot X_o-\dot X_o^d)-K_p(X_o-X_o^d)。 Fimp=MdX¨od−Kv(X˙o−X˙od)−Kp(Xo−Xod)。

物体阻抗

由牛顿欧拉公式可得
m o x ¨ o = f e x t + ∑ f c m d i + m o g I o ω ˙ o + ω o × I o ω o = τ e x t + ∑ r i × f c m d i + ∑ τ c m d i \begin{aligned} m_o\ddot x_o&=f_{ext}+\sum f_{cmd}^i+m_og\\ \mathcal I_o\dot \omega_o+\omega_o\times\mathcal I_o\omega_o&=\tau_{ext}+\sum r_i\times f_{cmd}^i+\sum\tau_{cmd}^i \end{aligned} mox¨oIoω˙o+ωo×Ioωo=fext+∑fcmdi+mog=τext+∑ri×fcmdi+∑τcmdi

假设实际物体动力学
M o X ¨ o + C o = F e x t + G F c m d ， M_o\ddot X_o+C_o=F_{ext}+G\boldsymbol F_{cmd}， MoX¨o+Co=Fext+GFcmd，
其中 M o = [ m o I 3 0 3 0 3 I o ] M_o=\begin{bmatrix}m_oI_3&0_3\\0_3&\mathcal I_o\end{bmatrix} Mo=[moI30303Io]， C o = [ − m o g ω o × I o ω o ] C_o=\begin{bmatrix}-m_og\\\omega_o\times\mathcal I_o\omega_o\end{bmatrix} Co=[−mogωo×Ioωo]是惯性矩阵和哥氏矩阵， G G G是抓持矩阵(grasp matrix)，通常为
G = [ I 3 0 I 3 0 ⋯ I 3 0 S ( r 1 ) I 3 S ( r 2 ) I 3 ⋯ S ( r n ) I 3 ] 。 G=\begin{bmatrix} I_3&0&I_3&0&\cdots&I_3&0\\ S(r_1)&I_3&S(r_2)&I_3&\cdots&S(r_n)&I_3 \end{bmatrix}。 G=[I3S(r1)0I3I3S(r2)0I3⋯⋯I3S(rn)0I3]。
这里注意三点：

上述物体动力学是在基坐标系下定义的，所以牛顿欧拉方程的牛顿部分非常简单
由此而产生的不便之处是， G G G是时变的，因为 r r r在基坐标系下时变（随物体旋转）
可以将上述物体动力学定义在物体坐标系下，这样 G G G是常数矩阵¹

期望的物体动力学，即物体阻抗²(object impedance），具有compliant behavior，
M d X ¨ o = F e x t + F i m p 。 M_d\ddot X_o=F_{ext}+F_{imp}。 MdX¨o=Fext+Fimp。
联立两式
G F c m d = C o − F e x t + M o M d − 1 ( F e x t + F i m p ) 。 G\boldsymbol F_{cmd} = C_o-F_{ext}+M_oM_d^{-1}(F_{ext}+F_{imp})。 GFcmd=Co−Fext+MoMd−1(Fext+Fimp)。
控制力(commanded forces)为
F c m d = G † [ C o − F e x t + M o M d − 1 ( F e x t + F i m p ) ] + F i n t ， \boldsymbol F_{cmd}=G^\dagger[C_o-F_{ext}+M_oM_d^{-1}(F_{ext}+F_{imp})]+\boldsymbol F_{int}， Fcmd=G†[Co−Fext+MoMd−1(Fext+Fimp)]+Fint，
其中 F i n t \boldsymbol F_{int} Fint为内力(internal forces)，在 G G G的零空间(null space)中。

分布阻抗

上节的物体阻抗方法将物体和机械臂考虑为一个整体，在物体层面(object level)进行集中式控制，但是集中式控制有如下弊端。

Such a centralized approach implies that specific pieces of hardware share data in real time from each manipulator force and position sensor. Notice that the structure of the controller depends on the system. This almost prohibits the implementation of regrasp- ing procedures or, more generally, any change in the system topology. Furthermore, when the system is built-up by manipulators with heterogeneous performance, the whole system performance reflects that of the worst one, especially for precision and stability³.

考虑分布式控制，可以在关节层面(joint level)和末端执行器层面(end-effector level)进行控制。末端执行器层面的控制有很多，如Leader-follower approach和Master-slave approach，前者是纯位置控制，后者master进行位置控制，slave进行力控制。

分布阻抗(distributed impedance)是在末端执行器层面的控制。由于多机械臂的冗余性，末端执行器可以通过点接触只控制力 f f f，无需控制力矩 τ \tau τ。

假设实际物体动力学
M o X ¨ o + C o = F e x t + G f ， M_o\ddot X_o+C_o=F_{ext}+G\boldsymbol f， MoX¨o+Co=Fext+Gf，
其中，
G = [ I 3 I 3 ⋯ I 3 S ( r 1 ) S ( r 2 ) ⋯ S ( r n ) ] 。 G=\begin{bmatrix} I_3&I_3&\cdots&I_3\\ S(r_1)&S(r_2)&\cdots&S(r_n) \end{bmatrix}。 G=[I3S(r1)I3S(r2)⋯⋯I3S(rn)]。
则实际末端速度为
x ˙ = G T X ˙ o 。 \dot {\boldsymbol x}=G^T\dot X_o。 x˙=GTX˙o。

末端期望的力为
f = G † ( M o X ¨ o + C o − F e x t ) + f i n t 。 \boldsymbol f=G^\dagger(M_o\ddot X_o+C_o-F_{ext})+\boldsymbol f_{int}。 f=G†(MoX¨o+Co−Fext)+fint。
末端期望阻抗模型为
− f i = M d i x ¨ i + B d i ( x ˙ i − x ˙ d i ) + K d i ( x i − x d i ) ， -f_i=M_d^i\ddot x_i+B_d^i(\dot x_i-\dot x_d^i)+K_d^i(x_i-x_d^i)， −fi=Mdix¨i+Bdi(x˙i−x˙di)+Kdi(xi−xdi)，
其中， f i f_i fi是末端输出的力，因此取负号才是受到的力。

定义 e i = x d i − x i e_i=x_d^i-x_i ei=xdi−xi，上式的拉普拉斯变换为
L f ( s ) = ( B d i s + K d i ) L e ( s ) 。 L_f(s)=(B_d^is+K_d^i)L_e(s)。 Lf(s)=(Bdis+Kdi)Le(s)。
当期望力看作常值时，为阶跃响应函数，有

lim ⁡ s → 0 s L e ( s ) = lim ⁡ s → 0 s ( B d i s + K d i ) − 1 ( f i / s ) = lim ⁡ s → 0 ( B d i s + K d i ) − 1 f i = K d i − 1 f i 。 \lim_{s\to 0}sL_e(s)=\lim_{s\to 0}s(B_d^is+K_d^i)^{-1}(f_i/s)=\lim_{s\to 0}(B_d^is+K_d^i)^{-1}f_i=K_d^{i^{-1}}f_i。 s→0limsLe(s)=s→0lims(Bdis+Kdi)−1(fi/s)=s→0lim(Bdis+Kdi)−1fi=Kdi−1fi。

期望的稳态末端轨迹与实际接触点轨迹之间的关系为
x d i = x i + K d i − 1 f i 。 x_d^i=x_i+K_d^{i^{-1}}f_i。 xdi=xi+Kdi−1fi。
机械臂末端速度和加速度有如下关系
q ˙ i = J i − 1 x ˙ i ， q ¨ i = J i − 1 x ¨ i − J i − 1 J ˙ i q ˙ i = J i − 1 x ¨ i − J i − 1 J ˙ i J i − 1 x ˙ i ， \dot q_i=J_i^{-1}\dot x_i，\quad \ddot q_i=J_i^{-1}\ddot x_i-J_i^{-1}\dot J_i\dot q_i=J_i^{-1}\ddot x_i-J_i^{-1}\dot J_iJ_i^{-1}\dot x_i， q˙i=Ji−1x˙i，q¨i=Ji−1x¨i−Ji−1J˙iq˙i=Ji−1x¨i−Ji−1J˙iJi−1x˙i，
其中 J i − 1 = M i − 1 J i T ( J i M i − 1 J i T ) − 1 J_i^{-1}=M_i^{-1}J_i^T(J_iM_i^{-1}J_i^T)^{-1} Ji−1=Mi−1JiT(JiMi−1JiT)−1。

将阻抗模型代入机械臂末端动力学
J i − T τ c m d i − f i = M ~ i ( q i ) x ¨ i + C ~ i ( q i , q ˙ i ) x ˙ i + N ~ i ( q i ) ， J_i^{-T}\tau_{cmd}^i-f_i=\tilde M_i(q_i)\ddot x_i+\tilde C_i(q_i,\dot q_i)\dot x_i+\tilde N_i(q_i)， Ji−Tτcmdi−fi=M~i(qi)x¨i+C~i(qi,q˙i)x˙i+N~i(qi)，
其中， M ~ i = J i − T M i J i − 1 \tilde M_i=J_i^{-T}M_iJ_i^{-1} M~i=Ji−TMiJi−1， C ~ i = J i − T ( C i J i − 1 + M i J i − 1 J ˙ i J i − 1 ) \tilde C_i=J_i^{-T}\left(C_iJ_i^{-1}+M_iJ_i^{-1}\dot J_iJ_i^{-1}\right) C~i=Ji−T(CiJi−1+MiJi−1J˙iJi−1)， N ~ i = J − T N i \tilde N_i=J^{-T}N_i N~i=J−TNi。

控制力矩为
τ c m d i = J i T [ − M ~ i M d i − 1 ( B d i ( x ˙ i − x ˙ d i ) + K d i ( x i − x d i ) ) + C ~ i x ˙ i + N ~ i + ( I 3 − M ~ i M d i − 1 ) f i ] \tau_{cmd}^i=J_i^T\left[-\tilde M_iM_d^{i^{-1}}(B_d^i(\dot x_i-\dot x_d^i)+K_d^i(x_i-x_d^i))+\tilde C_i\dot x_i+\tilde N_i+(I_3-\tilde M_iM_d^{i^{-1}})f_i\right] τcmdi=JiT[−M~iMdi−1(Bdi(x˙i−x˙di)+Kdi(xi−xdi))+C~ix˙i+N~i+(I3−M~iMdi−1)fi]

Matlab和RTB仿真

使用RTB工具箱建模二自由度机械臂，在竖直平面内搬运一个方块。

首先是机械臂建模和方块物体建模，代码示例如下。

%% robot modeling
lx = 1; lr = 0.1; g = [0 -9.81 0]; fvis = 0; fcou = 0;
rod = Cuboid([lx,lr,lr]);
Irod = rod.inertia;
dpm = {'a', lx, 'm', rod.mass, 'r', [-lx/2,0,0], 'qlim', [-pi/2, pi/2],'I', Irod,...'B', fvis, 'Tc', [fcou -fcou]};
rob(1) = SerialLink([Revolute(dpm{:}),Revolute(dpm{:})],'name','r1','gravity',...-g,'base',SE3([-1,0,0]/2));
rob(2) = SerialLink([Revolute(dpm{:}),Revolute(dpm{:})],'name','r2','gravity',...-g,'base',SE3([1,0,0]/2));
%% object modeling
obj = Cuboid([lx,lx,lr]);

然后定义几何位置关系，代码示例如下。

%% joint positions (x,y)
x0 = [-1,1.5,0;0,1.5,0];
for i=1:2q(i,:) = rob(i).ikine(SE3(x0(i,:)),'mask',[1,1,0,0,0,0]);
end
q(1,:) = [sum(q(1,:)),-q(1,2)];
dq = zeros(size(q));
qd = q;
%% object position (x,y,z,r,p,y)
Xo = [sum(x0)/size(x0,1),0,0,0];
dXo = zeros(1,6);
ddXo = dXo;
for i=1:2ro(i,:) = x0(i,:)-Xo(1:3);
end
%% object dynamics
Mo = blkdiag(obj.mass*eye(3),obj.inertia);
Co = @(dXo) [-obj.mass*g';skew(dXo(4:6))*obj.inertia*dXo(4:6)'];
r = @(Xo,i) (SO3.rpy(Xo(4:6))*ro(i,:)')';
G = @(Xo) [eye(3),eye(3);skew(r(Xo,1)),skew(r(Xo,2))];

物体阻抗

控制力由如下代码给出

%% command force
Fcmd = @(Xo,dXo,Xd) (pinv(G(Xo))*(Co(dXo)+Mo*md^-1*Fimp(Xo,dXo,Xd)')+Fint(Xo)')';

仿真结果如下图所示。

分布阻抗

控制力由如下代码给出

%% command force
Fcmd = @(Xo,dXo,Xd) (pinv(G(Xo))*(Co(dXo)+Mo*ddXo(Xo,dXo,Xd,dXd)')+Fint(Xo)')';

控制力矩由如下代码给出

md = 3; kv = 15; kp = 25; F = Fcmd(Xo,dXo,Xd);
% end-effector trajectory
J = rob(i).jacob0(q(i,:),dq(i,:)); J(3:6,:)=[];
Jdq = rob(i).jacob_dot(q(i,:),dq(i,:)); Jdq(3:6,:)=[];
x(i,:) = rob(i).fkine(q(i,:)).t;
dx(i,:) = J*dq(i,:)';
% desired trajectory
xd(i,:) = Xo(1:3)+(SO3.rpy(Xo(4:6))*ro(i,:)')'+F(3*i-2:3*i)/kp;
dxd = reshape((G(Xo)'*dXo')',[3,2])';
% joint torque
Mq = rob(i).inertia(q(i,:));
Cq = rob(i).coriolis(q(i,:),dq(i,:));
Gq = rob(i).gravload(q(i,:))';
Jinv = Mq^-1*J'*(J*Mq^-1*J')^-1;
Mx = Jinv'*Mq*Jinv;
Cx1 = Jinv'*Cq*Jinv;
Cx2 = Jinv'*Mq*Jinv*Jdq;
Gx = Jinv'*Gq;
tau(i,:) = J'*(-Mx*md^-1*(kv*(dx(i,1:2)-dxd(i,1:2))+kp*(x(i,1:2)-xd(i,1:2)))'+...
Cx1*dx(i,1:2)'+Cx2+Gx+(eye(2)-Mx*md^-1)*F(3*i-2:3*i-1)');

仿真结果如下图所示。

源代码

本文所需全部源代码已上传至我的GitHub，点击这里下载。运行two_link_object.m和two_link_distributed.m即可。使用前请确认RTB已经正确安装，下载和安装说明点击这里。

如果喜欢，欢迎点赞和fork。

Caccavale, F., Chiacchio, P., Marino, A., & Villani, L. (2008). Six-DOF impedance control of dual-arm cooperative manipulators. IEEE/ASME Transactions on Mechatronics, 13(5), 576–586. https://doi.org/10.1109/TMECH.2008.2002816 ↩︎
Schneider, S. A., & Cannon, R. H. (1992). Object Impedance Control for Cooperative Manipulation: Theory and Experimental Results. IEEE Transactions on Robotics and Automation, 8(3), 383–394. https://doi.org/10.1109/70.143355 ↩︎
Szewczyk, J., Plumet, F., & Bidaud, P. (2002). Planning and controlling cooperating robots through distributed impedance. Journal of Robotic Systems, 19(6), 283–297. https://doi.org/10.1002/rob.10041 ↩︎

机械臂协同搬运中的阻抗控制相关推荐

【控制】《复杂运动体系统的分布式协同控制与优化》-方浩老师-第6章-操作度优化条件下的移动机械臂协同搬运控制
第5章回到目录第7章第6章-操作度优化条件下的移动机械臂协同搬运控制 6.1 引言 6.2 问题描述 6.2.1 移动机械臂模型 6.2.2 协同搬运的优化问题描述 6.3 协同搬运过程中的移动 ...
5自由度并联机械臂实现搬运功能
1. 功能说明本文示例将实现R306样机5自由度并联机械臂搬运牛奶到指定地点的功能.该机械臂由1个5自由度并联机械臂和1个单轴丝杠平台构成,机械臂通过并联的方式同时控制同一个端点的运动.其驱动系统采 ...
机器人抓矸石_研究提出多机械臂协同煤矸分拣机器人
西安科技大学机械工程学院曹现刚教授团队提出一种基于煤矸图像识别和定位方法,并采用多机械臂协同煤矸分拣策略的煤矸分拣机器人.该机器人将机器视觉技术.机器人技术和深度学习技术结合,分拣效率高,煤矸快速识别 ...
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂（双臂）轨迹跟踪控制matlab仿真程序
空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...
两台Sawyer机械臂在rviz中的运动规划
两台Sawyer机械臂在rviz中的运动规划
共面阻抗对高频PCB 设计中传输线阻抗控制的影响
对传输线进行特性阻抗(Z)控制是RF及高速数字电路设计中的重要一环.良好的阻抗匹配能够显著减少高频高速信号的反射,从而避免出现信号完整性问题.在pcb设计的过程中,我们经常会遇到共面阻抗的情况. 如双 ...
二关节机械臂matlab控制,二连杆机械臂阻抗控制模拟（一）
在学习机器人动力学相关内容时看到MATLAB论坛上一个有意思的仿真项目Impedance Control for a 2-Link Robot Arm - User-interactive,一个用MA ...
如何实现Delta并联机械臂搬运磁铁物料？
1. 功能说明利用Delta并联机械臂实现不同点定点搬运磁铁物料的效果. 2. 结构说明 Delta并联机械臂,其驱动系统采用精度较高的42步进电机:传动系统为丝杠和万向球节:执行末端为搭载电磁铁的 ...
Delta并联机械臂实现电磁铁搬运功能
1. 功能说明本文示例将利用Delta并联机械臂实现不同点定点搬运磁铁物料的效果. 2. 结构说明 Delta并联机械臂,其驱动系统采用精度较高的42步进电机:传动系统为丝杠和万向球节:执行末端为搭 ...

机械臂协同搬运中的阻抗控制

文章目录

阻抗模型

物体阻抗

分布阻抗

Matlab和RTB仿真

物体阻抗

分布阻抗

源代码

机械臂协同搬运中的阻抗控制相关推荐

最新文章

热门文章