基于球面模型的飞行机械臂视觉伺服与多任务控制方法

基于球面模型的飞行机械臂视觉伺服与多任务控制方法

　　技术领域

　　本发明涉及无人机技术领域，尤其是基于球面模型的飞行机械臂视觉伺服与多任务控制方法。

　　背景技术

　　无人机系统具有运动机动性强、灵活性高等特点，可以在三维世界中自由飞行，因此近年来得到许多不同领域的行业的广泛运用，例如工业工厂检查、战场侦察、自然灾害侦察、地图创建和测量等方面有着广泛的应用。

　　飞行机械臂系统是在无人机上安装具有一定自由度的机械臂，与常规的无人机相比，作业型无人机系统具有诸多优势，它能在飞行过程中实现对空中或地面目标的快速捕捉，能迅速到达地面机器人无法进入的复杂环境中执行安装或回收测量设备等精细作业任务；但传统无人机系统极大地限制了它们主动操纵的能力，因此，研发无人机与环境之间的物理交互的飞行机械臂具有重大意义。本发明正是根据这种需求提出了基于球面模型的飞行机械臂视觉伺服与多任务协调控制方法。

　　发明内容

　　本发明提出基于球面模型的飞行机械臂视觉伺服与多任务控制方法，能在无人机上挂载具备主动操纵能力的机械臂。

　　本发明采用以下技术方案。

　　基于球面模型的飞行机械臂视觉伺服结构，所述伺服结构包括六自由度的无人机、三自由度的机械臂和控制模块；所述机械臂安装于无人机的基座的机械臂安装面处；所述机械臂安装面处还设有深度相机；所述深度相机的拍摄角度与机械臂安装面成角度设置，所述控制模块与深度相机、机械臂相连，并使深度相机、机械臂组合为机器人手眼标定系统。

　　所述机械臂包括三个转动关节；所述机械臂安装面位于基座的平台正下方；所述深度相机为Intel RealSense相机。

　　所述机械臂末端设有执行器；所述控制模块为可对无人机的飞行进行控制的控制模块；当所述飞行机械臂需对目标进行操作时，控制模块经深度相机对目标距离进行评估，若目标位于机械臂操作范围外，则控制模块驱动无人机向目标飞行，若目标位于机械臂操作范围内，则控制模块驱动机械臂对目标进行操作。

　　基于球面模型的飞行机械臂视觉伺服与多任务控制方法，所述控制方法使用上文中的基于球面模型的飞行机械臂视觉伺服结构，包括以下步骤；

　　步骤S1：控制模块通过深度相机获取目标物信息，并基于球面模型建立用于伺服飞行控制的球面坐标系，在无人机飞行过程中，基于球面坐标系实时计算目标物相对于无人机的期望位置实时位置之差，以基于球面坐标系的视觉伺服控制方法来控制无人机向目标物飞行，并得到无人机的伺服速度；

　　步骤S2：当无人机距目标物较近，使目标物到达相对于无人机的期望位置后，控制模块通过深度相机获取目标物信息和机械臂末端的执行器的信息，并基于球面坐标系对机械臂进行伺服控制，同时得到机械臂在球面坐标系内的角速度；

　　步骤S3：通过飞行机械臂视觉误差方程控制机械臂动作，以消除深度相机的视觉误差，使控制模块能够对机械臂进行伺服控制来移动执行器，把机械臂末端的执行器移动定位至目标物处；

　　步骤S4：采用针对多任务的协调控制方法控制飞行机械臂，对目标物定位并抓取，所述多任务包括机械臂末端位置控制、估计目标物体姿态、动态补偿系统重心的变化、避免机械臂关节极限，在对目标物定位并抓取的过程中，所述协调控制方法使飞行机械臂所执行的任务类别与飞行机械臂当前工况匹配。

　　所述球面模型采用纬度角与经度角坐标m(θz,θx)T表示，假设目标物或机械臂的执行器所在位置点在相机坐标系下的坐标为Mi＝(xi yi zi)T，则有：

　　

　　其中，R2＝x2+y2+z2。

　　所述步骤S1包括以下步骤；

