CN112731956B - 全方位移动侦查探测器、控制系统和控制方法 - Google Patents

Abstract

本发明公开了全方位移动侦查探测器、控制系统和控制方法，涉及无人机技术领域。本发明提供的全方位移动侦查探测器，在叉字型四旋翼的结构基础上，增加了两个所述副旋翼组件增加转矩，通过与四个主旋翼组件配合，使得全方位移动侦查探测器不仅能在空中飞行，也能在水域环境下灵活地完成前进、后退、左移、右移、转弯、前后纵倾等运动，机动性强，不被复杂多变的地域环境所限制，既能实现空中侦查，又能实现水下探测，功能性强。本发明基于模糊投票机制得到更为准确的采样数据，然后通过显式互补滤波算法算出当前位姿信息，使得全方位移动侦查探测器能够在狭长地段、湍急浪涌的环境下实现自身机体的强抗扰动控制。

Description

全方位移动侦查探测器、控制系统和控制方法

技术领域

本发明涉及无人机技术领域，特别是全方位移动侦查探测器、控制系统和控制方法。

背景技术

近年来，随着多旋翼无人机技术的快速发展，涌现出了种类繁多的无人机侦查探测器。这类无人机系统不仅能够完成一定的陆空侦查探测任务，还能够完成相应的实时有效信息反馈。但就目前已设计出的系统技术而言，它们存在许多不足的地方。首先，它们结构单一、机动性差，多采用空中旋翼机架的方式，因此受制于复杂多变的地域环境，如不能有效地完成水域环境下的实时侦查探测任务，这就大大缩小了其侦查范围。其次，它们功能受限、灵活性欠缺，如难以在夜间树林内完成其对应的搜寻侦查的艰巨作业任务，同时续航能力差，因此往往则需要耗费大量的人力物力。最后，这类系统目前往往采用传统单一的惯性测量元件采集数据以完成姿态解算，这就使得其难以在复杂多变的跨水陆空环境采集较准确的传感数据进行解算得到最优的姿态解，因此，这使得这类无人侦查探测系统在水空环境切换等飞行作业过程中自身机体姿态不能有效做到强抗扰动控制，且在大程度上影响了整个系统的运动规划任务。

发明内容

针对上述缺陷，本发明的目的在于提出全方位移动侦查探测器、控制系统和控制方法。

为达此目的，本发明采用以下技术方案：

本发明公开了全方位移动侦查探测器，包括X形机翼、安装架和主旋翼组件，所述安装架设置于所述X形机翼的下端面，四个所述主旋翼组件一一对应地设置在所述X形机翼的四个端部的上端面，各个所述主旋翼组件的旋翼旋转轴分别垂直于所述X形机翼的上端面；还包括副旋翼组件；两个所述副旋翼组件设置在所述安装架之上；两个所述副旋翼组件位于所述X形机翼的同一侧；

两个所述副旋翼组件的旋翼旋转轴在同一水平面上且相互垂直；两个所述副旋翼组件的旋翼旋转轴与所述X形机翼的前后方向之间的夹角为45°。

具体地，所述主旋翼组件和所述副旋翼组件分别包括旋翼和内置电调的防水无刷电机，所述防水无刷电机用于驱动所述旋翼转动。

具体地，对角的所述主旋翼组件的防水无刷电机的转向相同；相邻的所述主旋翼组件的防水无刷电机的转向相反；两个所述副旋翼组件的防水无刷电机的转向相反；所述旋翼分为正桨和反桨；防水无刷电机的转动轴朝上，俯视防水无刷电机，逆时针转动的防水无刷电机与正桨连接；顺时针转动的防水无刷电机与反桨连接。

优选地，所述安装架设有主电源密封舱和备用电源密封舱，主电源密封舱和备用电源密封舱的舱内两端分别设有进水检测感应器。

本发明公开了全方位移动侦查探测控制系统，应用在上述的全方位移动侦查探测器，包括电源模块、微控制单元、微型电脑、5G通信模组、接收机、机载传感模组、地面站和遥控器；电源模块分别与微型电脑、防水无刷电机、微控制单元和机载传感模组电连接；微控制单元分别与微型电脑和防水无刷电机有线通信连接；微控制单元通过接收机分别与地面站和遥控器无线通信连接；微型电脑通过5G通信模块分别与地面站和遥控器无线通信连接；机载传感模组分别与微控制单元和微型电脑有线通信连接；地面站和遥控器分别用于向微控制单元发送任务信息指令和用于向微型电脑发送目标位姿信息；所述微控制单元用于接收任务信息指令，并根据任务信息指令驱动机载传感模组中对应的模块工作；机载传感模组用于获取侦查探测器的当前位姿数据以及侦查探测信息，并将当前位姿数据和侦查探测信息发送至微型电脑；微型电脑用于接收目标位姿信息、当前位姿数据和侦查探测信息，并基于模糊投票机制的显式互补滤波算法，算出当前位姿信息，并将当前位姿信息和目标位姿信息发送至微控制单元；微控制单元还用于接收微型电脑发出的当前位姿信息和目标位姿信息，并用于根据当前位姿信息和目标位姿信息进行PID算法，调整各个防水无刷电机的转速；微型电脑还用于将侦查探测信息以及侦查探测器的实时工作状态发送至地面台；地面台还用于接收微型电脑发出的侦查探测信息以及侦查探测器的实时工作状态。

具体地，所述机载传感模组包括姿态传感模块、RTK模块、GPS模块、气压计模块、水压传感模块、红外热成像模块、夜视仪模块、防水照明模块、声呐模块和摄像模块；姿态传感模块用于获取当前姿态信息；RTK模块、GPS模块、气压计模块和水压传感模块均用于获取当前位置信息；红外热成像模块、夜视仪模块、防水照明模块、声呐模块和摄像模块均用于获取侦查探测信息。

本发明还公开了全方位移动侦查探测控制方法，应用在上述的全方位移动侦查探测控制系统，包括以下步骤：

S1：通过摄像模块检测侦查探测器将要完成对应姿态的环境；若在空中，则切换至空中侦查模式，GPS模块和气压计模块获取当前位置的高度信息数据；若在水中，则切换至水底探测模式，水压传感模块获取当前位置的深度信息数据；

