CN118664604A - 一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法 - Google Patents

Abstract

本发明提供一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，以解决机械臂快速移动给无人机基座带来的动力学耦合干扰影响，可显著提升飞行机械臂的控制精度。首先根据齐次变换原理构建飞行机械臂执行器末端位姿在惯性坐标系下的运动学方程，然后利用动量守恒原理构建飞行机械臂全状态的耦合动力学模型，其次设计鲁棒微分器获得飞行机械臂的全状态信息，并分别针对无人机平台位置环和姿态环耦合动力学的估计误差设计相应的残差观测器，最后根据级联控制原理建立无人机平台的位置环抗干扰控制器和姿态环抗干扰控制器以完成飞行机械臂系统在动力学强耦合干扰下的高精度运动控制。本发明能够显著改善飞行机械臂在动态强耦合干扰下的控制精度。

Description

一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法

技术领域

本发明属于飞行机器人控制领域，具体涉及一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，适用于需要执行高精度主动操作任务的飞行机械臂控制系统。

背景技术

近年来，以多旋翼作为驱动机构的小型无人机在灾害预警、地质勘察、应急救援等国防、电力、航拍等领域得到了深入广泛的应用，但大多只能局限在非接触式的信息感知领域。目前随着机械设计，高精度传感器以及先进控制理论的不断发展，一种由多旋翼无人机和机械臂组成可以与环境进行物理交互的飞行机械臂已经成为一个非常热门的领域。随着任务和场景的逐渐复杂，对飞行机械臂的控制精度提出了更高要求，但是与传统的裸多旋翼相比，飞行机械臂携带有可以执行复杂作交互任务的机械臂。机械臂的快速移动使得无人机与机械臂间的耦合动力学变得非常复杂，导致整个飞行机械臂系统的质心和惯量发生变化，从而严重影响飞行机械臂系统的控制精度甚至威胁到系统本身的安全。因此，为了保证飞行机械臂的高精度控制性能，提升主动操作任务的成功率，使得飞行机械臂能够完成高压电塔维修等精准交互任务，基于全状态耦合动力学的飞行机械臂抗干扰控制方法在设计过程中必须解决上述提到的机械臂快速运动引起的动态强耦合干扰问题。

中国发明专利CN201610389286.6设计了一种将微分几何与LQR相结合的控制算法来处理空间机械臂的干扰，但存在两个问题：（1）建立的动力学模型没有考虑重力，控制器仅在失重环境下适用；（2）机械臂自由度较少大大局限了其操作能力；中国发明专利CN201810010602.3提出了一种搭载并联型机械臂的带臂无人机，旋翼飞行器下方设有并联型机械臂，但其提出的装置存在工作空间较小，只能工作在旋翼飞行器的下方的问题；中国发明专利CN202123017099.0提出的旋翼飞行抓取机械臂也存在工作空间小，无法完成主动操作任务的问题。

因此，上述的方法都没有考虑存在动态强耦合干扰时实现飞行机械臂抗干扰控制，以完成高精度主动操作任务。

发明内容

为克服现有方法的缺陷，对于以多旋翼无人机和多自由度机械臂组成的飞行机器人系统，本发明提供一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，能够保证飞行机械臂在动态强耦合干扰下的高精度控制，以完成各类主动式操作任务。

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

一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，包括以下步骤：

第一步、根据齐次变换原理构建飞行机械臂执行器末端位姿在惯性坐标系下的运动学方程;

第二步、利用动量守恒原理构建飞行机械臂全状态的耦合动力学模型；

第三步，设计微分器获得飞行机械臂的全状态信息；

第四步、分别针对无人机平台位置环和姿态环耦合干扰的估计误差设计相应的耦合干扰力和耦合干扰力矩残差观测器；

第五步、根据级联控制原理并结合构建的耦合干扰力残差观测器建立无人机平台的位置环抗干扰控制器；

第六步、结合构建的耦合干扰力矩残差观测器建立姿态环抗干扰控制器以完成飞行机械臂系统在动力学强耦合干扰下的高精度运动控制。

