CN107450579B

CN107450579B - 一种搭载机械臂的四旋翼飞行器平衡作业控制方法

Info

Publication number: CN107450579B
Application number: CN201710623953.7A
Authority: CN
Inventors: 宋光明; 连杰; 王营华; 孙慧玉; 宋爱国
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2017-07-27
Filing date: 2017-07-27
Publication date: 2020-02-18
Anticipated expiration: 2037-07-27
Also published as: CN107450579A

Abstract

本发明公开了一种搭载机械臂的四旋翼飞行器平衡作业控制方法，步骤包括：一、构建系统平台，包括四旋翼飞行器、舵机安装架、机械臂、平衡机构、无线通信模块、舵机控制器、飞行控制器和PC机；二、建立载体坐标系；确定机械臂的安装方向；三、设定机械臂的运动目标角度，通过无线通信模块传送到舵机控制器，舵机控制器控制机械臂的各个关节舵机运动至所设角度；同时关节舵机的运动角度反馈至舵机控制器，计算出平衡机构所需转动的目标角度，平衡机构接收到的舵机控制器发出的命令，运动目标角度；本发明结构简洁，平衡计算简单可靠，节约了软硬件资源；机械臂操作平稳安全，消除了机械臂运动对空中作业平台稳定的影响，提高了空中作业精度。

Description

一种搭载机械臂的四旋翼飞行器平衡作业控制方法

技术领域

本发明涉及机器人控制方法，尤其涉及一种搭载机械臂的四旋翼飞行器作业过程中的平衡控制方法。

背景技术

四旋翼飞行器具有结构简单，操作灵活，垂直起降等优点，目前已应用于灾害现场勘测，科考勘察，广域环境建模等被动式监测任务方面，随着应用领域的拓展，人们期望四旋翼飞行器能对所处环境施加主动影响。早期，研究者们通过在四旋翼上安装简单的机械手爪和挂钩等与环境交互。由于单一的结构，这类四旋翼飞行器的作业能力和场景受到了极大的限制。通过搭载多自由度的机械臂，使得空中作业型四旋翼能够较好的完成抓取，组装，搬运等任务，然而，机械臂与环境的接触过程中产生的力和力矩对空中作业型四旋翼产生了极大的干扰，特别是抓取重物后，系统重心的偏移产生，导致系统的不稳定。

为了克服机械臂在环境交互中的不利影响，现有技术的解决方法主要包括如下两类：一是采取将作业装置视为飞行器的一部分进行动力学整体建模，该方法在动力学建模和控制时较为简单，但只适用于机械臂较小较轻的情况；另一类方法直接利用控制算法对四旋翼飞行器的重心进行调节，但飞行控制器对扰动的补偿是有限的，当系统的状态处于平衡点附近范围时，对机械臂关节小范围内运动有较好的镇定效果，但是当手臂摆动较大时，控制器无法使系统有效稳定。

发明内容

发明目的：本发明提出了一种用于实现四旋翼的运动控制和机械臂的抓取作业更加稳定精确的搭载机械臂的四旋翼飞行器平衡作业控制方法。

技术方案：一种搭载机械臂的四旋翼飞行器平衡作业控制方法，包括如下步骤：

步骤一：构建系统实施平台，所述系统实施平台包括四旋翼飞行器、舵机安装架、机械臂、平衡机构、无线通信模块、舵机控制器、飞行控制器和PC机；所述机械臂包括一个或多个连杆、一个或多个用于连接和驱动连杆的关节舵机以及设于机械臂端部的用于抓取和放置物体的机械手；

步骤二：以四旋翼飞行器的形心O_b为整个空中作业系统的原点，建立载体坐标系{O_b}-x_by_bz_b；机械臂的安装方向为x轴正方向，z轴正方向垂直x轴向上，并由右手系确定y轴正方向；

步骤三：系统上电初始化，在PC机上设定机械臂的各个关节舵机的运动目标角度H^*(t)，通过无线通信模块传送到舵机控制器，舵机控制器判断是否接收到新的设定目标角度H^*(t)，如果没有则进入等待模式，如果收到新目标角度则判断距离上次接收到控制信号的时间差，如果时间差大于设定值τ，舵机控制器产生指令，发送至机械臂的各个关节舵机，使其运动至所设角度；同时关节舵机的运动角度H反馈至舵机控制器，计算出平衡机构所需转动的目标角度

平衡机构接收到的舵机控制器发出的命令，运动目标角度；系统更新机械臂和平衡机构的位置信息。

步骤三中，通过平衡控制算法计算得到平衡机构的目标角度

具体步骤如下：

步骤(3.1)：确定各连杆、机械手和平衡机构的重心在其坐标系中的位置坐标和质量；

步骤(3.2)：计算步骤(3.1)中各部分坐标系到前一坐标系的转换矩阵；

步骤(3.3)：计算得到各部分坐标系上重心转换到载体坐标系{O_b}上的转换矩阵；并计算各部分的重心在载体坐标系的位置；

