共轴无人机扩展卡尔曼姿态控制研究.pdf

研 究 设 计 装备机械 山东省教育科学“十四五”规划项目（编号：）收稿时间：第一作者简介：有德义（），男，博士研究生，讲师，主要研究方向为嵌入式开发、导航控制共轴无人机扩展卡尔曼姿态控制研究 有德义，郝永平 沈阳理工大学 研究生院沈阳 德州职业技术学院 电子与新能源技术工程系山东德州 摘要：针对小型共轴双旋翼飞行器姿态估计中传感器误差累计和随机噪声漂移等问题，进行共轴无人机扩展卡尔曼姿态控制研究，提出一种可用于小型共轴无人机的姿态估计扩展卡尔曼滤波算法。这一算法通过分析共轴无人机的机械振动、噪声、微机电系统传感器误差，建立飞行姿态运动模型和四元数姿态角数学模型，以陀螺仪和磁力计数据为状态变量，以加速度计数据为观测向量。试验结果表明，这一算法不但能够减小共轴无人机高速飞行过程中所产生的机械振动、噪声对姿态角的影响，而且可以显著提高横滚角和俯仰角的稳定性，进而有效提高共轴姿态估计的精度。：，关键词：无人机；卡尔曼滤波；姿态控制；研究 ：；中图分类号：文献标志码：文章编号：（）研究背景共轴双旋翼飞行器是一种上下桨同轴反向旋转且能够自主悬停的无人机，是近年来无人机研究的热点机型之一。共轴双旋翼飞行器结构与双旋翼直升飞机结构的不同之处在于没有尾旋翼平衡扭矩，具有外形尺寸小、气动效率高等优势，只需要通过上 研 究 设 计装备机械 下两对螺旋桨产生的推力来保持自身平衡，飞行姿态通过舵机连接倾斜盘舵平面与下旋翼旋转平面来调整。无人机采用齿轮传动方式，将动力从电机传送到旋翼，并且上下桨叶之间存在气流扰动，加之操纵机构的复杂性和控制的非线性等原因，会导致共轴双旋翼飞行器有较明显的机械振动和噪声。近年来，世界各国都对多传感器数据融合、滤波算法、姿态估计的问题更加关注，许多科研单位和院校对无人机姿态控制和导航做了较为深入的研究。我国对无人机姿态控制的研究起步较晚，共轴无人机姿态控制的研究还有待发展。国防科技大学、北京理工大学、沈阳理工大学等高校开展了对共轴无人机自主系统的研究，并且取得了较好的效果。吴和龙 在多旋翼无人机的低成本组合导航关键技术研究文章中提出基于多维状态量的高维扩展卡尔曼滤波数据融合算法，在此基础上，建立了一种基于 维状态量的高维扩展卡尔曼滤波数据融合方法，进行完整的数学模型模拟仿真试验，结果表明具有较高的准确率。沈跃等 在植保无人机航姿估计与分析中，采用磁力计实时校准的无人机航姿两级解算方法，这一方法考虑外界环境磁场干扰，采用无迹卡尔曼滤波器融合算法，通过四元数较好地解析出横滚角和俯仰角姿态信息。刘旭航等 通过卡尔曼滤波建立加速度误差模型，结合外部传感器信息，得到陀螺仪校正量，并通过计算得到的增益信息对陀螺的初始值进行校正，设计互补滤波器，滤去来自加速度计、磁强计、陀螺仪的低频噪声，从而消除传感器噪声信号对姿态估计的影响。苏黎世联邦理工大学的研究学者建立完整扩展卡尔曼滤波算法的多传感器数据融合开源框架，集成视觉传感器、全球导航卫星系统定位模块、激光雷达、超声波雷达等，达到精准的姿态定位效果 。无人机常见的导航估计算法有互补滤波、卡尔曼滤波、粒子滤波、神经网络滤波 。吴东旭 为了提高姿态角解算的精度，利用多传感器数据融合，分别设计互补滤波算法和卡尔曼滤波算法，并且串级比例积分微分姿态控制算法，写入嵌入式飞行控制系统中，进行飞行试验，但是在研究中并没有考虑共轴无人机自身的主要误差来源。笔者以小型折叠共轴双旋翼飞行器为研究对象，分析误差、振动的主要来源，建立姿态四元数模型，提出基于改进的扩展卡尔曼滤波的数据融合算法，并在此基础上进行载体姿态静态和动态试验对比。姿态运动模型 共轴无人机结构共轴无人机包括双旋翼、共轴机体、倾斜装置、电机、舵机、控制核心板等结构。控制核心板在共轴机体内部提供所有的控制信号。电机位于共轴机体上方，通过齿轮传动提供旋转动力，依靠两个旋翼反向转动的升力和自身重力产生合力，实现上下运动。舵机位于共轴机体的上部，采用连杆控制操控盘倾斜，进而带动下旋翼产生俯仰和横滚等姿态运动。共轴无人机简化模型如图 所示。图 共轴无人机简化模型 参考坐标系在忽略地球自传和地球质心曲线运动的情况下，建立共轴无人机数学模型。为更好地确定姿态、速度、加速度和外力力矩的分量，需要引入多种坐标系。机体坐标系是固连在无人机上并随无人机运动的一种动坐标系，地面坐标系转换到机体坐标系，需要经过 轴、轴、轴旋转。定义 为横滚角，为俯仰角，为航向角，无人机绕 轴转动后的旋转矩阵（）为：（）（）无人机绕 轴转动后的旋转矩阵（）为：（）（）无人机绕 轴旋转后的旋转矩阵（）为：（）（）研 究 设 计 装备机械 可以得到旋转矩阵 公式为：（）（）（）（）将式（）、式（）、式（）代入式（），推导机体坐标系到地面坐标系的旋转矩阵 表达式，为：（）四元数姿态解算四元数表示三维空间的旋转信息，通过构造四元数得到变换矩阵。四元数满足：（，）（）四元数的模为：（）如果满足 ，那么表明该四元数为规范四元数，有：（）（）（）（）（）（）（）（）式中：为旋转矢量绕旋转轴的转角；、为旋转矢量在 、虚轴上的投影。用四元数表示姿态的优势为在任意姿态下都不会出现奇异性 。传感器读取四元数，首先应将四元数归一化为单位四元数，对四元数获取的实时欧拉角信息进行姿态估计，每个时钟周期运算出新的四元数，更新姿态角，进而计算出新的横滚角、俯仰角、航向角全姿态信息。四元数（，）表示为：（，）（）（）（）（）（）（）（）用四元数来更新横滚角、俯仰角、航向角，有：（）（）（）（）（）（）（）（）载体上的三轴陀螺仪可以测得共轴无人机在导航坐标系中的角速度在机体坐标系中的分量。、为机体坐标系下的旋转角速度，通过四元数微分方程建立的状态方程 为：（）采用一阶龙格库塔法求解四元数微分方程，可以得到离散时间模型 （）为：（）（）（）（）（）式中：为系统采样时间间隔；（）为 时刻姿态四元数。设置初始四元数 （），利用式（）可以计算递推出 时刻四元数 （）的取值，进而能够更新姿态矩阵 。误差分析陀螺仪测量角速度具有高动态性，但测量的是角度的导数，再对时间积分才能得到角度。陀螺仪内部运动一段时间，噪声和偏置会导致数据偏差，需要借助其它传感器进行校正。建立多维状态量，包含姿态四元数（，）、导航系三维速度信息（，）、导航系三维位置信息（，）、三维陀螺仪角增量误差（，）、三维加速度计增量误差（，）、三维地磁信息（，）、三维地磁误差（，）、二维水平风速信息（，）。初始时刻四元数为：（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）（）共轴无人机的振动源主要有七个方面。第一，无刷电机通过齿轮传动装置带动上下旋翼，构成一个由电机运转、齿轮啮合等组成的机械扭转系统，进而产生机械振动。第二，上下旋翼在转动时会产生 研 究 设 计装备机械 周期的空气动力，通过桨毂和旋翼轴向机体传动。其中，旋翼振动是主要振源，原因是机械不均衡和周期的空气动力相互作用。第三，微机电系统传感器安装，以及机体坐标系不对称和标度带来的误差。第四，初始条件校准时，参数和当前姿态角误差。第五，四元数计算误差、滤波算法误差等。第六，微机电系统陀螺仪因自身设计缺陷而由高动态特性产生的漂移误差等。第七，机体运动和姿态估计延时产生的动态误差。为减小机体振动对微机电系统传感器的影响，应用补偿算法，并且在姿态解算中进行惯性测量单元传感器权重调整，同时还应该避开结构的自振频率与转子所引起的激振力频率。卡尔曼滤波姿态估计利用线性卡尔曼滤波对非线性系统的状态进行估计，采用泰勒展开式，使非线性系统在局部线性化，这样高斯分布的参数在线性变换后依然保持高斯分布，就可以使用标准卡尔曼滤波器的卡尔曼滤波结构。在非线性不强烈的情况下，扩展卡尔曼滤波器可以有效过滤掉非线性。在非线性较强烈的情况下，滤波效果会降低。建立状态方程为：（）（），）（）（）（）（），）（）（）式中：（）为 时刻系统状态矩阵；（）为 时刻系统观测矩阵；（），）、（），）分别为非线性离散系统 时刻预测状态矢量函数和 时刻测量状态函数；（）为 时刻系统状态噪声矩阵；（）为 时刻测量噪声矩阵。此处假设系统的状态量 （）为：（）（）（）（）（）（）（）（）（）（）中包括姿态四元数和三个转动轴陀螺仪随机漂移量。初始化状态，假设初值滤波器为：（）（）（）（）（）进行扩展卡尔曼滤波，时间预测更新估计为：（，）（）（）式中：为 时刻状态转移矩阵和测量矩阵的雅可比矩阵；为 时刻过程噪声协方差矩阵；为 对应的协方差矩阵。测量更新为：（）（）（）式中：是 时刻测量雅可比矩阵；为 时刻观测噪声协方差矩阵。计算 时刻卡尔曼增益 为：（）（）（）?（）（）由观测变量更新估计。更新误差协方差时，测量更新方程利用观测变量校正状态估计和协方差估计，为：，（）根据状态噪声和观测噪声的统计特性，只有系统的状态方程和观测方程都接近线性，扩展卡尔曼滤波才可以更接近真实值。测试平台共轴无人机飞行测试平台如图 所示，其中包含对共轴无人机升力、振动、旋翼转速等的测试。共轴无人机垂直飞行过程中，主要受到来自桨叶旋转时产生的空气动力和自身重力的影响 。支撑结构作为测试平台中最主要的构造部分，用于稳固机身，桨叶产生的气动力及机身受到的气动力都会通过机身结构间接或直接施加在支撑结构上。机身、传感器，以及其它构件的重力，也都会直接施加在支撑结构上 。共轴无人机在测试过程中，由机械结构产生的振动力同样会施加在支撑结构上 。测试平台通过姿态测量传感器进行静态实时稳定的姿态数据采集和测量，通过实时测量的横滚角、俯仰角、航向角信息，来分析主要误差来源。支撑结构作为承受共轴无人机空气动力及自身重力的主体部件，在受力方面比较复杂。共轴无人机不仅产生维持飞行的气动升力，而且产生在发生横滚、俯仰、航向运动时的力矩。由此，共轴无人机施加在支撑结构上的力主要可以分为升力，以及横研 究 设 计 装备机械 图 共轴无人机测试平台滚运动力、俯仰运动力、航向运动力。测试平台使用的测试设备包括扭矩传感器、拉力传感器、数据采集等 。测试数据分析 静态姿态角姿态角包括横滚角、俯仰角、航向角。忽略共轴无人机下方固连在万向轴上，同时忽略转台绳索约束对姿态角的影响，共轴无人机处于准静态，即输入油门量为 时，姿态角随时间变化曲线如图 所示。测试选用扩展卡尔曼滤波和 互补滤波进行对比。共轴无人机处于准静态时，采用扩展卡尔曼滤波，姿态角变化小于 ，而采用 互补滤波，姿态角的变化接近 。可以看出，扩展卡尔曼滤波能够很好地减弱共轴无人机的机体振动。分析 内采样 次姿态角信息，显示采用扩展卡尔曼滤波对横滚角、俯仰角、航向角都有较明显的改善作用，为前期机械结构减振滤波设计和控制算法优化提供了参考。飞行姿态角共轴无人机飞行姿态角测试处于相对封闭的校园无风环境中，如图 所示。姿态角期望值和实际值变化曲线如图 所示，二维飞行日志导出轨迹如图 所示，实际飞行路径如图 所示。对于采集的共轴无人机振动数据，利用软件进行数据分析，具体包括 内的 个采样点数据。各方向位置变化曲线如图 所示。实际飞行时方向期望值与实际值误差小于 ，方向期望值与实际值非常接近，方向，即高度方向期望值与实际值同样误差较小，可见共轴无人机姿态估计跟随性较好，在一定程度上解决了误差累积和随机噪声漂移等问题。图 姿态角随时间变化曲线图 飞行姿态角测试环境 结束语针对小型共轴双旋翼飞行器误差源和姿态测量 研 究 设 计装备机械 图 姿态角期望值和实际值变化曲线图 二维飞行轨迹图 实际飞行路径的问题，分析了横滚、俯仰、航向姿态特性，提出四元数扩展卡尔曼滤波数据融合算法，在此基础上进行测试分析。图 各方向位置变化曲线在静态姿态角测试中，对共轴无人机振动幅度和频率进行分析，数据表明，扩展卡尔曼滤波在姿态估计中误差小于 互补滤波。飞行姿态角测试表明，扩展卡尔曼滤波跟踪效果较好，能够减小高速飞行过程中产生的机械振动噪声对姿态角的影响，提高姿态角的稳定性和控制鲁棒性，并有效提高共轴姿态和位置估计的精度。通过研究可见，扩展卡尔曼姿态控制在无人机姿态解算中具有一定的工程应用价值。参考文献 吴和龙 多旋翼无人机的低成本 组合导航关键技术研究 长春：中国科学院大学（中国科学院长春光学精密机械与物理研究所），（下转第 页）研 究 设 计装备机械 （）：陈冰茜，刘永寿，刘海涵，等 管路多源激励动响应分析及减振优化设计 兵器装备工程学报，（）：吴江海，孙玉东，尹志勇，等 充液管路轴向多吸振器波动特性研究 振动与冲击，（）：黎亚军，孙红灵，孙陆阳，等 弯管对末端带弹性障板充液管路辐射声能量的影响 声学学报，（）：（编辑：岚檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯）（上接第 页）沈跃，孙志伟，沈亚运，等 直线型植保无人机航姿 两级估计算法 农业机械学报，（）：刘旭航，刘小雄，章卫国，等 考虑运动加速度干扰的无人机姿态估计算法 哈尔滨工业大学学报，（）：左星星 面向鲁棒和智能化的多源融合 技术研究 杭州：浙江大学，王大轶，侯博文，王炯琦，等 航天器自主导航状态估计方法研究综述 航空学报，（）：吴东旭 共轴双旋翼飞行器姿态控制研究 沈阳：沈阳理工大学，蔡安江，刘凯峰，郭师虹，等 基于四元数衍生无迹卡尔曼滤波的二段式多旋翼无人机姿态估计算法 控制理论与应用，（）：宋宇，翁新武，郭昕刚 基于四元数 算法的小型无人机姿态估计 吉林大学学报（理学版），（）：吴志刚 直升机旋翼噪声实验室（全消声室）设计 噪声与振动控制，（）：，张桥 涵道风扇无人机设计与研究 沈阳：东北大学，冯恺鹏 基于嵌入式的四旋翼无人机姿态测量及控制算法研究 南京：南京理工大学，金红新，杨涛，王小刚，等 多传感器信息融合理论在无人机相对导航中的应用 国防科技大学学报，（）：石喜玲 多旋翼飞行器姿态测量及控制技术研究 太原：中北大学，（编辑：文文檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯）（上接第 页）王贤斌 岸边集装箱起重机检验研究 河南科技，（）：陈良 技术在机械设计中的应用 有色设备，（）：刘亚林，田伟华，何劲，等 中小型汽轮机的集装化整机运输设计 装备机械，（）：李与 自主创新：我国企业发展动力源泉 企业家天地（下半月刊），（）：（编辑：日月檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯檯）第 届无锡太湖国际智能工业展再启航月 日至 日，第 届无锡太湖国际智能工业展在无锡太湖国际博览中心隆重举行。作为举办地的无锡，地处太湖之滨，在长江三角洲装备制造业中占承载着重要使命。得益于长三角经济区良好的地理位置、丰富的人力资源、产业升级和创新发展的推动，以及政府的支持和服务，无锡太湖国际智能工业展为海内外展商与专业用户搭建了一个采购洽谈、技术交流的平台。本届展会总展出面积达 万，汇聚国内外超过 家优质参展商，来自多个国家的采购团和全国各省区市 余名专业买家参观展会。本届展会的主题是“智创未来，共享发展”，致力于向观众提供最新的行业趋势、技术和创新，共同推动行业发展。当前，中国工业行业持续稳定发展，产值和销售额均呈现出稳步增长的态势。尽管受全球经济形势不确定性影响，但是中国机械行业仍然展现出较强的韧性和发展潜力。本届展会参展产品类型多样，涵盖多个领域，从机床设备、金属成型设备、测量设备、工量刃具及配件到轴承及弹簧设备等，一应俱全，反响热烈，效果显著。无界创新，共同成长，第 届无锡太湖国际智能工业展将于 年 月 日至 日举行，目前招商工作已全面展开。（本刊）