CN115657474A - 一种针对人机协同运输的飞行机械臂柔性交互控制方法 - Google Patents

Abstract

本发明提供一种针对人机协同运输的飞行机械臂柔性交互控制方法，以解决人机交互外力及无人机的动力学特性影响飞行机械臂控制精度和飞行安全，首先构建含有人机交互外力及无人机动力学特性的飞行机械臂耦合动力学方程，其次，设计以机械臂末端夹爪为接触点的交互外力估计器，再次，针对交互外力和不同任务所需的柔性策略，搭建导纳滤波器以重构机械臂末端的期望位置；然后，设计模型预测控制将机械臂末端的期望位置映射到无人机和每个关节舵机的轨迹；本发明能够显著改善多旋翼飞行机械臂在人机协同作业中的交互刚度，可用于机器人领域特种作业无人机、飞行机械臂等多旋翼无人系统的人机交互和协同作业。

Description

一种针对人机协同运输的飞行机械臂柔性交互控制方法

技术领域

本发明属于飞行机器人控制领域，具体涉及一种针对人机协同运输的飞行机械臂柔性交互控制方法，适用于搭载多自由度机械臂执行人机交互或环境交互任务且需要实现稳定安全控制的飞行机械臂控制系统。

背景技术

近年来随着电子技术和材料工艺的不断提升和发展，尤其是以多旋翼作为驱动机构小型无人机为代表的空中机器人在灾害预警、地质勘察、应急救援等国防、电力、航拍等领域得到了深入广泛的应用，需求越来越大，任务复杂度也越来越高，目前的小型无人机大多应用非接触式信息感知领域，在面对诸如应急救援救灾等目前亟需与环境进行物理交互的场合中，空中机器人被期待能够对特定的目标施加力/力矩以改变周围的环境施加影响。因此，搭载三维云台、或者多自由度机械臂等各种任务载荷的飞行机械臂逐渐丰富，随着任务作业任务范围的不断扩展，飞行机器人需要携带更大容量的电池和更大推力的螺旋桨，但是为了能够执行环境交互任务，相应的机械臂需要更长的关节以伸出螺旋桨的转动范围，更长的关节又进一步增加了机器人的自身重量，会降低作业时间和任务执行范围。因此，上述飞行机器人所搭载的机械臂负载大部分为多自由度轻质材料组成的小型机械臂，同时提供稳定基座的飞行平台大多为可以垂直起降、保持悬停的多旋翼小型无人机。

在与环境甚至人类进行物理交互场景中，如：桥梁探测、管道检修、人机协同运输作业时，为了交互对象和自身的安全，飞行机器人需要在交互过程中展示一定的柔性，而且根据任务的不同进行调节柔顺度。

对于小型多旋翼无人机，由于其平动和转动耦合的动力学特性，不能同时控制其姿态和位置。相对于普通的飞行机器人，飞行机械臂包含了多旋翼无人机和多关节机械臂，因此，必须避免机械臂运动的时候与无人机机体发生碰撞。这对飞行机械臂的轨迹运动规划提出了更多高的要求。此外，由于无人机和机械臂之间刚性联接为一个整体，那么机械臂的装配首先会改变原来多旋翼无人机的质心位置和惯量分布等基本物理属性参数；其次，小型多旋翼无人机作为机械臂的基座为机械臂的运动提供足够支撑力，那么机械臂的反作用力会对多旋翼无人机的飞行安全和稳定性造成损害，所以需要对机械臂的运动轨迹加以约束。

因此，为了进一步增强人机协同作业任务中的安全性，并提升机器人展示的柔顺度，使空中飞行机器人尤其是飞行机械臂能够具有和人类协同作业的能力，完成协同运输等任务，飞行机器人的规划控制算法在设计过程中必须解决上述提到的交互外力感知和柔性轨迹规划以及机械臂末端逆运动学求解问题。