S2：姿态传感模块按设定频率采样三个惯性测量元件的各个数据分量，包括三轴角速度、三轴加速度和三轴磁分量，微型电脑通过模糊投票机制，投票得出准确的三轴角速度和三轴加速度；

S3：微型电脑基于显示互补滤波，建立陀螺仪积分任务和姿态计算任务，根据三轴角速度、三轴加速度和三轴磁分量的数据更新，对应更新出当前姿态角数据；若仅有三轴角速度的数据更新，即偏航角不更新，则陀螺仪积分任务更新姿态四元数，姿态计算任务更新出当前姿态角数据中的俯仰角和横滚角；若三轴加速度的数据更新且三轴磁分量的数据不更新，即偏航角不更新，则姿态计算任务对三轴角速度进行加速度修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的俯仰角和横滚角；若三轴加速度更新且三轴磁分量的数据更新，则姿态计算任务对三轴角速度进行加速度修正补偿和磁力修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的偏航角、俯仰角和横滚角；

S4：微型电脑向微控制单元发送更新后的当前姿态角数据以及对应的高度信息数据或深度信息数据；

S5：微控制单元根据当前姿态角数据和对应的高度信息数据或深度信息数据，利用PID控制器对各个防水无刷电机进行转速调整。

值得说明的是，所述S2中，投票得出准确的三轴角速度和三轴加速度的过程为：

S21：将从各个惯性测量元件采出的三轴角速度和三轴加速度作为投票输入，计算出任意两个惯性测量元件之间各采集三轴分量之间的距离d_ij，根据每个分量的最大值和最小值，确定各个方向的各个分量的(p、q、r)参数，其中p、q、r三个参数用于将每个分量的最大值和最小值分成四个等级区间，得出等级匹配模型，再根据等级匹配模型，确定各个输入分量之间的距离等级；

S22：根据距离等级，定义惯性测量元件各个分量之间距离的模糊元函数为μ(d_ij)，各个距离等级的模糊元取值分别如下：

其中，

表示为距离等级为小的模糊元，

表示为距离等级为中的模糊元，

表示为距离等级为大的模糊元；

S23：接着根据模糊准则表，找到其对应的置信度集合等级：

agSet＝{vlow,low,medium,high,vhigh} (2)

其中，vlow表示为置信度等级很低的置信度值，low表示为置信度等级低的置信度值，medium表示为置信度等级中的置信度值，high表示为置信度等级高的置信度值，vhigh表示为置信度等级很高的置信度值；

判断每个输入分量的置信度，接着按模糊准则量化各个惯性测量元件的各轴分量输入的置信度，得到各个惯性测量元件的各轴分量输入的置信度值P_j，即：

P_j∈agSet,agSet＝{vlow,low,medium,high,vhigh} (3)

然后确定每个输入分量的权重值w_i，计算式如下：

S24：最后计算并输出经过投票后的的三轴角速度和三轴加速度数据，计算式为：

其中χ_i表示为各个惯性测量元件的各轴分量的输入，y_k表示为模糊投票输出，w_x、w_y、w_z对应为角速度在x轴、y轴、z轴的分量，a_x、a_y、a_z对应为加速度在x轴、y轴、z轴的分量。

进一步，所述步骤3中更新出当前姿态角数据的过程为：

S31：建立坐标系，定义侦查探测器的机体系统坐标系b为前左上坐标系，选取导航坐标系n为东北天坐标系，定义分别如下：

其中，x_b，y_b，z_b对应表示为在机体系统坐标系b的x轴、y轴和z轴上的坐标，x_n，y_n，z_n对应表示为在导航坐标系n的x轴、y轴和z轴上的坐标；

S32：采用Z-X-Y顺推归纳法，算出导航坐标系n到机体系统坐标系b的旋转矩阵

其中，γ为横滚角，φ为俯仰角，θ为偏航角；

S33：令

的矩阵元素如下：

并分别解得对应的姿态角表达式：

S34：由坐标变换的四元数乘法表示式，计算机体系统坐标系b到导航坐标系n的四元数旋转矩阵表示旋转矩阵

其中Q表示的是旋转四元数，q₀、q₁、q₂、q₃分别对应旋转四元数中的4个元数；

其中r^b、rⁿ为零标量的四元数，M(Q)、M^*(Q)为对应的四元数矩阵表示形式；

则得到机体系统坐标系b到导航坐标系n的四元数旋转矩阵如下：

根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：

S35：将机体系统坐标系b采样并经投票得到的三轴加速度和三轴磁分量分别归一化处理，

其中，^ba表示为三轴加速度构成的向量，^bm表示为三轴磁分量构成的向量，a_x、a_y、a_z对应为加速度在机体系统坐标系b的x轴、y轴、z轴的分量，m_x、m_y、m_z对应为磁感应强度在机体系统坐标系b的x轴、y轴、z轴的分量，||a_norm||表示合成加速度，

||m_norm||表示合成加速度，

标准重力加速度：ⁿg＝(0 0 1)^T；

将其转化到机体系统坐标系b得到列向量^bv：

其中，v_x表示为列向量^bv在机体系统坐标系b的x轴的分量；v_y表示为列向量^bv在机体系统坐标系b的y轴的分量；v_z表示为列向量^bv在机体系统坐标系b的z轴的分量；

对列向量^bv和^ba作向量积，得到误差向量e₁，即：

其中，e_x表示为误差向量e₁在机体系统坐标系b的x轴的分量；e_y表示为误差向量e₁在机体系统坐标系b的y轴的分量；e_z表示为误差向量e₁在机体系统坐标系b的z轴的分量；

将机体系统坐标系b的归一化的三轴磁分量转化到导航坐标系n，得到列向量ⁿh：

其中，h_x表示为列向量ⁿh在导航坐标系n的x轴的分量；h_y表示为列向量ⁿh在导航坐标系n的y轴的分量；h_z表示为列向量ⁿh在导航坐标系n的z轴的分量；

取导航坐标系y_n轴指北，根据地磁场矢量分解，得到列向量ⁿb：

其中，b_x表示为列向量ⁿb在导航坐标系n的x轴的分量；b_y表示为列向量ⁿb在导航坐标系n的y轴的分量；b_z表示为列向量ⁿb在导航坐标系n的z轴的分量；

将其旋转到机体系统坐标系b，得到列向量^bk，即：

