一种基于VR与多传感器的电力巡线无人机的制作方法

本实用新型属无人机视觉识别与自主控制技术领域，具体涉及一种基于vr与多传感器的电力巡线无人机及巡线方法。

背景技术：

随着我国电力系统的飞速发展，电力铁塔的数目大有增长之势。架设在电力铁塔上的输电线路每日经受风吹日晒、雨雪霜冻，容易发生故障与损坏，因此需要进行日常的巡视和维护，以排除故障、消除隐患。目前，大部分电力巡线工作由人工完成，由于输电线路跨度大，且很大一部分输电线路架设在山区，故人工电力巡线存在以下弊端：1.效率低，人工巡线由巡线员步行完成，在地势险要的山区或遇到恶劣天气，巡视一条线路动辄六七个小时甚至更久；2.效果差，电力铁塔高度一般在25-40m，而故障多发区域为塔上部的塔横担部分铁塔与输电线路的连接处，排查故障需要巡线员使用望远镜进行观察，无法有效发现隐患与轻度故障；3.危险性高，巡线员在野外环境与恶劣天气条件下作业风险很高，有时为了有效排除故障，需要巡线员采取攀爬的方式作业，无法保证其人身安全。

以无人机为工具的电力巡线方法不但提高了电力巡线效率与效果，也可以保障巡线员的人身安全，因此逐渐为业界所接受。旋翼式微小型无人机具有灵活性好、结构紧凑、成本低、获取数据快速等特点，可以实现对电力线路与铁塔的全方位拍摄。然而遥控驾驶无人机需要专门的执照，门槛较高，限制了航拍无人机在电力巡线领域的应用，且通过拍摄图像方式进行电力巡线实时性较差，无法实现发现隐患后进一步的详细诊断。

因此，一种可以实现自主飞行且拥有良好的实时性的电力巡线无人机及巡线方法亟待提出。vr技术，即虚拟现实，可以使用双目摄像机传输的图像模拟出实时动态的三维立体图像，通过vr眼镜呈现给巡线员，实现实时的电力巡线。通过机器双目视觉系统、gps系统、惯性导航系统、超声传感器实现无人机的避障、自主路径规划与位姿控制。

技术实现要素：

本实用新型的目的在于针对上述已有技术存在的问题，提出一种基于vr与多传感器的电力巡线无人机及电力巡线方法。

本实用新型的基于vr与多传感器的电力巡线无人机由中心板平台a、动力与控制集成模块b、可折叠机臂组c、起落架自动收放机构组d、起落架对e、三轴高度可调云台f、摄像机对g、超声传感器h、可折叠桨叶组j组成，其中动力与控制集成模块b经螺钉固接于中心板平台a下部前面；可折叠机臂组c的六个可折叠机臂在同一水平面上，每个可折叠机臂上固定板对16所设的孔组ⅲ15，经螺钉固接于呈六边形的中心板平台a的六个角上；可折叠桨叶组j由六个结构完全相同的可折叠桨叶组成，六个可折叠桨叶通过六个销轴分别与可折叠机臂组c中六个可折叠机臂的桨叶支架1所设孔组ⅰ2铰接。起落架自动收放机构组d的两个起落架自动收放机构，经各自收放机构支架20上方所设孔组ⅳ19用螺钉固接于中心板平台a下方，且两个起落架自动收放机构左右对称；起落架对e由左右两个起落架组成，两个起落架的上端分别插入两个起落架自动收放机构上的套筒24中，起落架上端与套筒24过盈连接；三轴高度可调云台f经直线电机30上部所设的孔组ⅴ29用螺钉固接于中心板平台a正下方；摄像机对g的两台摄像机分别固接于三轴高度可调云台f的摄像机固定板ⅰ35和摄像机固定板ⅱ38；超声传感器h经工程胶水粘接于动力与控制集成模块b外壳正前方。