　　步骤S11：球面坐标系为以深度相机为原点的相机坐标系，深度相机在世界坐标系以速度vo＝(vT,wT)T移动，设相机观察到的目标物所在位置点为球面特征点，球面特征点相对于相机坐标系的速度为即：

　　

　　步骤S12：对公式一求导，并结合公式二得到基于球面的图像雅可比矩阵为：

　　

　　步骤S13：得到球面特征点运动方程为：

　　

　　其中，vo＝(vT,wT)T∈R6×1，v＝(vx vy vz)T∈R3×1、w＝(wx wy wz)T∈R3×1分别表示无人机的平移和旋转速度。

　　所述步骤S2包括以下步骤；

　　步骤S21：令pc＝(xc yc zc)T∈R3×1表示机械臂末端执行器所在位置点P在相机坐标系下的位置，对公式一进行时间微分，可以得到：

　　

　　其中，

　　步骤S22：将机械臂末端点P在相机坐标系下速度结合公式二，可以将它表示成末端执行器的运动函数：

　　

　　其中，Bve＝(BTeBWe)T∈R6×1表示机械臂末端点的速度矢量。sk()表示向量的斜对称矩阵。

　　步骤S23：根据机器人学，Bve可以表示成机械臂关节角速度函数：

　　

　　其中，L(q)∈R6×3为机械臂雅可比矩阵。

　　步骤S24：综合公式五、公式六、公式七，可以得到：

　　

　　所述飞行机械臂视觉误差方程为：

　　e＝me-m0 (公式九)

　　对公式九进行时间微分，可以得到：

　　

　　其中，Jfirst＝(Ls -Jov -Jowz)∈R2×7代表系统的第一任务雅可比矩阵，代表系统的广义的速度输入，最后

　　所述的步骤S4包括以下步骤：

　　步骤S41：第一个任务为对机械臂末端位置的执行器进行控制，可通过公式十得到系统第一个任务的输入控制律为：

　　

　　其中，假如Jfirst是行满秩的，则Λ1是正常数；

　　此时，需要设计含有权值来分离飞行平台和机械臂变量的系统任务协调控制加权公式，并把该公式的微分结果带入公式九，使最终系统可以趋于指数稳定：

　　

　　当飞行机械臂使用视觉引导来抓取目标对象得过程中，为避免机械臂被完全拉伸到奇异点，考虑以下两个可能出现的情况。一是当末端执行器距离目标较远时，移动无人机来移动四旋翼平台比移动机械臂更好；二是当末端执行器靠近目标时，目标在机械臂的可操作范围之内，这时通过机器臂的运动比四旋翼平台的运动更好达成伺服效果；对于飞行机械臂来说，飞行平台和机械手的运动是完全不同的；因此，设计含有权值来分离飞行平台和机械臂变量的系统任务协调控制加权公式：

　　

　　其中：μ是个正常数，W1,W2是分离四旋翼平台变量和机器人手臂关节变量的权值矩阵，如下所示：

　　

　　n*是飞行平台运动的视觉误差范数期望值，n是飞行平台运动的视觉误差范数当前值。D()是定义的阈值函数，定义如下：

　　

　　对任务协调控制加权公式进行数学微分得：

　　

　　根据多体系统质心求解理论，可从系统重心CG计算公式对任务协调控制加权公式Ψ进行偏导数得到：

　　

　　通过比较公式十三与公式十七，可以得到含有权值的即下式：

　　

　　步骤S42：第二任务：机械臂末端定位；在第二任务中，在已知的2D点和3D点坐标对应关系，根据EPnP算法求解少量的方程组能够得到相机在目标物质心参考系下的姿态矩阵Tc∈R3×3，对该矩阵求逆，就求得目标在摄像机坐标系下的姿态，记为Tc-1，目标物体在坐标系C下位置为pc∈R3×1，则目标物在座标系C下的定位姿态矩阵为：

　　

　　步骤S43：第三任务：系统重心偏移控制

　　偏航角只改变作业型无人机的机体方向，而对姿态的影响甚微，所以对作业型无人机的姿态主要控制俯仰和翻滚的稳定性。所以假设偏航角的期望值ψd为固定值,输出较为理想的翻滚角俯仰角θd。因此在飞行机械臂整体建模的基础之上，设计位置控制器为：

　　

　　