进一步地，所述第一步具体步骤如下：

针对飞行机械臂的运动特点，根据齐次变换原理建立飞行机械臂执行器末端位姿的运动学模型，表示如下：式中，表示飞行机械臂执行器末端在世界系下的位置，是飞行机械臂执行器末端在世界系下的旋转矩阵；表示无人机基座质心在世界系下的位置，是无人机质心在世界系下的旋转矩阵；表示机械臂执行器末端在无人机坐标系下的位置，表示机械臂执行器末端在无人机坐标系下的旋转矩阵。

进一步地，所述第二步具体步骤如下：

根据牛顿第二定律，考虑耦合效应的空中平台动力学可以表示为：式中，表示无人机平台质心在世界系下的速度；为无人机平台的运动角速度；为飞行机械臂的质量；为当地的重力加速度；为单位向量；为无人机平台的升力在惯性系下的投影向量；为无人机平台的升力；为机械臂运动造成的耦合干扰力；为无人机平台的转动惯量；为无人机平台的力矩；为机械臂运动造成的耦合干扰力矩；为执行叉乘操作的反对称矩阵算子。

机械臂快速运动造成的动态耦合效应由两部分组成:机械臂相对于无人机平台的运动和无人机平台自身的运动，表示如下：式中，，和分别表示机械臂质心在无人机本体坐标系下的位置,速度和加速度；和分别表示机械臂相对于无人机本体坐标系的惯性张量及其微分；和分别表示机械臂相对于无人机本体坐标系的角动量及其微分；表示无人机平台的运动角加速度；表示机械臂的质量。

进一步地，所述第三步具体步骤如下：

设计微分器获得飞行机械臂的全状态信息，并分别针对无人机平台位置环和姿态环耦合动力学的估计误差设计相应的残差观测器：

构建如下微分器获得和，表示如下：式中，代表采样时间，代表当前控制时刻。计算动态耦合干扰所需的其他状态量均可由传感器直接测量，因此利用微分器可以定量计算机械臂运动造成的动力学耦合干扰力和力矩。

进一步地，所述第四步具体步骤如下：

考虑惯性参数不确定性引起的估计误差，耦合扰动可重写为：式中，和分别表示根据全状态耦合模型直接计算的干扰力和干扰力矩。和分别表示耦合干扰力和力矩的残差估计值。

构建耦合干扰力和力矩的残差观测器进一步提升飞行机械臂的控制精度。

耦合干扰力的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力残差观测器的估计增益矩阵。

耦合干扰力矩的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力矩残差观测器的估计增益矩阵。

进一步地，所述第五步具体步骤如下：

建立无人机平台的位置环抗干扰控制器，首先定义位置跟踪误差，表示无人机平台在惯性系下的期望位置。定义辅助变量，其中为设计的稳定函数，表示如下：式中，表示无人机平台在惯性系下的期望速度，为待设计增益矩阵。

结合，，和，设计的无人机平台位置环控制器表示如下：式中，为待设计增益矩阵。

进一步地，所述第六步具体步骤如下：

建立无人机平台的姿态环抗干扰控制器，首先定义姿态跟踪误差和角速度跟踪误差，表示如下：式中，表示无人机平台的期望角速度，表示无人机平台的期望旋转矩阵，表示反对称矩阵算子的逆运算。

结合和，设计的无人机平台姿态环环控制器表示如下：式中，表示无人机平台的期望角加速度。和代表待设计的姿态环控制参数。

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

本方法针对机械臂快速运动引起的动态强耦合干扰，首先根据齐次变换原理构建飞行机械臂执行器末端位姿在惯性坐标系下的运动学方程，然后利用动量守恒原理构建飞行机械臂全状态的耦合动力学模型，其次设计鲁棒微分器获得飞行机械臂的全状态信息，并分别针对无人机平台位置环和姿态环耦合动力学的估计误差设计相应的残差观测器，最后根据级联控制原理建立无人机平台的位置环抗干扰控制器和姿态环抗干扰控制器以完成飞行机械臂系统在动力学强耦合干扰下的高精度运动控制。本方法能够显著改善飞行机械臂在动态强耦合干扰下的控制精度，可用于高压线异物清理，高空雕塑检测和修复等高精度特种作业任务。