所述的可折叠机臂组c由六个结构完全相同的可折叠机臂组成，其中单个可折叠机臂由桨叶支架1、桨叶电机4、桨叶电机支座5、碳纤维管支架7和折叠机构k组成，其中折叠机构k由活动臂9、固定板对16、止动销11、弹簧12、固定杆13、导向杆18和固定板组限位杆14组成，活动臂9上设有孔10；固定板对16中部设有止动销导向槽对17；固定板对16为左右两个固定板，止动销导向槽对17为左右两个导向槽；止动销11左右两端分别与止动销导向槽对17的左右两个导向槽滑动连接；固定杆13左右两端经螺钉固接于固定板对16的两个固定板之间，且位于固定板近后端上部；导向杆18的后端与固定杆13固接，且导向杆18与止动销11的中心孔为滑动连接；弹簧12穿于导向杆18上，且弹簧12由止动销11固定杆13限位；固定板组限位杆14左右两端经螺钉固接于固定板对16的两个固定板之间，且位于固定板近后端下部；固定板对16上设有由上下对称的8个孔组成的孔组ⅲ15；桨叶电机4、桨叶电机支座5、碳纤维管支架7和折叠机构k自前至后顺序排列，其中桨叶电机支座5前端与桨叶电机4固接，桨叶电机支座5后端经螺母组ⅰ6与碳纤维管支架7前端固接；碳纤维管支架7后端经螺母组ⅱ8与折叠机构k中活动臂9前端固接；桨叶支架1左右端设有由对称的2个孔组成的孔组ⅰ2；桨叶支架1中部设有由对称的2个孔组成的孔组ⅱ3。

所述的起落架自动收放机构组d由四个结构完全相同的起落架自动收放机构组成，其中单个起落架自动收放机构由收放机构支架20、套筒24、销轴22、活动杆ⅰ21、活动杆ⅱ23、活动杆ⅲ25、活动杆ⅳ26和舵机28组成，收放机构支架20上方设有由4个梯形分布的孔构成的孔组ⅳ19；套筒24上端与收放机构支架20前端下面铰接；套筒24下端与活动杆ⅱ23和活动杆ⅲ25下端铰接；活动杆ⅰ21和活动杆ⅳ26上端与收放机构支架20后端下面铰接；活动杆ⅱ23和活动杆ⅲ25上端与销轴22铰接；活动杆ⅳ26上设有方孔27，方孔27与舵机28的主轴连接；舵机28经螺钉与收放机构支架20左面固接。

所述的三轴高度可调云台f由直线电机30、x轴转动电机43、y轴转动电机ⅰ34、y轴转动电机ⅱ36、z轴转动电机31、电机支架ⅰ46、电机支架ⅱ44、电机支架ⅲ33、电机支架ⅳ39、连接杆ⅰ45、连接杆ⅱ41、固定板42、半圆固定环组32、摄像机固定板ⅰ35和摄像机固定板ⅱ38组成，直线电机30上部设有由4个孔组成的孔组ⅴ29；直线电机30下部经螺钉与z轴转动电机31固连；z轴转动电机31与电机支架ⅰ46经螺钉固接；连接杆ⅰ45上端与电机支架ⅰ46固接，连接杆ⅰ45下端与电机支架ⅱ44固接；x轴转动电机43上端与电机支架ⅱ44下端固接，x轴转动电机43前端与固定板42后面固接；半圆固定环组32由2个左右对称的半圆固定环组成，螺母副组40由2个螺母副组成，连接杆ⅱ41经半圆固定环组32固接于固定板42前面；连接杆ⅱ41近左端前面与电机支架ⅲ33后端经螺母副组40中一个螺母副固接；连接杆ⅱ41近右端前面与电机支架ⅳ39后端经螺母副组40中另一个螺母副固接；电机支架ⅲ33前端左面与y轴转动电机ⅰ34固接；电机支架ⅳ39前端右面与y轴转动电机ⅱ36固接；摄像机固定板ⅰ35固接于y轴转动电机ⅰ34左面；摄像机固定板ⅱ38固接于y轴转动电机ⅱ36右面；摄像机固定板ⅰ35和摄像机固定板ⅱ38上设有由左右对称布置的共4个孔构成的孔组ⅵ37。

本实用新型的巡线无人机具有两种作业模式，巡视输电线路时采用巡查模式，检查电力铁塔时采用观察模式。巡线作业前利用卫星数字地图确定巡线的轨迹，并确定每个电力铁塔的gps坐标，确保无人机按照预定轨迹飞行并在每个铁塔处悬停。巡查模式下，双目摄像机的信号同时传输给机器视觉处理系统与vr系统，机器视觉处理系统通过不断识别输电线路图像来矫正无人机飞行轨迹，vr系统将实时图像经过处理传输到vr眼镜，由巡线员实时观察输电线路情况。观察模式下，飞机在铁塔处悬停，通过vr眼镜上的位姿传感器可对搭载双目摄像机的云台及无人机机体进行微调，以实现巡线员从不同角度观察塔横担处情况。无人机在巡查模式与观察模式下反复切换，直至完成巡线任务。飞行过程中为了保证无人机不发生碰撞事故，采用超声传感器实现无人机的避障，为了保证传输图像的平稳清晰，采用gps与惯性导航系统的组合导航系统控制无人机位姿。