其中，k_x表示为列向量^bk在机体系统坐标系b的x轴的分量；k_y表示为列向量^bk在机体系统坐标系b的y轴的分量；k_z表示为列向量^bk在机体系统坐标系b的z轴的分量；

对向量^bk、^bm作向量积，得到误差向量e₂，即：

将误差向量进行叠加，得到总误差向量e₃，并对其进行PI计算，得到误差修正补偿项，即：

e₃＝e₁+e₂ (20)

其中，K_p、K_i分别为比例系数、积分系数；e_comx误差修正补偿项e_com在机体系统坐标系b的x轴的分量，e_comy误差修正补偿项e_com在机体系统坐标系b的y轴的分量，e_comz误差修正补偿项e_com在机体系统坐标系b的z轴的分量，当三轴加速度没有更新时，e₁为零；当三轴磁分量没有更新时，e₂为零；

经投票得到的三轴角速度^bw_raw，即：

^bw_raw＝(w_x w_y w_z)^T (22)

将其与经PI控制器输出的补偿项分别进行叠加得到补偿后数据：

^bw＝(e_comx+w_x e_comy+w_y e_comz+w_z)^T (23)

再代入下式四元数微分更新方程中，

由上式可解出Q，对其进行单位化得到姿态四元数Q_attitude：

Q_attitude＝(q₀ q₁ q₂ q₃) (25)

最后根据四元数对应旋转矩阵，求得对应的姿态角φ、θ、γ：

其中，γ为横滚角，φ为俯仰角，θ为偏航角，q₀、q₁、q₂、q₃为姿态四元数Q_attitude中的4个元数。

优选地，三轴角速度数据的采集频率为2000Hz，三轴加速度数据的采集频率为800Hz，三轴磁分量数据的采集频率为100Hz。

本发明的有益效果：

本发明提供的全方位移动侦查探测器，在叉字型四旋翼的结构基础上，增加了两个所述副旋翼组件增加转矩，通过与四个主旋翼组件配合，使得全方位移动侦查探测器不仅能在空中飞行，也能在水域环境下灵活地完成前进、后退、左移、右移、转弯、前后纵倾等运动，机动性强，不被复杂多变的地域环境所限制，既能实现空中侦查，又能实现水下探测，功能性强。其中，通过将两个副旋翼组件设置在X形机翼的同一侧；两个副旋翼组件的旋翼旋转轴在同一水平面上且相互垂直，且两个副旋翼组件的旋翼旋转轴与X形机翼的前后方向之间的夹角为45°，利于全方位移动侦查探测器充分利用的力矩的矢量合成，能够合理分配动力，利于在水下实现灵活地前进、后退、左移、右移及任意角度倾斜移动等，进一步提高全方位移动侦查探测器的机动性。同时，通过将各个所述主旋翼组件的旋翼旋转轴分别垂直于所述X形机翼的上端面，实现既考虑了空中姿态控制平稳性，也综合协调了全方位移动侦查探测器在水域环境的控制，实现充分利用四个主旋翼组件来完成全方位移动侦查探测器直接下潜以及上浮至水面完成一键式地起飞。

本发明提供的全方位移动侦查探测控制系统，微型电脑先采样机载传感器，获取当前的位姿信息，再基于模糊投票机制得到更为准确的采样数据，然后通过显式互补滤波算法算出当前位姿信息，最后将当前位姿信息和目标位姿信息发送至微控制单元进行运算以实现改变防水无刷电机的转速，进而实现控制全方位移动侦查探测器的位姿，使得全方位移动侦查探测器能够在狭长地段、湍急浪涌的环境下实现自身机体的强抗扰动控制，利于提高全方位移动侦查探测器的机动性。

本发明提供的全方位移动侦查探测控制方法，利用三个较低成本的惯性测量元件检测，再通过模糊投票机制对各个惯性测量元件的数据进行一个置信的考察，得到信任度更高的采样数据，有效降低单个惯性测量元件在显式互补滤波算法使用过程中所采集到的数据存在较大的漂移误差和累积误差。然后，再通过协同的陀螺仪积分任务和姿态计算任务，对姿态传感器不同时段设定不同的信任度，结合三轴加速度和三轴磁分量独立或组合式地对三轴角速度进行修正补偿，改善原始显式互补滤波过程中陀螺仪积分和解算同时进行但处理效果不能满足全方位移动侦查探测器在空中侦查和水中探测的实时期望姿态的问题。最后将当前位姿信息和目标位姿信息发送至微控制单元进行运算以实现改变防水无刷电机的转速，进而实现控制全方位移动侦查探测器的位姿，使得全方位移动侦查探测器能够在狭长地段、湍急浪涌的环境下实现自身机体的强抗扰动控制，利于提高全方位移动侦查探测器的机动性。

附图说明

图1是本发明的一个实施例的结构示意图；

图2是本发明的一个实施例的系统示意图；

图3是本发明的一个实施例中陀螺仪积分的计算流程图；

图4是本发明的一个实施例中姿态计算任务的计算流程图；

图5是本发明的一个实施例中模糊元函数在等级匹配模型的示意图。

具体实施方式

参照图1，本发明公开了全方位移动侦查探测器，包括X形机翼100、安装架200、主旋翼组件300和副旋翼组件400。所述安装架200设置于所述X形机翼100的下端面。四个所述主旋翼组件300一一对应地设置在所述X形机翼100的四个端部的上端面。各个所述主旋翼组件300的旋翼旋转轴分别垂直于所述X形机翼100的上端面。两个所述副旋翼组件400设置在所述安装架200之上。两个所述副旋翼组件400位于所述X形机翼100的同一侧，两个所述副旋翼组件400的旋翼旋转轴在同一水平面上且相互垂直，两个所述副旋翼组件400的旋翼旋转轴与所述X形机翼100的前后方向之间的夹角为45°。

