论文阅读笔记《Optimal Image-Based Guidance of Mobile Manipulators Using Direct Visual Servoing》

核心思想

该文提出一种基于图像的直接伺服算法用于引导移动机械臂的动作。该文的研究还是比较有特色的，首先该文的控制对象是移动机器人平台+7自由度的机械臂；其次本文采用直接视觉伺服控制，与非直接视觉伺服输出相机的速度控制律不同，直接视觉伺服输出施加到电机的力和力矩；最后该文是采用了优化控制的思路来实现视觉伺服控制的。

整个系统的坐标系关系如上图所示，为方便叙述，现将涉及到的重要参数符号整理在下

qmTq_m^TqmT表示机械臂关节的广义坐标
qpTq_p^TqpT表示移动平台的广义坐标
s=[f1,f2...fk]Ts=[f_1,f_2...f_k]^Ts=[f1,f2...fk]T表示kkk个点的视觉特征，本文kkk取4
Js=[Jf1,Jf2...Jfk]TJ_s=[J_{f_1},J_{f_2}...J_{f_k}]^TJs=[Jf1,Jf2...Jfk]T表示图像雅可比矩阵，交互矩阵
r=[tx,ty,tz,ϕx,ϕy,ϕz]r=[t_x,t_y,t_z,\phi_x,\phi_y,\phi_z]r=[tx,ty,tz,ϕx,ϕy,ϕz]表示相机在世界坐标系下的坐标
rc˙=[vc,wc]T\dot{r_c}=[v_c,w_c]^Trc˙=[vc,wc]T表示相机的速度
upu_pup表示移动平台的速度，qp˙=Ψ(qp)up\dot{q_p}=\Psi(q_p)u_pqp˙=Ψ(qp)up
umu_mum表示机械臂的速度，qm˙=um\dot{q_m}=u_mqm˙=um
u=[upT,umT]Tu=[u_p^T,u_m^T]^Tu=[upT,umT]T表示机器人的速度
JcJ_cJc表示相机的运动速度rc˙\dot{r_c}rc˙和机器人速度uuu之间的关系
FbF_bFb表示作用在平台上的力和力矩
τ\tauτ表示作用在机械臂关节上的转矩

根据视觉伺服理论，图像特征点的运动速度s˙\dot{s}s˙与机器人的速度uuu存在以下关系
s˙=Jiu\dot{s}=J_ius˙=Jiu其中Ji=JsJcJ_i=J_sJ_cJi=JsJc，则s˙=Js(s,Z)[vcwc]=Js(s,Z)Jc(q)u\dot{s}=J_s(s,Z)\begin{bmatrix}v_c\\ w_c \end{bmatrix}=J_s(s,Z)J_c(q)us˙=Js(s,Z)[vcwc]=Js(s,Z)Jc(q)u因为相机固定在机械臂上因此JcJ_cJc通过一系列的矩阵运算是可以计算得到的。对sss求二阶导数，及视觉特征的加速度为s¨=Jiu˙+J˙iu\ddot{s}=J_i\dot{u}+\dot{J}_ius¨=Jiu˙+J˙iu
下面考虑系统动力学问题，整个机器人系统的动力学方程为

如果将机械臂和移动平台统一表示，上式也可写为

其中τ~\tilde{\tau}τ~表示作用在机器人上的力、力矩和转矩，M~\tilde{M}M~表示系统的转动惯量矩阵，C~\tilde{C}C~表示与速度和位移有关的非线性项。
因为本文是从最优控制的角度来实现视觉伺服控制的，也就是将机器人的控制过程看作一个优化问题，将作用在机器人上的控制律看作是约束条件A(t)u˙=b(t)A(t)\dot{u}=b(t)A(t)u˙=b(t)将最小化机器人运动所需的力和力矩作为优化目标，目标函数为Ω(t)=τ~TW(t)τ~\Omega(t)=\tilde{\tau}^TW(t)\tilde{\tau}Ω(t)=τ~TW(t)τ~其中W(t)W(t)W(t)是一个与时间相关的权重矩阵。按照优化算法，最优的τ~\tilde{\tau}τ~可以表示为

回到视觉伺服问题中来，由参考视觉特征的二阶导数s¨r=Jiu˙+J˙iu\ddot{s}_r=J_i\dot{u}+\dot{J}_ius¨r=Jiu˙+J˙iu可得Jiu˙=s¨r−J˙iuJ_i\dot{u}=\ddot{s}_r-\dot{J}_iuJiu˙=s¨r−J˙iu对应到优化问题中可得A=Ji，b=s¨r−J˙iuA=J_i，b=\ddot{s}_r-\dot{J}_iuA=Ji，b=s¨r−J˙iu带入到τ~\tilde{\tau}τ~中可得

当W=M~−2W=\tilde{M}^{-2}W=M~−2时

当W=M~−1W=\tilde{M}^{-1}W=M~−1时

从动力学角度来看视觉伺服的任务约束可以表示为

其中sd,s˙d,s¨d{s}_d,\dot{s}_d,\ddot{s}_dsd,s˙d,s¨d分别表示期望的特征点位置、速度和加速度（一般视觉伺服只考虑位置sds_dsd），KDK_DKD表示微分矩阵，KPK_PKP表示比例矩阵。如果用误差e˙s\dot{e}_se˙s来表示(s˙d−s˙)(\dot{s}_d-\dot{s})(s˙d−s˙),误差es{e}_ses来表示(sd−s)({s}_d-{s})(sd−s)，则上式可以进一步写为

再将s¨r\ddot{s}_rs¨r带入到τ~\tilde{\tau}τ~中可得

根据上式就可以利用视觉特征的位置、速度误差和期望加速度来计算作用在机器人上的力、力矩和转矩了。

创新点

融合统一了移动平台和机械臂两个系统的视觉伺服控制
采用直接视觉伺服控制，计算作用在机器人上的力和力矩
通过优化的形式来计算控制律

算法评价

本文非常全面的介绍了如何通过优化的方法来对一个移动平台+机械臂的系统整体进行直接视觉伺服控制，虽然整体的创新性并不高，但整个流程可以作为一个好的参考。并且其中考虑到视觉特征的速度和加速度误差，而不是仅仅考虑了位置误差，使得整个控制过程更加稳定。

如果大家对于深度学习与计算机视觉领域感兴趣，希望获得更多的知识分享与最新的论文解读，欢迎关注我的个人公众号“深视”。