一种基于vr与多传感器的电力巡线无人机的巡线方法包括下列步骤：

1.1根据给定的要进行巡线的输电线路，在卫星数字地图上确定无人机的飞行轨迹，并确定线路上每一座铁塔的编号与gps坐标；经无人机携带的gps传感器，指引无人机飞行至巡线起始坐标点附近准备作业；

1.2无人机开启巡查模式，经无人机双目视觉系统识别输电线路，保证无人机飞行轨迹与输电线路平行，同时摄像机对g的图像传输给vr系统，巡线员实时观察输电线路状况；

1.3当无人机飞行至铁塔gps坐标附近时减速，并经双目视觉系统匹配识别输电铁塔的塔横担部位，匹配成功后无人机悬停并开启观察模式，巡线员观测塔横担部位并经vr眼镜对搭载摄像机对g的三轴高度可调云台f及无人机机体进行微调；具体包括如下步骤：

1.3.1在无人机进行巡线作业前，将不同角度拍摄的塔横担部分图像进行处理，并存放至无人机图像匹配数据库，以便无人机双目视觉系统对塔横担进行匹配识别；

1.3.2无人机双目视觉系统识别塔横担部位后进行悬停，悬停后由双目视觉系统进行三维重建，计算无人机允许的位置调整安全范围；

1.3.3无人机开启观察模式，摄像机对g将图像传输至vr系统，经处理后形成实时三维图像再传输至vr眼镜，由佩戴vr眼镜的巡线员对塔横担部分进行观察，巡线员轻微晃动头部实现对三轴高度可调云台f及无人机机体的微调，以便观察不同位置的细节；

1.3.4经过t/2时间后，经直线电机30自动调整摄像机对g的高度，从另一高度继续对塔横担部分进行观察；

1.3.5经过t时间后，无人机自动退出观察模式，直线电机30恢复初始位置，无人机返回至巡查模式并继续按照既定路线进行巡线，直至到达下一个铁塔，再重复步骤1.3；

1.4巡线过程中，超声传感器实时监测无人机与障碍物的距离，一旦小于安全距离，无人机悬停并重新调整位置；

1.5巡线过程中，经gps与惯性导航系统实时检测并调整无人机位姿，保证观测图像的平稳。

本实用新型能保证无人机在自主控制的情况下辅助巡线员完成大部分巡线作业，弥补了人工巡线的诸多不足，巡线作业的效率与效果也得到了显著提高。

附图说明

图1为基于vr与多传感器的电力巡线无人机的整体结构示意图

图2为可折叠机臂结构示意图

图3为可折叠机臂的折叠机构结构示意图

图4为起落架自动收放机构结构示意图

图5为三轴高度可调云台结构示意图

图6为电力巡线无人机自主巡线方法流程图

图7为电力巡线无人机各传感器硬件交互关系示意图

其中：a.中心板平台b.动力与控制集成模块c.可折叠机臂组d.起落架自动收放机构组e.起落架对f.三轴高度可调云台g.摄像机对h.超声传感器j.可折叠桨叶组k.折叠机构1.桨叶支架2.孔组ⅰ3.孔组ⅱ4.桨叶电机5.桨叶电机支座6.螺母组ⅰ7.碳纤维管支架8.螺母组ⅱ9.活动臂10.孔11.止动销12.弹簧13.固定杆14.固定板组限位杆15.孔组ⅲ16.固定板对17.止动销导向槽18.导向杆19.孔组ⅳ20.收放机构支架21.活动杆ⅰ22.销轴23.活动杆ⅱ24.套筒25.活动杆ⅲ26.活动杆ⅳ27.方孔28.舵机29.孔组ⅴ30.直线电机31.z轴转动电机32.半圆固定环组33.电机支架ⅲ34.y轴转动电机ⅰ35.摄像机固定板ⅰ36.y轴转动电机ⅱ37.孔组ⅵ38.摄像机固定板ⅱ39.电机支架ⅳ40.螺母副组41.连接杆ⅱ42.固定板43.x轴转动电机44.电机支架ⅱ45.连接杆ⅰ46.电机支架ⅰ