附图说明

图1为本发明一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法。

具体实施方式

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

本发明以一类通用的由多旋翼无人机和多自由度机械臂组成的飞行机械臂系统为例来说明系统和方法的具体实现，飞行机械臂在执行高精度的主动操作任务时，对系统的精度和自身的安全性具有很高要求。

如图1所示，本发明的基于全状态耦合动力学的飞行机械臂抗干扰控制方法具体实施步骤如下：

第一步，针对飞行机械臂的运动特点，根据齐次变换原理建立飞行机械臂执行器末端位姿的运动学模型，表示如下：式中，表示飞行机械臂执行器末端在世界系下的位置，是飞行机械臂执行器末端在世界系下的旋转矩阵；表示无人机基座质心在世界系下的位置，是无人机质心在世界系下的旋转矩阵；表示机械臂执行器末端在无人机坐标系下的位置，表示机械臂执行器末端在无人机坐标系下的旋转矩阵。

第二步，根据牛顿第二定律，考虑耦合效应的空中平台动力学可以表示为：式中，表示无人机平台质心在世界系下的速度；为无人机平台的运动角速度；为飞行机械臂的质量；为当地的重力加速度；为单位向量；为无人机平台的升力在惯性系下的投影向量；为无人机平台的升力；为机械臂运动造成的耦合干扰力；为无人机平台的转动惯量；为无人机平台的力矩；为机械臂运动造成的耦合干扰力矩；为执行叉乘操作的反对称矩阵算子。

机械臂快速运动造成的动态耦合效应由两部分组成:机械臂相对于无人机平台的运动和无人机平台自身的运动，表示如下：式中，，和分别表示机械臂质心在无人机本体坐标系下的位置,速度和加速度；和分别表示机械臂相对于无人机本体坐标系的惯性张量及其微分；和分别表示机械臂相对于无人机本体坐标系的角动量及其微分；表示无人机平台的运动角加速度；表示机械臂的质量。

第三步，设计微分器获得飞行机械臂的全状态信息。构建如下微分器获得和表示如下：式中，代表采样时间，代表当前控制时刻。计算动态耦合干扰所需的其他状态量均可由传感器直接测量，因此利用微分器可以定量计算机械臂运动造成的动力学耦合干扰力和力矩。

第四步，考虑惯性参数不确定性引起的估计误差，耦合扰动可重写为：式中，和分别表示根据全状态耦合模型直接计算的干扰力和干扰力矩。和分别表示耦合干扰力和力矩的残差估计值。

构建耦合干扰力和力矩的残差观测器进一步提升飞行机械臂的控制精度。

耦合干扰力的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力残差观测器的估计增益矩阵。

耦合干扰力矩的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力矩残差观测器的估计增益矩阵。

第五步、建立无人机平台的位置环抗干扰控制器，首先定义位置跟踪误差，表示无人机平台在惯性系下的期望位置。定义辅助变量，其中为设计的稳定函数，表示如下：式中，表示无人机平台在惯性系下的期望速度，为待设计增益矩阵。

结合，，和，设计的无人机平台位置环控制器表示如下：式中，为待设计增益矩阵。

第六步，建立无人机平台的姿态环抗干扰控制器，首先定义姿态跟踪误差和角速度跟踪误差，表示如下：式中，表示无人机平台的期望角速度，表示无人机平台的期望旋转矩阵，表示反对称矩阵算子的逆运算。

结合和，设计的无人机平台姿态环环控制器表示如下：式中，表示无人机平台的期望角加速度。和代表待设计的姿态环控制参数。

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

Claims (7)

1.一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，包括以下步骤：