中国发明CN202110148055.7提出了一种基于鸽群优化控制的带臂四旋翼无人机系统及控制方法，但存在两个问题：(1)其使用反步设计方法对二自由度机械臂连杆关节进行控制；(2)并未考虑与环境存在物理接触甚至动态交互的情况；中国发明专利CN202120279837.X提出了一种搭载并联机械臂飞行器，旋翼飞行器下方设有并联机械臂组件，其提出的装置和控制方法也存在类似的问题：(1)其并联机械臂的工作空间只存在于多旋翼飞行器的下方；(2)也没有考虑与外力交互的情况。申请号202110248852.2提出了一种基于操作员意图识别的飞行机械臂抓取作业遥操作方法，同样地，没有考虑存在环境交互外部力的情况。因此，上述的方法都没有考虑存在外界环境交互力的情况下实现人机协同作业任务。

发明内容

未克服现有方法的缺陷，对于以多旋翼小型无人机搭载多自由度轻型机械臂的空中机器人，本发明提供一种针对人机协同运输的飞行机械臂柔性交互控制方法，能够为以多旋翼螺旋桨作为推进机构的飞行机器人提供安全、稳定的飞行能力，实现在动态地外力交互过程中飞行机器人能够展示出指定的柔顺度。

为达到上述目的，本发明的技术解决方案为：

一种针对人机协同运输的飞行机械臂柔性交互控制方法，包括以下步骤：

步骤1、构建含有人机交互外力及无人机动力学特性的飞行机械臂耦合动力学方程，具体包括：

针对飞行机械臂飞行机械臂的多自由度机械臂特点，并同时考虑作为机械臂浮动基座的无人机飞行平台具有多旋翼螺旋桨驱动特性，建立含有交互外力耦合以及多自由度机械臂特性的多旋翼飞行机器人耦合动力学模型，表示如下：

式中，

表示飞行机械臂的质心平动加速度，m_s是飞行机械臂的总重量；f_t表示多旋翼驱动机构的总推力，R_b是旋转矩阵，e₃＝[0 0 1]^T表示单位向量，F_ext＝[F_x F_y F_z]^T分别表示飞行机械臂在X-Y-Z轴上收到的交互外力，g表示重力加速度；M_η是飞行机械臂的正定惯性矩阵，

是飞行机械臂的角加速度，C_η是飞行机械臂的科里奥利矩阵，

是飞行机械臂的角速度；τ_b表示多旋翼驱动机构产生的总扭矩，τ_ext表示飞行机械臂受到的交互外力矩；另外，T_b表示机体坐标系到惯性坐标系的变换矩阵，其计算公式如下：

式中，[φ θ ψ]^T＝η_b分别表示飞行机械臂的姿态角，其中φ表示无人机的俯仰角，θ表示无人机的滚转角，ψ表示无人机的偏航角。

步骤2、设计以机械臂末端夹爪为接触点的交互外力估计器，具体包括：

当飞行机械臂在执行人机协同交互作业时，构建交互力观测器的输出

其中，

表示交互外力的估计值，

表示交互外力矩的估计值。

进一步，构建交互力观测器，形式如下：

式中，t表示时间，χ(t)表示机器人的动量，a(t)表示无人机的运动和转动的加速度，K₁和K₂表示正定对角矩阵为可调参数；

表示飞行机械臂的质心平动速度；I₃是3x3的单位矩阵，O₃是3x3的零矩阵；

步骤3、针对交互外力和不同任务所需的柔性策略，搭建导纳滤波器以重构机械臂末端的期望位置，具体包括：

对于不同的交互任务或者行为，需要飞行机械臂能够调节不同的柔性交互策略，设计可变参数的导纳滤波器，形式如下：

式中，导纳滤波器的输出为υ_r＝[υ_x υ_y υ_z]^T，其中，υ_x、υ_y、υ_z分别表示交互接触点沿X-Y-Z轴的位移，υ_o表示飞行机械臂的机械臂末端在与环境交互点的当前位置；M_d是期望惯量参数，D_d是期望阻尼参数，K_d是期望刚度系数；E_o-r表示经过导纳滤波器重构之后的机械臂末端位置，即末端处于该位置时能够向外展示希望的柔顺度，