具体实施方式

下面结合附图对本实用新型作进一步描述。

电力巡线无人机的结构设计方案如下：

如图1所示，一种基于vr与多传感器的电力巡线无人机由中心板平台a、动力与控制集成模块b、可折叠机臂组c、起落架自动收放机构组d、起落架对e、三轴高度可调云台f、摄像机对g、超声传感器h、可折叠桨叶组j组成，其中动力与控制集成模块b经螺钉固接于中心板平台a下部前面；可折叠机臂组c由六个结构完全相同的可折叠机臂组成，可折叠机臂组c的六个可折叠机臂，在水平面上均布排列，每个可折叠机臂上固定板对16所设的孔组ⅲ15，经螺钉固接于中心板平台a；可折叠桨叶组j由六个结构完全相同的可折叠桨叶组成，六个可折叠桨叶通过六个销轴分别与可折叠机臂组c中六个可折叠机臂的桨叶支架1所设孔组ⅰ2铰接；起落架自动收放机构组d的两个起落架自动收放机构，经各自收放机构支架20上方所设孔组ⅳ19用螺钉固接于中心板平台a下方，且两个起落架自动收放机构左右对称。起落架对e由左右两个起落架组成，两个起落架的上端分别插入两个起落架自动收放机构上的套筒24中，起落架上端与套筒通过过盈配合实现固定连接，并由螺栓锁紧；三轴高度可调云台f经直线电机30上部所设的孔组ⅴ29用螺钉固接于中心板平台a正下方；摄像机对g的两台摄像机分别固接于三轴高度可调云台f的摄像机固定板ⅰ35和摄像机固定板ⅱ38；超声传感器h经工程胶水粘接于动力与控制集成模块b外壳正前方。

动力与控制集成模块b中集成了无人机的电源模块、惯性传感器、gps传感器、微控制器和机载电脑等电子部分，用于提供无人机的飞行动力，实时检测无人机的位置姿态与飞行状态，控制无人机的自主飞行与工作模式切换。超声传感器h安装于动力与控制集成模块b的外壳上，以避免对超声传感器h的封装影响超声波的发射与接受，保证其使用效果。可折叠桨叶j通过旋转为无人机提供升力，本实用新型采用六旋翼设计方案，增强了无人机飞行时的稳定性，提高了双目摄像机g的成像质量，保证了无人机的巡线作业效果。

如图2与图3所示，可折叠机臂组c由六个结构完全相同的可折叠机臂组成，其中单个可折叠机臂由桨叶支架1、桨叶电机4、桨叶电机支座5、碳纤维管支架7和折叠机构k组成，其中折叠机构k由活动臂9、固定板对16、止动销11、弹簧12、固定杆13、导向杆18和固定板组限位杆14组成，活动臂9上设有孔10；固定板对16中部设有止动销导向槽对17；固定板对16为左右两个固定板，止动销导向槽对17为左右两个导向槽；止动销11左右两端分别与止动销导向槽对17的左右两个导向槽滑动连接；固定杆13左右两端经螺钉固接于固定板对16的两个固定板之间，且位于固定板近后端上部；导向杆18由后端的螺纹与固定杆13固接，且导向杆18与止动销11的中心孔为滑动连接；弹簧12穿于导向杆18上，且弹簧12的前端顶在止动销11上，弹簧12的后端顶在固定杆13上；固定板组限位杆14左右两端经螺钉固接于固定板对16的两个固定板之间，且位于固定板近后端下部；固定板对16上设有由上下对称的8个孔组成的孔组ⅲ15；桨叶电机4、桨叶电机支座5、碳纤维管支架7和折叠机构k自前至后顺序排列，其中桨叶电机支座5前端与桨叶电机4固接，桨叶电机支座5后端经螺母组ⅰ6与碳纤维管支架7前端固接；碳纤维管支架7后端经螺母组ⅱ8与折叠机构k中活动臂9前端固接；桨叶支架1左右端设有由对称的2个孔组成的孔组ⅰ2；桨叶支架1中部设有由对称的2个孔组成的孔组ⅱ3。

可折叠机臂c展开与折叠时，手动拉动止动销11后即可对活动臂9进行转动。活动臂9绕孔10的轴线转动，转动到极限位置后松开止动销11，弹簧推动止动销实现对活动臂9的锁死。在移动和存放无人机时，可通过折叠的方式来减小无人机占地面积，提高了无人机运输和存放的便携性和有效性。