第一步、根据齐次变换原理构建飞行机械臂执行器末端位姿在惯性坐标系下的运动学方程;

第二步、利用动量守恒原理构建飞行机械臂全状态的耦合动力学模型；

第三步，设计微分器获得飞行机械臂的全状态信息；

第四步、分别针对无人机平台位置环和姿态环耦合干扰的估计误差设计相应的耦合干扰力和耦合干扰力矩残差观测器；

第五步、根据级联控制原理并结合构建的耦合干扰力残差观测器建立无人机平台的位置环抗干扰控制器；

第六步、结合构建的耦合干扰力矩残差观测器建立姿态环抗干扰控制器以完成飞行机械臂系统在动力学强耦合干扰下的高精度运动控制。

2.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第一步具体如下：

针对飞行机械臂的运动特点，根据齐次变换原理建立飞行机械臂执行器末端位姿的运动学模型，表示如下：式中，表示飞行机械臂执行器末端在世界系下的位置，是飞行机械臂执行器末端在世界系下的旋转矩阵；表示无人机基座质心在世界系下的位置，是无人机质心在世界系下的旋转矩阵；表示机械臂执行器末端在无人机坐标系下的位置，表示机械臂执行器末端在无人机坐标系下的旋转矩阵。

3.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第二步具体如下：

根据牛顿第二定律，考虑耦合效应的空中平台动力学表示为：式中，表示无人机平台质心在世界系下的速度；为无人机平台的运动角速度；为飞行机械臂的质量；为当地的重力加速度；为单位向量；为无人机平台的升力在惯性系下的投影向量；为无人机平台的升力；为机械臂运动造成的耦合干扰力；为无人机平台的转动惯量；为无人机平台的力矩；为机械臂运动造成的耦合干扰力矩；为执行叉乘操作的反对称矩阵算子；

机械臂快速运动造成的动态耦合效应由两部分组成:机械臂相对于无人机平台的运动和无人机平台自身的运动，表示如下：式中，，和分别表示机械臂质心在无人机本体坐标系下的位置,速度和加速度；和分别表示机械臂相对于无人机本体坐标系的惯性张量及其微分；和分别表示机械臂相对于无人机本体坐标系的角动量及其微分；表示无人机平台的运动角加速度；表示机械臂的质量。

4.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第三步具体如下：

设计微分器获得飞行机械臂的全状态信息，并分别针对无人机平台位置环和姿态环耦合动力学的估计误差设计相应的残差观测器：

构建如下微分器获得和，表示如下：式中，代表采样时间，代表当前控制时刻。

5.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第四步具体如下：

考虑惯性参数不确定性引起的估计误差，耦合扰动重写为：式中，和分别表示根据全状态耦合模型直接计算的干扰力和干扰力矩，和分别表示耦合干扰力和力矩的残差估计值；

耦合干扰力的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力残差观测器的估计增益矩阵；

耦合干扰力矩的残差观测器表示如下：式中，代表残差的估计值，为辅助变量，为耦合力矩残差观测器的估计增益矩阵。

6.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第五步具体如下：

建立无人机平台的位置环抗干扰控制器，首先定义位置跟踪误差，表示无人机平台在惯性系下的期望位置，定义辅助变量，其中为设计的稳定函数，表示如下：式中，表示无人机平台在惯性系下的期望速度，为待设计增益矩阵；

结合，，和，设计的无人机平台位置环控制器表示如下：式中，为待设计增益矩阵。

7.根据权利要求1所述的一种基于全状态耦合动力学的飞行机械臂抗干扰控制方法，其特征在于，所述第六步具体如下：

建立无人机平台的姿态环抗干扰控制器，首先定义姿态跟踪误差和角速度跟踪误差，表示如下：式中，表示无人机平台的期望角速度，表示无人机平台的期望旋转矩阵，表示反对称矩阵算子的逆运算；

结合和，设计的无人机平台姿态环环控制器表示如下：式中，表示无人机平台的期望角加速度，和代表待设计的姿态环控制参数。