本发明提供的全方位移动侦查探测器，在叉字型四旋翼的结构基础上，增加了两个所述副旋翼组件400增加转矩，通过与四个主旋翼组件300配合，使得全方位移动侦查探测器不仅能在空中飞行，也能在水域环境下灵活地完成前进、后退、左移、右移、转弯、前后纵倾等运动，机动性强，不被复杂多变的地域环境所限制，既能实现空中侦查，又能实现水下探测，功能性强。其中，通过将两个副旋翼组件400设置在X形机翼100的同一侧；两个副旋翼组件400的旋翼旋转轴在同一水平面上且相互垂直，且两个副旋翼组件400的旋翼旋转轴与X形机翼100的前后方向之间的夹角为45°，利于全方位移动侦查探测器充分利用的力矩的矢量合成，能够合理分配动力，利于在水下实现灵活地前进、后退、左移、右移及任意角度倾斜移动等，进一步提高全方位移动侦查探测器的机动性。同时，通过将各个所述主旋翼组件300的旋翼旋转轴分别垂直于所述X形机翼100的上端面，实现既考虑了空中姿态控制平稳性，也综合协调了全方位移动侦查探测器在水域环境的控制，实现充分利用四个主旋翼组件300来完成全方位移动侦查探测器直接下潜以及上浮至水面完成一键式地起飞。

值得说明的是，在本发明的一些实施例中，全方位移动侦查探测器在空中侦查时，四个主旋翼组件300工作既能完成不同姿态的调整，此模式中，副旋翼组件400不工作。当全方位移动侦查探测器潜入水中探测时，四个主旋翼组件300和两个副旋翼组件400工作配合，以完成不同姿态的调整。当然，全方位移动侦查探测器在空中侦查时，副旋翼组件400也能工作，以增加偏转力矩，使移动侦查探测器能实现急转弯的运动。

具体地，所述主旋翼组件300和所述副旋翼组件400分别包括旋翼(图中未标出)和内置电调的防水无刷电机(图中未标出)，所述防水无刷电机用于驱动所述旋翼转动。如此，确保全方位移动侦查探测器的电路安全，使主旋翼组件300和副旋翼组件400的旋翼尽管在水域中也能正常旋转，以完成全方位移动侦查探测器各个姿态的调整。

值得说明的是，对角的所述主旋翼组件300的防水无刷电机的转向相同；相邻的所述主旋翼组件300的防水无刷电机的转向相反；两个所述副旋翼组件400的防水无刷电机的转向相反；所述旋翼分为正桨和反桨；防水无刷电机的转动轴朝上，俯视防水无刷电机，逆时针转动的防水无刷电机与正桨连接；顺时针转动的防水无刷电机与反桨连接。如此，防水无数电机之间的转速差能实现力矩的矢量合成，能够合理分配动力，使全方位移动侦查探测器能完成前进、后退、左移、右移、纵倾、转弯等运动模式，以调整至不同姿态完成侦查探测任务。

优选地，所述安装架200设有主电源密封舱(图中未标出)和备用电源密封舱(图中未标出)，主电源密封舱和备用电源密封舱的舱内两端分别设有进水检测感应器。

主电源密封舱用于搭载主电源，备用电源密封舱用于搭载备用电源，采用分舱搭载主电源和备用电源，有效避免主电源和备用电源因进水而同时损坏，确保方位移动侦查探测器有电源可用。具体地，进水检测感应器用于检测舱内的进水状况，设立潮湿、轻度进水、大量漏水三层“进水级别”报警，当信息接收端的界面实时显示进水检测程度，并设立故障报警实时提示；且设立严格的电源管理逻辑：若系统检测判断两组舱内一段时间内为潮湿状况，则继续维持正常运行；若检测到主电源密封舱为轻度进水，备用电源密封舱舱为潮湿，则切换到电源管理任务，控制全方位移动侦查探测器使用备用电源供电，并延时检测一段设定的时间，正常后可以选择让全方位移动侦查探测器继续运行或者采取一键起飞模式，返回地面处理故障；若检测到电源密封舱大量漏水，则立即切断电源密封舱供电，若备用电源舱正常，采取备用电源先供电，当备用电源舱也发生进水故障时，则切断供电。通过备用电源舱和进水检测传感器，能大大提高全方位移动侦查探测器在水下作业时的故障分析处理性能。

参照图2，本发明公开了全方位移动侦查探测控制系统，应用在上述的全方位移动侦查探测器中，包括电源模块A10、微控制单元A20、微型电脑A30、机载传感模组A40、地面站A50、遥控器A60、5G通信模组A70和接收机A80。电源模块A10分别与微型电脑A30、防水无刷电机、微控制单元A20和机载传感模组A40电连接。在本实施例中，电源模块A10包括主电源和备用电源，主电源设置在主电源密封舱内，备用电源设置在备用电源密封舱内，主电源和备用电源均能用于为微型电脑A30、防水无刷电机、微控制单元A20和机载传感模组A40供电，以完成侦查探测任务。微控制单元A20、微型电脑A30以及机载传感模组A40中的各个模块分别设置在安装架200上。其中，微型电脑A30可以是树莓派。具体地，如图2所示，微控制单元A20分别与微型电脑A30和防水无刷电机有线通信连接；微控制单元A20通过接收机A80分别与地面站A50和遥控器A60无线通信连接；微型电脑A30通过5G通信模块A70分别与地面站A50和遥控器A60无线通信连接；机载传感模组A40分别与微控制单元A20和微型电脑A30有线通信连接。

具体地，地面站A50和遥控器A60分别用于向微控制单元A20发送任务信息指令和用于向微型电脑A30发送目标位姿信息；所述微控制单元A20用于接收任务信息指令，并根据任务信息指令驱动机载传感模组A40中对应的模块工作。机载传感模组A40用于获取侦查探测器的当前位姿数据以及侦查探测信息，并将当前位姿数据和侦查探测信息发送至微型电脑A30。微型电脑A30用于接收目标位姿信息、当前位姿数据和侦查探测信息，并基于模糊投票机制的显式互补滤波算法，算出当前位姿信息，并将当前位姿信息和目标位姿信息发送至微控制单元A20。微控制单元A20还用于接收微型电脑A30发出的当前位姿信息和目标位姿信息，并用于根据当前位姿信息和目标位姿信息进行PID(Proportion IntegrationDifferentiation，即比例-积分-微分)算法，调整各个防水无刷电机的转速。微型电脑A30还用于将侦查探测信息以及侦查探测器的实时工作状态发送至地面台。地面台还用于接收微型电脑A30发出的侦查探测信息以及侦查探测器的实时工作状态。

在本发明的全方位移动侦查探测控制系统中，微型电脑A30先采样机载传感器，获取当前的位姿信息，再基于模糊投票机制得到更为准确的采样数据，然后通过显式互补滤波算法算出当前位姿信息，最后将当前位姿信息和目标位姿信息发送至微控制单元进行运算以实现改变防水无刷电机的转速，进而实现控制全方位移动侦查探测器的位姿，使得全方位移动侦查探测器能够在狭长地段、湍急浪涌的环境下实现自身机体的强抗扰动控制，利于提高全方位移动侦查探测器的机动性。