步骤(3.4)：将上述步骤所得数据带入平衡方程：

∑m_ix_i+m_cx_c+m_tx_t＝0

得到平衡机构所需要运动的目标角度

其中x_i、x_c、x_t分别机械臂各部分、平衡机构以及被抓取物体的重心在载体系{O_b}中的位置。

所述机械臂为三自由度机械臂，包括第一连杆、第二连杆和用于驱动连杆运动的关节部分；所述关节部分包括用于驱动第一连杆的第一舵机、用于驱动第二连杆的第二舵机和用于驱动机械手动作的第三舵机，各关节舵机的运动角度分别为θ₁，θ₂，θ₃。

所述平衡机构包括一配重支架、用于驱动所述配置支架运动的第四舵机，以及两个对称布置在配重支架端部的配重块，所述第四舵机的运动角度为θ_c。

优选的，所述配重块为锂电池。

所述系统实施平台还包括摄像头。

有益效果：与现有技术相比，本发明具有如下显著优势：1、消除了机械臂运动对空中作业平台稳定的影响，提高了空中作业精度；2、平衡控制下，抓取质量较大的物体时，能稳定悬停，保证了空中作业安全；3、平衡装置结构要求低，平衡计算简单可靠，节约了软硬件资源。

附图说明

图1是本发明的系统结构示意图；

图2是系统坐标系示意图；

图3是三自由度机械臂结构图；

图4是平衡机构结构图；

图5是控制系统原理图；

图6是平衡机构控制流程图。

具体实施方式

下面结合附图和实施例对本发明的技术方案作进一步说明。

如图1所示，本发明的系统实施平台主要由四旋翼飞行器1，舵机安装架2，包含一机械手的三自由度机械臂3，平衡机构4，摄像头5，无线通信模块、舵机控制器6、飞行控制器7和PC机组成；其中，机械手安装在三自由度机械臂3端部，用于抓取和放置物体；舵机安装架2为左右对称结构，用于支撑各机械部件，三自由度机械臂3设于系统实施平台的前端，平衡机构4设于系统实施平台的后端；飞行控制器7为控制四旋翼飞行器在三维空间中基本运动，调节飞行姿态的模块，固定于四旋翼飞行器机体中心正上方。

如图2所示，以四旋翼飞行器的形心O_b为整个空中作业系统的原点，建立载体坐标系{O_b}-x_by_bz_b；三自由度机械臂3的安装方向为x轴正方向，z轴正方向垂直x轴向上，由右手系确定y轴正方向；三自由度机械臂3和平衡机构4的具体组成及所建立的坐标系如图3和图4所示，图3中，机械臂3包括第一连杆3-1、第二连杆3-2、机械手3-3和关节部分；关节部分由第一舵机3-4，第二舵机3-5和第三舵机3-6组成，其运动角度分别为θ₁，θ₂，θ₃，第一舵机3-4用于将机械臂固定于舵机安装架2，并控制第一连杆3-1在x_bOz_b平面的运动，第二舵机3-5安装在第一连杆3-1末端，用于连接第一连杆3-1和第二连杆3-2，并控制第二连杆3-2在x_bOz_b平面运动，第三舵机安装在第二连杆末端，用于控制机械手3-3腕部转动，其运动平面垂直于x_bOz_b平面；图4中，平衡机构4由第四舵机4-1、配重支架4-2和两个安装在配重支架4-2上并关于配重支架4-2对称布置的配重块4-3组成，第四舵机4-1的运动角度为θ_c，考虑到系统整体的质量分布，配重块4-3由锂电池充当。

如图5所示，操作者在PC机上设定机械臂运动目标角度

通过无线通信模块传送到舵机控制器6，舵机控制器6产生指令，发送至三自由度机械臂3的各关节舵机：第一舵机3-4，第二舵机3-5和第三舵机3-6，运动至所设角度；同时各关节舵机的运动角度H＝[θ₁,θ₂,θ₃]^T反馈至舵机控制器6，通过平衡控制算法计算出平衡机构4的第四舵机4-1所需转动的目标角度发送执行，从而平衡三自由度机械臂3运动所产生的扰动。

如图6所示，首先系统上电初始化，H和θ_c为初始平衡状态下三自由度机械臂3的各关节的角度和平衡机构4关节的角度。舵机控制器6判断是否接收到新的设定目标角度H^*(t)，如果没有则进入等待模式，如果收到新目标角度则判断距离上次接收到控制信号的时间差，如果时间差大于设定值τ，则更新H，三自由度机械臂3按照目标角度运动。由平衡控制方法计算出平衡机构4的目标角度

平衡机构关节舵机4-1接收到的控制器6发出的命令，运动目标角度，同时更新现在的机械臂3和平衡机构4的位置信息，进入等待模式。

图6中利用平衡控制算法计算得到平衡机构4运动角度的详细实施步骤如下：