表示E_o-r一阶导数，

表示E_o-r的二阶导数；通过调节不同的M_d、D_d、K_d数值能够改变飞行机械臂在人机协同作业时的柔顺度；

步骤4、设计模型预测控制器，将机械臂末端的期望位置映射到无人机和每个关节舵机的轨迹，具体包括：

对飞行机械臂进行运动学状态空间建模，形式如下：

式中，

表示飞行机械臂的状态变量h及其导数

h代表无人机的平动位移(3维)和关节角度(4维)；

表示无人机平动加速度和关节的加速度；y表示状态空间的输出；另外，A、B、C的取值如下：

式中，Δt为采样时间间隔，I₇表示7x7的单位矩阵，0₇表示7x7的零矩阵；

进一步设计模型预测控制器代价函数，同时考虑无人机机架与机械臂碰撞避免、机械臂关节自身碰撞避免、执行器输出饱和等多种约束，形式如下：

式中，u(j)表示输入变量，j表示模型预测控制器中的控制域长度，min表示通过优化输入变量u(j)达到最小化优化目标J_mpc，J_mpc表示代价函数的输出，U表示模型预测控制的输入矩阵；W_e是终端状态的权重矩阵，E_ter(i)为终端位置误差，i表示模型预测控制器中的预测域长度；W_u表示输入权重矩阵，为可调参数，控制无人机和机械臂的规划权重；

讲一步，模型预测控制满足的约束条件如下：

式中，u_min和u_max表示执行器的饱和约束，x_min和x_max表示机器人的状态约束；

从而完成人机协同交互下飞行机械臂安全稳定控制并实现协同运输作业。

本发明与现有技术相比的优点在于：

本发明中涉及的一种针对人机协同运输的飞行机械臂柔性交互控制方法，主要面向采用多螺旋桨和舵机作为主要执行机构的飞行机械臂系统，系统主要由小型多旋翼无人机作为飞行平台，采用多自由度机械臂作为任务负载。同时考虑存在环境交互外力任务，使得对于多自由度机械臂的轨迹规划更加合理，具有更广泛的使用面。此外针对动态地外力交互，飞行平台使用了外力观测器进行外力估计，能够快速地估计交互外力的变化趋势，然后根据不同的任务调节飞行机器人对外展示的交互刚度，增加了飞行机器人的任务执行能力，最后使用了模型预测控制进行飞行机械臂系统的逆运动学规划，显著提高飞行机械臂末端位置控制的精度与稳定度。

附图说明

图1为本发明一种针对人机协同运输的飞行机械臂柔性交互控制方法的设计流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明以一类通用搭载多自由度轻型机械臂的多旋翼小型无人机的空中机器人系统为例来说明具体实现，飞行机械臂在与外界环境交互过程中，对外力和接触点的位置关系以及多自由度机械臂轨迹规划的稳定度具有很高要求。

如图1所示，本发明的针对人机协同运输的飞行机械臂柔性交互控制方法具体实施步骤如下：

步骤1、建立含有人机交互外力及无人机动力学特性的飞行机械臂耦合动力学方程：

针对飞行机械臂的多自由度机械臂特点，并同时考虑作为机械臂基座的无人机飞行平台的多旋翼螺旋桨驱动特性，建立含有交互外力耦合以及多自由度机械臂的多旋翼飞行机器人耦合动力学模型，表示如下：

式中，

表示飞行机械臂的质心平动加速度，m_s是飞行机械臂的总重量；f_t表示多旋翼驱动机构的总推力，R_b是旋转矩阵，e₃＝[0 0 1]^T表示单位向量，F_ext＝[F_x F_y F_z]^T分别表示飞行机械臂在XYZ轴上收到的交互外力，g表示重力加速度；M_η是飞行机械臂的正定惯性矩阵，

是飞行机械臂的角加速度，C_η是飞行机械臂的科里奥利矩阵，