值得说明的是，所述机载传感模组A40包括姿态传感模块A41、RTK(Real-timekinematic，即实时动态)模块A42、GPS模块A43、气压计模块A44、水压传感模块A45、红外热成像模块A46、夜视仪模块A47、防水照明模块A48、声呐模块A49和摄像模块A411。姿态传感模块A41用于获取当前姿态信息。具体地，姿态传感模块A41具有三个或三个以上的惯性测量元件，用于测量全方位移动侦查探测器当前的角速度、加速度和磁场强度，以确定全方位移动侦查探测器当前的姿态信息。RTK模块A42、GPS模块A43、气压计模块A44和水压传感模块A45均用于获取当前位置信息；以利于对全方位移动侦查探测器进行精确且全面的定位。红外热成像模块A46、夜视仪模块A47、防水照明模块A48、声呐模块A49和摄像模块A411均用于获取侦查探测信息。具体地，红外热成像模块A46、夜视仪模块A47和摄像模块A411均用于拍摄，以侦查当前环境；防水照明模块A48用于为全方位移动侦查探测器提供亮度，以利于提高摄像的清晰度；声呐模块A49用于全方位移动侦查探测器水下探测。通过红外热成像模块A46、夜视仪模块A47、防水照明模块A48、声呐模块A49和摄像模块A411获取侦查探测信息，实现全方位移动侦查探测器既能在空中侦查，又能在水下探测，功能性强。

参照图3-5，本发明还公开了全方位移动侦查探测控制方法，应用在上述的全方位移动侦查探测控制系统，包括以下步骤：

S1：通过摄像模块A411检测侦查探测器将要完成对应姿态的环境，若在空中，则切换至空中侦查模式，GPS模块A43和气压计模块A44获取当前位置的高度信息数据；若在水中，则切换至水底探测模式，水压传感模块A45获取当前位置的深度信息数据。

S2：姿态传感模块A41按设定频率采样三个惯性测量元件的各个数据分量，包括三轴角速度、三轴加速度和三轴磁分量，微型电脑A30通过模糊投票机制，投票得出准确的三轴角速度和三轴加速度。其中，各个惯性测量元件分别包括用于测量三轴角速度的陀螺仪、用于测量三轴加速度的加速度计和用于测量三轴磁分量的磁力计。三轴角速度是指角速度在x、y、z三轴方向上的分量，三轴加速度是指加速度在x、y、z三轴方向上的分量，三轴磁分量是指磁场感应在x、y、z三轴方向上的分量。

S3：微型电脑A30基于显示互补滤波，建立陀螺仪积分任务和姿态计算任务，根据三轴角速度、三轴加速度和三轴磁分量的数据更新，对应更新出当前姿态角数据；若仅有三轴角速度的数据更新，即偏航角不更新，则陀螺仪积分任务更新姿态四元数，姿态计算任务更新出当前姿态角数据中的俯仰角和横滚角；若三轴加速度的数据更新且三轴磁分量的数据不更新，即偏航角不更新，则姿态计算任务对三轴角速度进行加速度修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的偏航角、俯仰角和横滚角；若三轴加速度更新且三轴磁分量的数据更新，则姿态计算任务对三轴角速度进行加速度修正补偿和磁力修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的偏航角、俯仰角和横滚角；具体地，参照图4所示的陀螺仪积分任务的流程图和图5所示的姿态计算任务的流程图，陀螺仪积分任务通过设定特定的修正补偿标志位，实现三轴加速度和三轴磁分量独立或组合地对三轴角速度进行修正补偿。

S4：微型电脑A30向微控制单元A20发送更新后的当前姿态角数据以及对应的高度信息数据或深度信息数据；

S5：微控制单元A20根据当前姿态角数据和对应的高度信息数据或深度信息数据，利用PID(Proportion Integration Differentiation，即比例-积分-微分)控制器，对各个防水无刷电机进行转速调整。

本发明提供的全方位移动侦查探测控制方法中，利用三个较低成本的惯性测量元件检测，再通过模糊投票机制对各个惯性测量元件的数据进行一个置信的考察，得到信任度更高的采样数据，有效降低单个惯性测量元件在显式互补滤波算法使用过程中所采集到的数据存在较大的漂移误差和累积误差。然后，再通过协同的陀螺仪积分任务和姿态计算任务，对姿态传感器不同时段设定不同的信任度，结合三轴加速度和三轴磁分量独立或组合式地对三轴角速度进行修正补偿，改善原始显式互补滤波过程中陀螺仪积分和解算同时进行但处理效果不能满足全方位移动侦查探测器在空中侦查和水中探测的实时期望姿态的问题。最后将当前位姿信息和目标位姿信息发送至微控制单元进行运算以实现改变防水无刷电机的转速，进而实现控制全方位移动侦查探测器的位姿，使得全方位移动侦查探测器能够在狭长地段、湍急浪涌的环境下实现自身机体的强抗扰动控制，利于提高全方位移动侦查探测器的机动性。

值得说明的是：所述S2中，投票得出准确的三轴角速度和三轴加速度的过程为：

S21：将从各个惯性测量元件采出的三轴角速度和三轴加速度作为投票输入，计算出任意两个惯性测量元件之间各采集三轴分量之间的距离d_ij，根据每个分量的最大值和最小值，即三个惯性测量元件中，角速度和加速度在三轴上的分量的最大值和最小值，确定各个方向的各个分量的(p、q、r)参数，其中p、q、r三个参数用于将每个分量的最大值和最小值分成四个等级区间，得出等级匹配模型，再根据等级匹配模型，确定各个输入分量之间的距离等级。具体地，p、q、r三个参数都是需要先经过数据预先采集后离线反复多次试验测试，然后再对应设定的。

S22：根据距离等级，如图5所示，定义惯性测量元件各个分量之间距离的模糊元函数为μ(d_ij)，各个距离等级的模糊元取值分别如下：

其中，

表示为距离等级为小的模糊元，

表示为距离等级为中的模糊元，

表示为距离等级为大的模糊元；

S23：接着根据模糊准则表，找到其对应的置信度集合等级：