　　其中，u1为系统输入升力，Ms为系统总质量，η＝(η1 η2 η2)T，F(CG)为系统重心偏移参数。在无人机机体基座坐标系下，根据多体系统质心求解理论，系统重心CG为：

　　

　　其中，Mi表示机械臂第i个关节重量，CGi表示关节i的质心在无人机机体基座坐标系的位置坐标；

　　姿态控制器主要控制作业型无人机的翻滚角、俯仰角和偏航角，通过估计系统实际惯性张量偏差值和重心偏移参数为使飞行机械臂的姿态能达到期望姿态，定义姿态误差函数为：

　　

　　利用反演控制，设计如下控制律：

　　

　　其中，u2、u3、u4分别为系统翻滚力矩输入、俯仰力矩输入、偏航力矩输入，si,(i＝1,2,3)为系统定义的状态误差，状态变量分别是分别调节对应的参数ki、li来调整姿态误差和状态误差的上限值，函数G(·)表示高斯基函数输出，μi、ξi,(i＝1,2,3)为正常数，p、q、r为坐标系B下的角速度矢量，为常参数矩阵。

　　步骤S44：第四任务：避免关节限制

　　定义关节角度误差：

　　eq＝q*-q (公式二十五)

　　其中，q*为关节角度期望值，qL、qH为关节极限最小值与最大值。定义如下任务函数：

　　

　　其中，kt是控制关节角度正常化的权值矩阵，kt＝diag{(qH1-qL1)-2(qH2-qL2)-2(qH3-qL3)-2}，任务函数期望值为因此期望的任务为：

　　

　　其中，是矩阵Jf＝(I3×3 03×4)的伪逆。

　　与现有技术相比，本发明有以下有益效果：与经典的视觉伺服建模方案不同，本发明控制系统采用基于球面模型的图像特征来建立视觉误差方程。本发明视觉伺服方案具有尽可能大的视场，非常适合无人机。它消除了保留视场特征的需要，降低了光流场运动的不确定性。针对飞行机械臂欠驱动、高度耦合的特点，本发明采用具有线速度和角速度解耦的图像雅可比矩阵进行视觉伺服任务。

　　同时，本发明设计了无人机与机械手之间的任务协调函数，得到了基于无人机视觉误差范数的加权雅可比矩阵，推导出该雅可比矩阵的伪逆矩阵。这些措施提高了无人机与机械手之间的任务协调性，避免了机械手关节极限。

　　针对机械手运动引起的重心偏移问题，本发明设计了一种基于重心偏移补偿的滑模位置控制系统。同时，设计了基于反演控制的姿态控制器，提高了飞行机械臂系统的稳定性。无人机可以稳定地悬停在目标上空，机械手可以有序、安全地抓取目标。

　　本发明的飞行机械臂与常规的无人机相比，它能在飞行过程中实现对空中或地面目标的快速捕捉，能迅速到达地面机器人无法进入的复杂环境中执行安装或回收测量设备等精细作业任务，极大的提高了无人机与环境之间的物理交互能力。因此其具有很大的应用价值。

　　附图说明

　　下面结合附图和具体实施方式对本发明进一步详细的说明：

　　附图1是本发明所述设备的示意图；

　　附图2是本发明的流程示意图；

　　附图3是步骤S1中的伺服飞行控制时的坐标变化示意图；

　　附图4是步骤S2中对机械臂进行伺服控制的坐标变化示意图；

　　附图5是步骤S3中对机械臂进行伺服控制来移动执行器时的坐标变化示意图；

　　图中：1-无人机；2-深度相机；3-机械臂。

　　具体实施方式

　　如图所示，基于球面模型的飞行机械臂视觉伺服结构，所述伺服结构包括六自由度的无人机、三自由度的机械臂和控制模块；所述机械臂安装于无人机的基座的机械臂安装面处；所述机械臂安装面处还设有深度相机；所述深度相机的拍摄角度与机械臂安装面成角度设置，所述控制模块与深度相机、机械臂相连，并使深度相机、机械臂组合为机器人手眼标定系统。

　　所述机械臂包括三个转动关节；所述机械臂安装面位于基座的平台正下方；所述深度相机为Intel RealSense相机。