是飞行机械臂的角速度；τ_b表示多旋翼驱动机构产生的总扭矩，τ_ext表示飞行机械臂受到的交互外力矩；另外，T_b表示机体坐标系到惯性坐标系的变换矩阵，其计算公式如下：

式中，[φ θ ψ]^T＝η_b分别表示飞行机械臂的姿态角，其中φ表示无人机的俯仰角，θ表示无人机的滚转角，ψ表示无人机的偏航角。

步骤2、设计以机械臂末端夹爪为接触点的交互外力估计器：

当飞行机械臂在执行人机协同交互作业时，构建交互力观测器的输出

其中，

表示交互外力的估计值，

表示交互外力矩的估计值。

进一步，构建交互力观测器，形式如下：

式中，t表示时间，χ(t)表示机器人的动量，a(t)表示无人机的运动和转动的加速度，K₁和K₂表示正定对角矩阵为可调参数；

表示飞行机械臂的质心平动速度；I₃是3x3的单位矩阵，O₃是3x3的零矩阵；

适当地选取K₁和K₂，能够使得观测器避免速度和加速度噪声带来的影响，较为准确地输出飞行机械臂所受到的外力和外力矩及其变化趋势。

步骤3、搭建导纳滤波器以重构机械臂末端的期望位置：

对于不同的交互任务或者行为，需要飞行机械臂能够调节不同的柔性交互策略，设计可变参数的导纳滤波器，形式如下：

式中，导纳滤波器的输出为υ_r＝[υ_x υ_y υ_z]^T，其中，υ_x、υ_y、υ_z分别表示交互接触点沿X-Y-Z轴的位移，υ_o表示飞行机械臂的机械臂末端在与环境交互点的当前位置；M_d是期望惯量参数，D_d是期望阻尼参数，K_d是期望刚度系数；E_o-r表示经过导纳滤波器重构之后的机械臂末端位置，即末端处于该位置时能够向外展示希望的柔顺度，

表示E_o-r一阶导数，

表示E_o-r的二阶导数；通过调节不同的M_d、D_d、K_d数值能够改变飞行机械臂在人机协同作业时的柔顺度。

步骤4、设计模型预测控制器，将机械臂末端的期望位置映射到无人机和每个关节舵机的轨迹：

对飞行机械臂进行运动学状态空间建模，形式如下：

式中，

表示飞行机械臂的状态变量h及其导数

h代表无人机的平动位移(3维)和关节角度(4维)；

表示无人机平动加速度和关节的加速度；y表示状态空间的输出；另外，A、B、C的取值如下：

式中，Δt为采样时间间隔，I₇表示7x7的单位矩阵，0₇表示7x7的零矩阵；

进一步设计模型预测控制器代价函数，同时考虑无人机机架与机械臂碰撞避免、机械臂关节自身碰撞避免、执行器输出饱和等多种约束，形式如下：

式中，u(j)表示输入变量，j表示模型预测控制器中的控制域长度，min表示通过优化输入变量u(j)达到最小化优化目标J_mpc，J_mpc表示代价函数的输出，U表示模型预测控制的输入矩阵；W_e是终端状态的权重矩阵，E_ter(i)为终端位置误差，i表示模型预测控制器中的预测域长度；W_u表示输入权重矩阵，为可调参数，控制无人机和机械臂的规划权重；

进一步，模型预测控制满足的约束条件加下：

式中，u_min和u_max表示执行器的饱和约束，x_min和x_max表示机器人的状态约束，包含了飞行区域、自身碰撞避免等约束条件；不会超过无人机电机和机械臂舵机的极值，此外也能够保证飞行机械臂系统稳定，实现存在外力交互下柔性协同控制。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种针对人机协同运输的飞行机械臂柔性交互控制方法，其特征在于：包括以下步骤：

步骤1、构建含有人机交互外力及无人机动力学特性的飞行机械臂耦合动力学方程，具体包括：

针对飞行机械臂的多自由度机械臂特点，并同时考虑作为机械臂浮动基座的无人机飞行平台具有多旋翼螺旋桨驱动特性，建立含有交互外力耦合以及多自由度机械臂特性的多旋翼飞行机器人耦合动力学模型，表示如下：