agSet＝{vlow,low,medium,high,vhigh} (2)

其中，vlow表示为置信度等级很低的置信度值，low表示为置信度等级低的置信度值，medium表示为置信度等级中的置信度值，high表示为置信度等级高的置信度值，vhigh表示为置信度等级很高的置信度值；

判断每个输入分量的置信度，接着按模糊准则量化各个惯性测量元件的各轴分量输入的置信度，得到各个惯性测量元件的各轴分量输入的置信度值P_j，即：

P_j∈agSet,agSet＝{vlow,low,medium,high,vhigh} (3)

然后确定每个输入分量的权重值w_i，计算式如下：

S24：最后计算并输出经过投票后的的三轴角速度和三轴加速度数据，计算式为：

其中χ_i表示为各个惯性测量元件的各轴分量的输入，y_k表示为模糊投票输出，w_x、w_y、w_z对应为角速度在x轴、y轴、z轴的分量，a_x、a_y、a_z对应为加速度在x轴、y轴、z轴的分量。

进一步，所述步骤3中更新出当前姿态角数据的过程为：

S31：建立坐标系，定义侦查探测器的机体系统坐标系b为前左上坐标系，选取导航坐标系n为东北天坐标系，定义分别如下：

其中，x_b，y_b，z_b对应表示为在机体系统坐标系b的x轴、y轴和z轴上的坐标，x_n，y_n，z_n对应表示为在导航坐标系n的x轴、y轴和z轴上的坐标；

S32：采用Z-X-Y顺推归纳法，算出导航坐标系n到机体系统坐标系b的旋转矩阵

其中，γ为横滚角，φ为俯仰角，θ为偏航角；

S33：令

的矩阵元素如下：

并分别解得对应的姿态角表达式：

S34：由坐标变换的四元数乘法表示式，计算机体系统坐标系b到导航坐标系n的四元数旋转矩阵表示旋转矩阵

其中Q表示的是旋转四元数，q₀、q₁、q₂、q₃分别对应旋转四元数中的4个元数；

其中r^b、rⁿ为零标量的四元数，M(Q)、M^*(Q)为对应的四元数矩阵表示形式；

则得到机体系统坐标系b到导航坐标系n的四元数旋转矩阵如下：

根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：

S35：将机体系统坐标系b采样并经投票得到的三轴加速度和三轴磁分量分别归一化处理，

其中，^ba表示为三轴加速度构成的向量，^bm表示为三轴磁分量构成的向量，a_x、a_y、a_z对应为加速度在机体系统坐标系b的x轴、y轴、z轴的分量，m_x、m_y、m_z对应为磁感应强度在机体系统坐标系b的x轴、y轴、z轴的分量，||a_norm||表示合成加速度，

||m_norm||表示合成加速度，

标准重力加速度：ⁿg＝(0 0 1)^T；

将其转化到机体系统坐标系b得到列向量^bv：

其中，v_x表示为列向量^bv在机体系统坐标系b的x轴的分量；v_y表示为列向量^bv在机体系统坐标系b的y轴的分量；v_z表示为列向量^bv在机体系统坐标系b的z轴的分量；

对列向量^bv和^ba作向量积，得到误差向量e₁，即：

其中，e_x表示为误差向量e₁在机体系统坐标系b的x轴的分量；e_y表示为误差向量e₁在机体系统坐标系b的y轴的分量；e_z表示为误差向量e₁在机体系统坐标系b的z轴的分量；

将机体系统坐标系b的归一化的三轴磁分量转化到导航坐标系n，得到列向量ⁿh：

其中，h_x表示为列向量ⁿh在导航坐标系n的x轴的分量；h_y表示为列向量ⁿh在导航坐标系n的y轴的分量；h_z表示为列向量ⁿh在导航坐标系n的z轴的分量；

取导航坐标系y_n轴指北，根据地磁场矢量分解，得到列向量ⁿb：

其中，b_x表示为列向量ⁿb在导航坐标系n的x轴的分量；b_y表示为列向量ⁿb在导航坐标系n的y轴的分量；b_z表示为列向量ⁿb在导航坐标系n的z轴的分量；

将其旋转到机体系统坐标系b，得到列向量^bk，即：

其中，k_x表示为列向量^bk在机体系统坐标系b的x轴的分量；k_y表示为列向量^bk在机体系统坐标系b的y轴的分量；k_z表示为列向量^bk在机体系统坐标系b的z轴的分量；

对向量^bk、^bm作向量积，得到误差向量e₂，即：

将误差向量进行叠加，得到总误差向量e₃，并对其进行PI计算，得到误差修正补偿项，即：

e₃＝e₁+e₂ (20)

其中，K_p、K_i分别为比例系数、积分系数；e_comx误差修正补偿项e_com在机体系统坐标系b的x轴的分量，e_comy误差修正补偿项e_com在机体系统坐标系b的y轴的分量，e_comz误差修正补偿项e_com在机体系统坐标系b的z轴的分量，当三轴加速度没有更新时，e₁为零；当三轴磁分量没有更新时，e₂为零；

经投票得到的三轴角速度^bw_raw，即：

^bw_raw＝(w_x w_y w_z)^T (22)

将其与经PI控制器输出的补偿项分别进行叠加得到补偿后数据：

^bw＝(e_comx+w_x e_comy+w_y e_comz+w_z)^T (23)

再代入下式四元数微分更新方程中，

由上式可解出Q，对其进行单位化得到姿态四元数Q_attitude：

Q_attitude＝(q₀ q₁ q₂ q₃) (25)

最后根据四元数对应旋转矩阵，求得对应的姿态角φ、θ、γ：

其中，γ为横滚角，φ为俯仰角，θ为偏航角，q₀、q₁、q₂、q₃为姿态四元数Q_attitude中的4个元数。

优选地，三轴角速度数据的采集频率为2000Hz，三轴加速度数据的采集频率为800Hz，三轴磁分量数据的采集频率为100Hz。如此，通过设计不同的采样频率来获取全方位移动侦查探测器的各种数据，解决以往同频率采集但因传感器特性差异而融合的显式互补算法得到姿态角偏差较大的问题。由于陀螺仪具有良好的动态特性，因此在系统长时间作业时，往往对陀螺仪采集的三轴角数据采取高信任度，通过设计陀螺仪数据采样高频率，如设置为2000Hz，快速获取其三轴角速度数据进行四元数微分更新运算，得到较准确的姿态四元数。另外，通过设计频率低一点的加速度、磁力计，如分别为800Hz、100Hz，这只需在一定时间内完成对姿态角的修正，这样既能减轻系统的繁重数据运算处理，又能达到获取比较准确的姿态解算效果。