如图4所示，起落架自动收放机构组d由四个结构完全相同的起落架自动收放机构组成，其中单个起落架自动收放机构由收放机构支架20、套筒24、销轴22、活动杆ⅰ21、活动杆ⅱ23、活动杆ⅲ25、活动杆ⅳ26和舵机28组成，收放机构支架20上方设有由4个梯形分布的孔构成的孔组ⅳ19；套筒24上端与收放机构支架20前端下面铰接；套筒24下端与活动杆ⅱ23和活动杆ⅲ25下端铰接；活动杆ⅰ21和活动杆ⅳ26上端与收放机构支架20后端下面铰接；活动杆ⅱ23和活动杆ⅲ25上端与销轴22铰接；活动杆ⅳ26上设有方孔27，方孔27与舵机28的主轴连接；舵机28经螺钉与收放机构支架20左面固接。

起落架自动收放机构组d起落的两个极限位置设计为曲柄连杆机构的两个死点位置，可以大大提高起落架的稳定性，采用舵机27作为动力机带动曲柄旋转可以精确控制曲柄的运动角度。

如图5所示，三轴高度可调云台f由直线电机30、x轴转动电机43、y轴转动电机ⅰ34、y轴转动电机ⅱ36、z轴转动电机31、电机支架ⅰ46、电机支架ⅱ44、电机支架ⅲ33、电机支架ⅳ39、连接杆ⅰ45、连接杆ⅱ41、固定板42、半圆固定环组32、摄像机固定板ⅰ35和摄像机固定板ⅱ38组成，直线电机30上部设有由4个孔组成的孔组ⅴ29；直线电机30下部经螺钉与z轴转动电机31固连；z轴转动电机31与电机支架ⅰ46经螺钉固接；连接杆ⅰ45上端与电机支架ⅰ46固定连接，连接杆ⅰ45下端与电机支架ⅱ44固接；x轴转动电机43上端与电机支架ⅱ44下端固接，x轴转动电机43前端与固定板42后面固接；半圆固定环组32由2个左右对称的半圆固定环组成，螺母副组40由2个螺母副组成，连接杆ⅱ41经半圆固定环组32固接于固定板42前面；连接杆ⅱ41近左端前面与电机支架ⅲ33后端通过过盈配合固定连接，螺母副组40中一个螺母副锁紧；连接杆ⅱ41近右端前面与电机支架ⅳ39后端通过过盈配合固定连接，螺母副组40中另一个螺母副锁紧；电机支架ⅲ33前端左面与y轴转动电机ⅰ34固接；电机支架ⅳ39前端右面与y轴转动电机ⅱ36固接；摄像机固定板ⅰ35固接于y轴转动电机ⅰ34左面；摄像机固定板ⅱ38固接于y轴转动电机ⅱ36右面；摄像机固定板ⅰ35和摄像机固定板ⅱ38上设有由左右对称布置的共4个孔构成的孔组ⅵ37。

x轴转动电机42、y轴转动电机ⅰ33、y轴转动电机ⅱ35、z轴转动电机30用于控制双目摄像机g的x、y、z三个方向的旋转自由度，实现巡线过程中摄像机多角度的拍摄与消除无人机位姿变化对双目摄像机g的不利影响。由于无人机的位姿控制方法较为复杂，若通过改变无人机高度来调整摄像机高度会破环无人机原有的稳定状态，降低双目摄像机g的成像质量，因此选用直线电机29用于调整三轴高度可调云台f与无人机机体的相对高度，实现在保证双目摄像机g的成像质量的前提下拍摄不同高度的图像，提高巡线无人机观察模式下的作业效果。

如图6所示，电力巡线无人机的巡线方法包括下列步骤：

步骤一，根据给定的要进行巡线的输电线路，在卫星数字地图上确定无人机的飞行轨迹，并确定线路上每一座铁塔的编号与gps坐标。通过无人机携带的gps传感器，指引无人机飞行至巡线起始坐标点附近准备作业；

步骤二，无人机开启巡查模式，通过无人机双目视觉系统识别输电线路，保证无人机飞行轨迹与输电线路平行，同时双目摄像机的图像传输给vr系统，巡线员实时观察输电线路状况。保证无人机飞行轨迹与输电线路平行的方法具体为：将双目摄像机拍摄的图像进行图像分割与最小二乘法拟合，实时计算无人机前进方向中轴线与输电线路的夹角θ与偏差距离d，通过控制夹角θ与偏差距离d不变来保证无人机对输电线路的跟踪飞行。