　　所述机械臂末端设有执行器；所述控制模块为可对无人机的飞行进行控制的控制模块；当所述飞行机械臂需对目标进行操作时，控制模块经深度相机对目标距离进行评估，若目标位于机械臂操作范围外，则控制模块驱动无人机向目标飞行，若目标位于机械臂操作范围内，则控制模块驱动机械臂对目标进行操作。

　　基于球面模型的飞行机械臂视觉伺服与多任务控制方法，所述控制方法使用上文中的基于球面模型的飞行机械臂视觉伺服结构，包括以下步骤；

　　步骤S1：控制模块通过深度相机获取目标物信息，并基于球面模型建立用于伺服飞行控制的球面坐标系，在无人机飞行过程中，基于球面坐标系实时计算目标物相对于无人机的期望位置实时位置之差，以基于球面坐标系的视觉伺服控制方法来控制无人机向目标物飞行，并得到无人机的伺服速度；

　　步骤S2：当无人机距目标物较近，使目标物到达相对于无人机的期望位置后，控制模块通过深度相机获取目标物信息和机械臂末端的执行器的信息，并基于球面坐标系对机械臂进行伺服控制，同时得到机械臂在球面坐标系内的角速度；

　　步骤S3：通过飞行机械臂视觉误差方程控制机械臂动作，以消除深度相机的视觉误差，使控制模块能够对机械臂进行伺服控制来移动执行器，把机械臂末端的执行器移动定位至目标物处；

　　步骤S4：采用针对多任务的协调控制方法控制飞行机械臂，对目标物定位并抓取，所述多任务包括机械臂末端位置控制、估计目标物体姿态、动态补偿系统重心的变化、避免机械臂关节极限，在对目标物定位并抓取的过程中，所述协调控制方法使飞行机械臂所执行的任务类别与飞行机械臂当前工况匹配。

　　所述球面模型采用纬度角与经度角坐标m(θz,θx)T表示，假设目标物或机械臂的执行器所在位置点在相机坐标系下的坐标为Mi＝(xi yi zi)T，则有：

　　

　　其中，R2＝x2+y2+z2。

　　在本例中，步骤S1如图3所示，具体为：

　　所述步骤S1包括以下步骤；

　　步骤S11：球面坐标系为以深度相机为原点的相机坐标系，深度相机在世界坐标系以速度vo＝(vT,wT)T移动，设相机观察到的目标物所在位置点为球面特征点，球面特征点相对于相机坐标系的速度为即：

　　

　　步骤S12：对公式一求导，并结合公式二得到基于球面的图像雅可比矩阵为：

　　

　　步骤S13：得到球面特征点运动方程为：

　　

　　其中，vo＝(vT,wT)T∈R6×1，v＝(vx vy vz)T∈R3×1、w＝(wx wy wz)T∈R3×1分别表示无人机的平移和旋转速度。

　　在本例中，步骤S2如图4所示，具体为：

　　所述步骤S2包括以下步骤；

　　步骤S21：令pc＝(xc yc zc)T∈R3×1表示机械臂末端执行器所在位置点P在相机坐标系下的位置，对公式一进行时间微分，可以得到：

　　

　　其中，

　　步骤S22：将机械臂末端点P在相机坐标系下速度结合公式二，可以将它表示成末端执行器的运动函数：

　　

　　其中，Bve＝(BTeBWe)T∈R6×1表示机械臂末端点的速度矢量。sk()表示向量的斜对称矩阵。

　　步骤S23：根据机器人学，Bve可以表示成机械臂关节角速度函数：

　　

　　其中，L(q)∈R6×3为机械臂雅可比矩阵。

　　步骤S24：综合公式五、公式六、公式七，可以得到：

　　

　　在本例中，步骤S3如图5所示，具体为：

　　所述飞行机械臂视觉误差方程为：

　　e＝me-m0 (公式九)

　　对公式九进行时间微分，可以得到：

　　

　　其中，Jfirst＝(Ls -Jov -Jowz)∈R2×7代表系统的第一任务雅可比矩阵，代表系统的广义的速度输入，最后

　　所述的步骤S4包括以下步骤：