Claims (2)

1.全方位移动侦查探测控制方法，应用在全方位移动侦查探测控制系统，所述全方位移动侦查探测控制系统应用在全方位移动侦查探测器；

所述全方位移动侦察探测器包括X形机翼、安装架、主旋翼组件和副旋翼组件，所述安装架设置于所述X形机翼的下端面，四个所述主旋翼组件一一对应地设置在所述X形机翼的四个端部的上端面，各个所述主旋翼组件的旋翼旋转轴分别垂直于所述X形机翼的上端面；两个所述副旋翼组件设置在所述安装架之上；两个所述副旋翼组件位于所述X形机翼的同一侧；两个所述副旋翼组件的旋翼旋转轴在同一水平面上且相互垂直；两个所述副旋翼组件的旋翼旋转轴与所述X形机翼的前后方向之间的夹角为45°；所述主旋翼组件和所述副旋翼组件分别包括旋翼和内置电调的防水无刷电机，所述防水无刷电机用于驱动所述旋翼转动；对角的所述主旋翼组件的防水无刷电机的转向相同；相邻的所述主旋翼组件的防水无刷电机的转向相反；两个所述副旋翼组件的防水无刷电机的转向相反；所述旋翼分为正桨和反桨；防水无刷电机的转动轴朝上，俯视防水无刷电机，逆时针转动的防水无刷电机与正桨连接；顺时针转动的防水无刷电机与反桨连接；所述安装架设有主电源密封舱和备用电源密封舱，主电源密封舱和备用电源密封舱的舱内两端分别设有进水检测感应器；

所述全方位移动侦查探测控制系统包括电源模块、微控制单元、微型电脑、5G通信模组、接收机、机载传感模组、地面站和遥控器；电源模块分别与微型电脑、防水无刷电机、微控制单元和机载传感模组电连接；微控制单元分别与微型电脑和防水无刷电机有线通信连接；微控制单元通过接收机分别与地面站和遥控器无线通信连接；微型电脑通过5G通信模块分别与地面站和遥控器无线通信连接；机载传感模组分别与微控制单元和微型电脑有线通信连接；地面站和遥控器分别用于向微控制单元发送任务信息指令和用于向微型电脑发送目标位姿信息；所述微控制单元用于接收任务信息指令，并根据任务信息指令驱动机载传感模组中对应的模块工作；机载传感模组用于获取侦查探测器的当前位姿数据以及侦查探测信息，并将当前位姿数据和侦查探测信息发送至微型电脑；微型电脑用于接收目标位姿信息、当前位姿数据和侦查探测信息，并基于模糊投票机制的显式互补滤波算法，算出当前位姿信息，并将当前位姿信息和目标位姿信息发送至微控制单元；微控制单元还用于接收微型电脑发出的当前位姿信息和目标位姿信息，并用于根据当前位姿信息和目标位姿信息进行PID算法，调整各个防水无刷电机的转速；微型电脑还用于将侦查探测信息以及侦查探测器的实时工作状态发送至地面台；地面台还用于接收微型电脑发出的侦查探测信息以及侦查探测器的实时工作状态；所述机载传感模组包括姿态传感模块、RTK模块、GPS模块、气压计模块、水压传感模块、红外热成像模块、夜视仪模块、防水照明模块、声呐模块和摄像模块；姿态传感模块用于获取当前姿态信息；RTK模块、GPS模块、气压计模块和水压传感模块均用于获取当前位置信息；红外热成像模块、夜视仪模块、防水照明模块、声呐模块和摄像模块均用于获取侦查探测信息；

其特征在于，所述全方位移动侦查探测控制方法包括以下步骤：

S1：通过摄像模块检测侦查探测器将要完成对应姿态的环境；

若在空中，则切换至空中侦查模式，GPS模块和气压计模块获取当前位置的高度信息数据；

若在水中，则切换至水底探测模式，水压传感模块获取当前位置的深度信息数据；

S2：姿态传感模块按设定频率采样三个惯性测量元件的各个数据分量，包括三轴角速度、三轴加速度和三轴磁分量，微型电脑通过模糊投票机制，投票得出准确的三轴角速度和三轴加速度；

S3：微型电脑基于显示互补滤波，建立陀螺仪积分任务和姿态计算任务，根据三轴角速度、三轴加速度和三轴磁分量的数据更新，对应更新出当前姿态角数据；

若仅有三轴角速度的数据更新，即偏航角不更新，则陀螺仪积分任务更新姿态四元数，姿态计算任务更新出当前姿态角数据中的俯仰角和横滚角；

若三轴加速度的数据更新且三轴磁分量的数据不更新，即偏航角不更新，则姿态计算任务对三轴角速度进行加速度修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的俯仰角和横滚角；

若三轴加速度更新且三轴磁分量的数据更新，则姿态计算任务对三轴角速度进行加速度修正补偿和磁力修正补偿，陀螺仪积分任务根据修正补偿后的三轴角速度更新姿态四元数，姿态计算任务根据新的姿态四元数更新出当前姿态角数据中的偏航角、俯仰角和横滚角；

S4：微型电脑向微控制单元发送更新后的当前姿态角数据以及对应的高度信息数据或深度信息数据；

S5：微控制单元根据当前姿态角数据和对应的高度信息数据或深度信息数据，利用PID控制器对各个防水无刷电机进行转速调整；

其中，所述S2中，投票得出准确的三轴角速度和三轴加速度的过程为：

S21：将从各个惯性测量元件采出的三轴角速度和三轴加速度作为投票输入，计算出任意两个惯性测量元件之间各采集三轴分量之间的距离d_ij，根据每个分量的最大值和最小值，确定各个方向的各个分量的(p、q、r)参数，其中p、q、r三个参数用于将每个分量的最大值和最小值分成四个等级区间，得出等级匹配模型，再根据等级匹配模型，确定各个输入分量之间的距离等级；

S22：根据距离等级，定义惯性测量元件各个分量之间距离的模糊元函数为μ(d_ij)，各个距离等级的模糊元取值分别如下：