步骤1：由图2确定空中作业平台各部分重心在其坐标系中的位置坐标

具体包括如下参量：

第一连杆3-1重心位置，Link1_pos＝[96.5,0,0,1]^T，连杆1质量Link1_m＝107；

第二连杆3-2重心位置，Link2_pos＝[7,-82.5,0,1]^T，连杆2质量Link2_m＝98；

机械手3-3重心位置，Gripper_pos＝[0,0,55.5,1]^T，机械手质量Gripper_m＝108；

平衡机构4重心位置，BalanceM_pos＝[105.5,0,0,1]^T，平衡机构质量BalanceM_m＝521；

简化计算设定抓取目标在机械手3-3抓取中心，Target_pos＝[0,0,0,1]^T,目标质量Target_m＝m，以上长度单位mm，质量单位为g；

步骤2：计算各部分坐标系到前一坐标系的转换矩阵

上述各部分指舵机安装架2、第一舵机3-4、第二舵机3-5、第三舵机3-6和机械手3-3；

设A₀为机械臂舵机安装架2坐标系到{O_b}转换矩阵：

同理可得：

A₁＝Transform(0,0,0,θ₁)为第一舵机3-4安装位置到舵机安装架2坐标系的转换矩阵；

A₂＝Transform(0,130,0,θ₂)为第二舵机3-5安装位置到第一舵机3-4安装位置的转换矩阵；

A₃＝Transform(90,0,109.5,θ₃)为第三舵机3-6安装位置到第二舵机3-5安装位置的转换矩阵；

A_t＝Transform(0,0,113.5,0)为机械手3-3抓取中心O_t的坐标系到舵机第三3-6安装位置的矩阵。

其中，

步骤3：计算得到各部分坐标系上重心转换到载体坐标系{O_b}上的转换矩阵

T₀＝A₀为在载体坐标系{O_b}的转换矩阵，

T₁＝T₀*A₁为第一连杆3-1上的点在载体坐标系{O_b}的转换矩阵，

T₂＝T₁*A₂为第二连杆3-2上的点在载体坐标系{O_b}的转换矩阵，

T₃＝T₂*A₃为机械手3-3上的点在载体坐标系{O_b}的转换矩阵，

T_t＝T₃*A_t为抓取物体在载体坐标系{O_b}的转换矩阵。

各部分的重心在载体坐标系的位置如下：

PosG_Link1＝T₁*Link1_pos；

PosG_Link2＝T₂*Link2_pos；

PosG_Gripper＝T₃*Gripper_pos；

PosG_Target＝T_t*Target_pos；

步骤4：计算平衡机构运动角度θ_c

将上述步骤所得数据带入平衡方程可计算出平衡机构4所需要运动的角度θ_c；

∑m_ix_i+m_cx_c+m_tx_t＝0

其中，x_i，x_c，x_t分别为三自由度机械臂3各部分、平衡机构4以及被抓取物体的重心在载体系{O_b}中的为位置，令sum_c＝m_cx_c，代入数据后有：

Link1_m*PosG_Link1(1,1)

+Link2_m*PosG_Link2(1,1)

+Gripper_m*PosG_gripper(1,1)

+Target_m*PosG_Target+sum_c＝0

令

则θ_c＝57.3*arcsin(1-a²)/2，考虑实际情况，通过程序设定平衡机构的运动范围θ_c∈(-45°,90°)。

如上所述，尽管参照特定的优选实施例已经表述和阐明了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims

1.一种搭载机械臂的四旋翼飞行器平衡作业控制方法，其特征在于，包括如下步骤：

平衡机构接收到的舵机控制器发出的命令，运动目标角度；系统更新机械臂和平衡机构的位置信息；

通过平衡控制算法计算得到平衡机构的目标角度

具体步骤如下：

步骤(3.4)：将上述步骤所得数据带入平衡方程：

∑m_ix_i+m_cx_c+m_tx_t＝0

得到平衡机构所需要运动的目标角度

2.根据权利要求1所述的四旋翼飞行器平衡作业控制方法，其特征在于：所述机械臂为三自由度机械臂，包括第一连杆、第二连杆和用于驱动连杆运动的关节部分；所述关节部分包括用于驱动第一连杆的第一舵机、用于驱动第二连杆的第二舵机和用于驱动机械手动作的第三舵机，各关节舵机的运动角度分别为θ₁，θ₂，θ₃。

3.根据权利要求1所述的四旋翼飞行器平衡作业控制方法，其特征在于：所述平衡机构包括一配重支架、用于驱动所述配重支架运动的第四舵机，以及两个对称布置在配重支架端部的配重块，所述第四舵机的运动角度为θ_c。

4.根据权利要求3所述的四旋翼飞行器平衡作业控制方法，其特征在于：所述配重块为锂电池。

5.根据权利要求1所述的四旋翼飞行器平衡作业控制方法，其特征在于：所述系统实施平台还包括摄像头。