式中，

表示飞行机械臂的质心平动加速度，m_s是飞行机械臂的总重量；f_t表示多旋翼驱动机构的总推力，R_b是旋转矩阵，e₃＝[0 0 1]^T表示单位向量，F_ext＝[F_x F_y F_z]^T分别表示飞行机械臂在X-Y-Z轴上收到的交互外力，g表示重力加速度；M_η是飞行机械臂的正定惯性矩阵，

是飞行机械臂的角加速度，C_η是飞行机械臂的科里奥利矩阵，

是飞行机械臂的角速度；τ_b表示多旋翼驱动机构产生的总扭矩，τ_ext表示飞行机械臂受到的交互外力矩；T_b表示机体坐标系到惯性坐标系的变换矩阵，其计算公式如下：

式中，[φ θ ψ]^T＝η_b分别表示飞行机械臂的姿态角，其中φ表示无人机的俯仰角，θ表示无人机的滚转角，ψ表示无人机的偏航角；

步骤2、设计以机械臂末端夹爪为接触点的交互外力估计器，具体包括：

当飞行机械臂在执行人机交互协同作业时，构建交互力观测器的输出

其中，

表示交互外力的估计值，

表示交互外力矩的估计值；

进一步，构建交互力观测器，形式如下：

式中，t表示时间，χ(t)表示机器人的动量，a(t)表示无人机的运动和转动的加速度，K₁和K₂表示正定对角矩阵为可调参数；

表示飞行机械臂的质心平动速度；I₃是3x3的单位矩阵，O₃是3x3的零矩阵；

步骤3、针对交互外力和不同任务所需的柔性策略，搭建导纳滤波器以重构机械臂末端的期望位置，具体包括：

对于不同的交互任务或者行为，需要飞行机械臂能够调节不同的柔性交互策略，设计可变参数的导纳滤波器，形式如下：

式中，导纳滤波器的输出为v_r＝[v_x v_y v_z]^T，其中，v_x、υ_y、υ_z分别表示交互接触点沿X-Y-Z轴的位移，υ_o表示飞行机械臂的机械臂末端在与环境交互点的当前位置；M_d是期望惯量参数，D_d是期望阻尼参数，K_d是期望刚度系数；E_o-r表示经过导纳滤波器重构之后的机械臂末端位置，即末端处于该位置时能够向外展示希望的柔顺度，

表示E_o-r一阶导数，

表示E_o-r的二阶导数；通过调节不同的M_d、D_d、K_d数值能够改变飞行机械臂在人机协同作业时的柔顺度；

步骤4、设计模型预测控制器，将机械臂末端的期望位置映射到无人机和每个关节舵机的轨迹，具体包括：

对飞行机械臂进行运动学状态空间建模，形式如下：

式中，

表示飞行机械臂的状态变量h及其导数

h代表无人机的3维平动位移和4维关节角度；

表示无人机平动加速度和关节的加速度；y表示状态空间的输出；A、B、C的取值如下：

式中，Δt为采样时间间隔，I₇表示7x7的单位矩阵，0₇表示7x7的零矩阵；

进一步设计模型预测控制器代价函数，同时考虑无人机机架与机械臂碰撞避免、机械臂关节自身碰撞避免、执行器输出饱和等多种约束，形式如下：

式中，u(j)表示输入变量，j表示模型预测控制器中的控制域长度，min表示通过优化输入变量u(j)达到最小化优化目标J_mpc，J_mpc表示代价函数的输出，U表示模型预测控制的输入矩阵；W_e是终端状态的权重矩阵，E_ter为终端位置误差，i表示模型预测控制器中的预测域长度；W_u表示输入权重矩阵，为可调参数，控制无人机和机械臂的规划权重；

进一步，模型预测控制满足的约束条件如下：

式中，u_min和u_max表示执行器的饱和约束，x_min和x_max表示机器人的状态约束；

从而完成人机协同交互下飞行机械臂安全稳定控制并实现协同运输作业。

Images