其中，

表示为距离等级为小的模糊元，

表示为距离等级为中的模糊元，

表示为距离等级为大的模糊元；

S23：接着根据模糊准则表，找到其对应的置信度集合等级：

agSet＝{vlow,low,medium,high,vhigh} (2)

其中，vlow表示为置信度等级很低的置信度值，low表示为置信度等级低的置信度值，medium表示为置信度等级中的置信度值，high表示为置信度等级高的置信度值，vhigh表示为置信度等级很高的置信度值；

判断每个输入分量的置信度，接着按模糊准则量化各个惯性测量元件的各轴分量输入的置信度，得到各个惯性测量元件的各轴分量输入的置信度值P_j，即：

P_j∈agSet,agSet＝{vlow,low,medium,high,vhigh} (3)

然后确定每个输入分量的权重值w_i，计算式如下：

S24：最后计算并输出经过投票后的的三轴角速度和三轴加速度数据，计算式为：

其中χ_i表示为各个惯性测量元件的各轴分量的输入，y_k表示为模糊投票输出，w_x、w_y、w_z对应为角速度在x轴、y轴、z轴的分量，a_x、a_y、a_z对应为加速度在x轴、y轴、z轴的分量；

其中，所述S3中更新出当前姿态角数据的过程为：

S31：建立坐标系，定义侦查探测器的机体系统坐标系b为前左上坐标系，选取导航坐标系n为东北天坐标系，定义分别如下：

其中，x_b，y_b，z_b对应表示为在机体系统坐标系b的x轴、y轴和z轴上的坐标，x_n，y_n，z_n对应表示为在导航坐标系n的x轴、y轴和z轴上的坐标；

S32：采用Z-X-Y顺推归纳法，算出导航坐标系n到机体系统坐标系b的旋转矩阵

其中，γ为横滚角，φ为俯仰角，θ为偏航角；

S33：令

的矩阵元素如下：

并分别解得对应的姿态角表达式：

S34：由坐标变换的四元数乘法表示式，计算机体系统坐标系b到导航坐标系n的四元数旋转矩阵表示旋转矩阵

其中Q表示的是旋转四元数，q₀、q₁、q₂、q₃分别对应旋转四元数中的4个元数；

其中r^b、rⁿ为零标量的四元数，M(Q)、M^*(Q)为对应的四元数矩阵表示形式；

则得到机体系统坐标系b到导航坐标系n的四元数旋转矩阵如下：

根据旋转矩阵性质，得到从坐标系n到坐标系b的四元数旋转矩阵，即：

S35：将机体系统坐标系b采样并经投票得到的三轴加速度和三轴磁分量分别归一化处理，

其中，^ba表示为三轴加速度构成的向量，^bm表示为三轴磁分量构成的向量，a_x、a_y、a_z对应为加速度在机体系统坐标系b的x轴、y轴、z轴的分量，m_x、m_y、m_z对应为磁感应强度在机体系统坐标系b的x轴、y轴、z轴的分量，||a_norm||表示合成加速度，

||m_norm||表示合成加速度，

标准重力加速度：ⁿg＝(0 0 1)^Τ；

将其转化到机体系统坐标系b得到列向量^bv：

其中，v_x表示为列向量^bv在机体系统坐标系b的x轴的分量；v_y表示为列向量^bv在机体系统坐标系b的y轴的分量；v_z表示为列向量^bv在机体系统坐标系b的z轴的分量；

对列向量^bv和^ba作向量积，得到误差向量e₁，即：

其中，e_x表示为误差向量e₁在机体系统坐标系b的x轴的分量；e_y表示为误差向量e₁在机体系统坐标系b的y轴的分量；e_z表示为误差向量e₁在机体系统坐标系b的z轴的分量；

将机体系统坐标系b的归一化的三轴磁分量转化到导航坐标系n，得到列向量ⁿh：

其中，h_x表示为列向量ⁿh在导航坐标系n的x轴的分量；h_y表示为列向量ⁿh在导航坐标系n的y轴的分量；h_z表示为列向量ⁿh在导航坐标系n的z轴的分量；

取导航坐标系y_n轴指北，根据地磁场矢量分解，得到列向量ⁿb：

其中，b_x表示为列向量ⁿb在导航坐标系n的x轴的分量；b_y表示为列向量ⁿb在导航坐标系n的y轴的分量；b_z表示为列向量ⁿb在导航坐标系n的z轴的分量；

将其旋转到机体系统坐标系b，得到列向量^bk，即：

其中，k_x表示为列向量^bk在机体系统坐标系b的x轴的分量；k_y表示为列向量^bk在机体系统坐标系b的y轴的分量；k_z表示为列向量^bk在机体系统坐标系b的z轴的分量；

对向量^bk、^bm作向量积，得到误差向量e₂，即：

将误差向量进行叠加，得到总误差向量e₃，并对其进行PI计算，得到误差修正补偿项，即：

e₃＝e₁+e₂ (20)

其中，K_p、K_i分别为比例系数、积分系数；e_comx误差修正补偿项e_com在机体系统坐标系b的x轴的分量，e_comy误差修正补偿项e_com在机体系统坐标系b的y轴的分量，e_comz误差修正补偿项e_com在机体系统坐标系b的z轴的分量，当三轴加速度没有更新时，e₁为零；当三轴磁分量没有更新时，e₂为零；

经投票得到的三轴角速度^bw_raw，即：

^bw_raw＝(w_x w_y w_z)^Τ (22)

将其与经PI控制器输出的补偿项分别进行叠加得到补偿后数据：

^bw＝(e_comx+w_x e_comy+w_y e_comz+w_z)^Τ (23)

再代入下式四元数微分更新方程中，

由上式可解出Q，对其进行单位化得到姿态四元数Q_attitude：

Q_attitude＝(q₀ q₁ q₂ q₃) (25)

最后根据四元数对应旋转矩阵，求得对应的姿态角：

其中，γ为横滚角，φ为俯仰角，θ为偏航角，q₀、q₁、q₂、q₃为姿态四元数Q_attitude中的4个元数。

2.根据权利要求1所述的全方位移动侦查探测控制方法，其特征在于，三轴角速度数据的采集频率为2000Hz，三轴加速度数据的采集频率为800Hz，三轴磁分量数据的采集频率为100Hz。

Images