　　步骤S41：第一个任务为对机械臂末端位置的执行器进行控制，可通过公式十得到系统第一个任务的输入控制律为：

　　

　　其中，假如Jfirst是行满秩的，则Λ1是正常数；

　　此时，需要设计含有权值来分离飞行平台和机械臂变量的系统任务协调控制加权公式，并把该公式的微分结果带入公式九，使最终系统可以趋于指数稳定：

　　

　　当飞行机械臂使用视觉引导来抓取目标对象得过程中，为避免机械臂被完全拉伸到奇异点，考虑以下两个可能出现的情况。一是当末端执行器距离目标较远时，移动无人机来移动四旋翼平台比移动机械臂更好；二是当末端执行器靠近目标时，目标在机械臂的可操作范围之内，这时通过机器臂的运动比四旋翼平台的运动更好达成伺服效果；对于飞行机械臂来说，飞行平台和机械手的运动是完全不同的；因此，设计含有权值来分离飞行平台和机械臂变量的系统任务协调控制加权公式：

　　

　　其中：μ是个正常数，W1,W2是分离四旋翼平台变量和机器人手臂关节变量的权值矩阵，如下所示：

　　

　　n*是飞行平台运动的视觉误差范数期望值，n是飞行平台运动的视觉误差范数当前值。D()是定义的阈值函数，定义如下：

　　

　　对任务协调控制加权公式进行数学微分得：

　　

　　根据多体系统质心求解理论，可从系统重心CG计算公式对任务协调控制加权公式Ψ进行偏导数得到：

　　

　　通过比较公式十三与公式十七，可以得到含有权值的即下式：

　　

　　步骤S42：第二任务：机械臂末端定位；在第二任务中，在已知的2D点和3D点坐标对应关系，根据EPnP算法求解少量的方程组能够得到相机在目标物质心参考系下的姿态矩阵Tc∈R3×3，对该矩阵求逆，就求得目标在摄像机坐标系下的姿态，记为Tc-1，目标物体在坐标系C下位置为pc∈R3×1，则目标物在座标系C下的定位姿态矩阵为：

　　

　　步骤S43：第三任务：系统重心偏移控制

　　偏航角只改变作业型无人机的机体方向，而对姿态的影响甚微，所以对作业型无人机的姿态主要控制俯仰和翻滚的稳定性。所以假设偏航角的期望值ψd为固定值,输出较为理想的翻滚角俯仰角θd。因此在飞行机械臂整体建模的基础之上，设计位置控制器为：

　　

　　

　　其中，u1为系统输入升力，Ms为系统总质量，η＝(η1 η2 η2)T，F(CG)为系统重心偏移参数。在无人机机体基座坐标系下，根据多体系统质心求解理论，系统重心CG为：

　　

　　其中，Mi表示机械臂第i个关节重量，CGi表示关节i的质心在无人机机体基座坐标系的位置坐标；

　　姿态控制器主要控制作业型无人机的翻滚角、俯仰角和偏航角，通过估计系统实际惯性张量偏差值和重心偏移参数为使飞行机械臂的姿态能达到期望姿态，定义姿态误差函数为：

　　

　　利用反演控制，设计如下控制律：

　　

　　其中，u2、u3、u4分别为系统翻滚力矩输入、俯仰力矩输入、偏航力矩输入，si,(i＝1,2,3)为系统定义的状态误差，状态变量分别是分别调节对应的参数ki、li来调整姿态误差和状态误差的上限值，函数G(·)表示高斯基函数输出，μi、ξi,(i＝1,2,3)为正常数，p、q、r为坐标系B下的角速度矢量，为常参数矩阵。

　　步骤S44：第四任务：避免关节限制

　　定义关节角度误差：

　　eq＝q*-q (公式二十五)

　　其中，q*为关节角度期望值，qL、qH为关节极限最小值与最大值。定义如下任务函数；

　　其中，kt是控制关节角度正常化的权值矩阵，kt＝diag{(qH1-qL1)-2 (qH2-qL2)-2(qH3-qL3)-2}，任务函数期望值为因此期望的任务为：

　　

　　其中，是矩阵Jf＝(I3×3 03×4)的伪逆。