步骤三，当无人机飞行至铁塔gps坐标附近时减速，并通过双目视觉系统匹配识别输电铁塔的塔横担部位，匹配成功后无人机悬停并开启观察模式，巡线员观测塔横担部位并通过vr眼镜对搭载双目摄像机的云台及无人机机体进行微调。

具体步骤如下：

(1)在无人机进行巡线作业前，将不同角度拍摄的塔横担部分图像进行处理，并存放至无人机图像匹配数据库，以便无人机双目视觉系统对塔横担进行匹配识别。具体图像处理方法为：首先采用中值滤波对图像进行滤波处理，并进一步进行灰度化处理和二值化处理，再通过laplace算子实施图像锐化及边缘检测，最后通过sift算法和harris角点检测方法提取特征点以便于后续的图像匹配；

(2)无人机双目视觉系统将拍摄的图像进行滤波处理、灰度化处理和二值化处理，再通过laplace算子实施图像锐化及边缘检测，最后通过sift算法和harris角点检测方法提取特征点并与(1)无人机图像匹配数据库中处理后的图像进行匹配。一旦匹配成功，说明无人机已经到达观察位置，无人机悬停。

无人机悬停后通过双目视觉系统采集的三维图像，计算无人机允许的位置调整安全范围；

(3)无人机开启观察模式，双目摄像机将图像传输至vr系统，经处理后形成实时三维图像再传输至vr眼镜，由佩戴vr眼镜的巡线员对塔横担部分进行观察。

vr眼镜中安装有陀螺仪，加速度计和地磁位置传感器，可以检测到轻微的移动。巡线员轻微晃动头部实现对搭载双目摄像机的云台及无人机机体的微调，以便观察不同位置的细节。其中，vr眼镜左右轻微晃动控制云台的左右摆动，上下的轻微晃动控制云台的上下摆动，前后的轻微移动控制无人机机体的前后移动。无人机的移动范围应限定在(2)中计算的安全范围内；

(4)经过t/2时间后，通过直线电机(30)自动调整摄像机对(g)的高度，从另一高度继续对塔横担部分进行观察。

(5)经过t时间后，无人机自动退出观察模式，直线电机(30)恢复初始位置，无人机返回至巡查模式并继续按照既定路线进行巡线，直至到达下一个铁塔，再重复步骤三。

步骤四，巡线过程中，超声传感器作为辅助避障措施实时监测无人机与障碍物的距离，一旦小于安全距离，无人机悬停并重新调整位置；

步骤五，巡线过程中，通过gps与惯性导航系统组成的组合导航系统实时检测并调整无人机位姿，保证观测图像的平稳。惯性导航系统中包括陀螺仪，加速度计与磁强计。在组合导航系统中，主要通过加速度计预测无人机位置信息，利用gps观测值修正；通过陀螺仪预测无人机姿态信息，利用加速度计观测值修正滚动角、俯仰角，利用磁强计修正偏航角。

组合导航系统的数据融合采用拓展的卡尔曼滤波算法，选用导航坐标系下的位置，机体坐标系下的速度、姿态，陀螺仪漂移量和加速度计漂移量作为系统状态参数，构建系统状态方程如下：

其中，是由16个变量构成的系统状态向量；um＝[pqraxayaz]^t是由三轴速率陀螺仪和加速度计读入的系统输入量，通过误差修正后用于推算状态变量；ω是服从正态分布n(0,q)的白噪声测量值；q是系统过程噪声ω的对称非负定方差矩阵；(xyz)是导航坐标系下无人机位置坐标；(uvw)是机体坐标系下无人机沿坐标轴方向上的速度；(q0q1q2q3)是在机体坐标系下由四元数描述的无人机姿态；(bpbqbr)是三轴速率陀螺仪漂移量；是沿着三个机体坐标轴方向的加速度计偏差；g代表当地重力加速度。

选用gps、加速度计、磁强计读入的变量直接或间接作为观测量构建观测方程如下：

z(t)＝h(xs)+v

其中，z(t)表示系统观测向量；h(xs)表示关于系统状态向量的状态观测函数；v是服从正态分布n(0,r)的白噪声测量值；r是系统过程噪声ω的对称正定方差阵。