CN109596090B

CN109596090B - 一种单兵测姿方法和装置

Info

Publication number: CN109596090B
Application number: CN201910011210.3A
Authority: CN
Inventors: 王洪清; 陈雷; 包静
Original assignee: Wuhan Hongxin Technology Development Co Ltd
Current assignee: Wuhan Hongxin Technology Development Co Ltd
Priority date: 2019-01-07
Filing date: 2019-01-07
Publication date: 2021-10-22
Anticipated expiration: 2039-01-07
Also published as: CN109596090A

Abstract

本发明实施例提供一种单兵测姿方法和装置，其中方法包括：若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；基于自适应参数对当前角速率进行校准，获取校准角速率；基于校准角速率，获取单兵设备的姿态角。本发明实施例提供的方法和装置，通过加速度误差值、磁场强度误差值和自适应函数获取的自适应参数对当前角速率进行校准，并基于校准角速率获取姿态角，实现了单兵设备的测姿，降低了环境因素对单兵测姿误差的影响，提高了单兵设备的环境适应能力。此外，该方法无需额外增加硬件设备，实用性强。

Description

一种单兵测姿方法和装置

技术领域

本发明实施例涉及无线通信技术领域，尤其涉及一种单兵测姿方法和装置。

背景技术

随着移动通信的发展，诸如反恐、消防、公安、特勤和边境安全等安防机构可以通过单兵设备定位犯罪嫌疑人，实现信息探测和安全防护等工作。由于单兵设备工作环境较为复杂，确认单兵自身方向，及时做好路径规划，从而缩短抓捕嫌疑犯时间显得尤为重要。其重点在于减小环境干扰对姿态测量传感器，即加速度计、陀螺仪、磁传感器的影响，提升测姿精度。

由于单兵设备使用的传感器精度较低，需要通过滤波提高测姿精度。目前通用的测姿方法包括卡尔曼滤波和互补滤波两种，其中卡尔曼滤波计算量较大，互补滤波则需要根据实际使用环境和传感器精度整定滤波参数；在不对两种滤波方法的滤波参数进行人工干涉的前提下，将单兵设备置于不同的使用环境下，测姿误差均较大。

因此，如何能够使得单兵设备具备适应各种不同环境的能力，提高单兵设备的测姿精度，仍然是本领域亟待解决的问题。

发明内容

本发明实施例提供一种单兵测姿方法和装置，用以解决现有的单兵设备适应能力差，测姿精度低的问题。

第一方面，本发明实施例提供一种单兵测姿方法，包括：

若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；

基于自适应参数对当前角速率进行校准，获取校准角速率；

基于校准角速率，获取单兵设备的姿态角。

第二方面，本发明实施例提供一种单兵测姿装置，包括：

自适应单元，用于若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；

校准单元，用于基于自适应参数对当前角速率进行校准，获取校准角速率；

第一测姿单元，用于基于校准角速率，获取单兵设备的姿态角。

第三方面，本发明实施例提供一种电子设备，包括处理器、通信接口、存储器和总线，其中，处理器，通信接口，存储器通过总线完成相互间的通信，处理器可以调用存储器中的逻辑指令，以执行如第一方面所提供的方法的步骤。

第四方面，本发明实施例提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如第一方面所提供的方法的步骤。

本发明实施例提供的一种单兵测姿方法和装置，通过加速度误差值、磁场强度误差值和自适应函数获取的自适应参数对当前角速率进行校准，得到校准角速率，并基于校准角速率获取姿态角，实现单兵设备的测姿，降低了环境因素对单兵测姿误差的影响，提高了单兵设备的环境适应能力。此外，该方法无需额外增加硬件设备，实用性强。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的单兵测姿方法的流程示意图；

图2为本发明实施例提供的单兵测姿方法的控制框图；

图3为本发明实施例提供的单兵测姿装置的结构示意图；

图4为本发明实施例提供的电子设备的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

单兵设备中加速度计的测量值容易受到受单兵设备运动加速度和设备震动噪声的影响，磁传感器易受单兵设备本身和工作环境周围铁磁物体的影响，而当前的测姿方法无法适应各种复杂环境，单兵设备在复杂环境下的测向精度极易受到外界因素的干扰。针对上述问题，本发明实施例提供了一种单兵测姿方法，以下将通过多个实施例进行展开说明和介绍。

图1为本发明实施例提供的单兵测姿方法的流程示意图，如图1所示，该方法的执行主体为处理单元，处理单元可以是设置在单兵设备内部的CPU(Central ProcessingUnit，中央处理器)，还可以是与单兵设备通信的服务器，本发明实施例对此不作具体限定。该单兵测姿方法包括：

110，若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的。

此处，当前加速度是基于单兵设备中的加速度计测量得到的加速度，当前磁场强度是基于单兵设备中的磁传感器测量得到的磁场强度，本发明实施例不对单兵设备中的加速度计和磁传感器的型号、设置位置等作具体限定。加速度误差值是基于当前加速度获取的加速度计测量过程中的误差值，加速度误差值用于表征加速度计受到的来自运动加速度、震动噪声等干扰的大小。磁场强度误差值是基于当前磁场强度获取的磁传感器测量过程中的误差值，磁场强度误差值用于表征磁传感器受到的来自单兵设备本身和工作环境周围铁磁物体等因素的磁干扰的大小。

在获取当前加速度之后，基于预设加速度区间对当前加速度的大小进行判断，如果当前加速度处于预设加速度区间，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数。此处，预设加速度区间是预先设定的加速度区间，预设加速度区间用于标定当前加速度中包含的干扰量的大小，从而选取对应的测姿方法。自适应函数是预先设定的函数，自适应函数用于表征加速度误差值、磁场强度误差值与自适应参数之间的对应关系，自适应函数可以分别表征加速度误差值与自适应参数之间的对应关系和磁场强度误差值与自适应参数间的对应关系，还可以综合表征加速度误差值和磁场强度误差值两者与自适应参数之间的对应关系，本发明实施例对此不作具体限定。此处，自适应函数用于校准当前角速率。

120，基于自适应参数对当前角速率进行校准，获取校准角速率。

具体地，当前角速率是基于单兵设备中的陀螺仪测量得到的角速率，本发明实施例不对单兵设备中陀螺仪的型号、设置位置等作具体限定。在获取自适应参数后，基于自适应参数对当前角速率进行校准，将校准后的当前角速率作为校准角速率。此处，基于自适应参数对当前角速率进行校准的方式有多种，例如自适应参数为滤波器参数，将自适应参数代入滤波器中，通过滤波器获取角速率校准量，在当前角速率上叠加角速率校准量，得到校准角速率。

130，基于校准角速率，获取单兵设备的姿态角。具体地，在完成角速率的校准后，对校准角速率进行积分，得到单兵设备的姿态角。

本发明实施例提供的方法，通过加速度误差值、磁场强度误差值和自适应函数获取的自适应参数对当前角速率进行校准，得到校准角速率，并基于校准角速率获取姿态角，实现单兵设备的测姿，降低了环境因素对单兵测姿误差的影响，提高了单兵设备的环境适应能力。此外，该方法无需额外增加硬件设备，实用性强。

基于上述实施例，该方法还包括：若当前加速度不处于预设加速度区间内，则基于当前角速率获取单兵设备的姿态角。

具体地，在获取当前加速度之后，基于预设加速度区间对当前加速度的大小进行判断：如果当前加速度处于预设加速度区间，则确认当前加速度短期内较为稳定，获取自适应参数并对当前角速率进行校准，基于校准角速率获取姿态角。如果当前加速度不处于预设加速度区间内，则确认当前加速度包含的干扰较大，如果将基于当前加速度得到的加速度误差值应用于当前角速率的校准，可能会影响最终的测姿精度。在此情况下，不再对当前角速率进行校准，直接对当前角速率进行积分，获取单兵设备的姿态角。

基于上述任一实施例，步骤110之前还包括：将当前加速度与重力加速度在单兵设备上的投影的向量积作为加速度误差值；将当前磁场强度与地磁场在单兵设备上的投影的向量积作为磁场强度误差值。

具体地，在根据加速度误差值、磁场强度误差值和自适应函数获取自适应参数之前，需要基于当前加速度计算得到加速度误差值，基于当前磁场强度计算得到磁场强度误差值。本发明实施例中，预先测量得到重力加速度在单兵设备上的投影，对当前加速度与重力加速度在单兵设备上的投影作叉乘，得到两者的向量积作为加速度误差值；预先测量得到地磁场在单兵设备上的投影，对当前磁场强度与地磁场在单兵设备上的投影作叉乘，得到两者的向量积作为磁场强度误差值。

基于上述任一实施例，步骤110之前还包括：分别读取单兵设备中加速度计、磁传感器和陀螺仪输出的数字信号，并将数字信号转换为当前加速度、当前磁场强度和当前角速率。

具体地，在单兵设备运动时，首先启动单兵设备的侧向部分，即单兵设备的加速度计、磁传感器和陀螺仪。加速度计对单兵设备的加速度进行测量，并输出表征加速度的数字信号，处理单元读取上述表征加速度的数字信号，并根据加速度计预设的规则将上述表征加速度的数字信号转换为模拟量，即当前加速度。同样地，磁传感器对单兵设备的磁场强度进行测量，并输出表征磁场强度的数字信号，处理单元读取上述表征磁场强度的数字信号，并根据磁传感器预设的规则将上述表征磁场强度的数字信号转换为模拟量，即当前磁场强度。陀螺仪对单兵设备的角速率进行测量，并输出表征角速率的数字信号，处理单元读取上述表征角速率的数字信号，并根据陀螺仪预设的规则将上述表征角速率的数字信号转换为模拟量，即当前角速率。

基于上述任一实施例，自适应函数包括加速度自适应函数和磁场强度自适应函数。

具体地，加速度自适应函数和磁场强度自适应函数是预先设定的函数，加速度自适应函数用于表征加速度误差值和/或磁场强度误差值与加速度自适应参数之间的对应关系，磁场强度自适应函数用于表征加速度误差值和/或磁场强度误差值与磁场自适应参数之间的对应关系。此处，加速度自适应参数和磁场自适应参数均用于当前角速率的校准。

对应地，步骤110具体包括：若当前加速度处于预设加速度区间内，将加速度误差值和/或磁场强度误差值代入加速度自适应函数，获取加速度自适应参数；将加速度误差值和/或磁场强度误差值代入磁场强度自适应函数，获取磁场自适应参数。

例如，若加速度自适应函数用于表征加速度误差值与加速度自适应参数之间的对应关系，则将加速度误差值代入加速度自适应函数，若加速度自适应函数用于表征加速度误差值和磁场强度误差值与加速度自适应参数之间的对应关系，则将加速度误差值和磁场强度误差值代入加速度自适应函数。同样地，当磁场强度自适应函数用于表征磁场强度误差值与磁场自适应参数之间的对应关系，则将磁场强度误差值代入磁场强度自适应函数，若磁场强度自适应函数用于表征加速度误差值和磁场强度误差值与磁场自适应参数之间的对应关系，则将加速度误差值和磁场强度误差值代入磁场强度自适应函数。

基于上述任一实施例，步骤120具体包括：

121，将加速度误差值输入至基于加速度自适应参数设置的PI控制器，获取加速度校正值。

具体地，加速度自适应参数为将加速度误差值代入加速度自适应函数获取的，加速度自适应参数包括加速度积分参数和加速度比例参数，将加速度积分参数设置为PI控制器的积分参数，将加速度比例参数设置为PI控制器的比例参数，得到基于加速度自适应参数设置的PI控制器。将加速度误差输入上述PI控制器，得到加速度校正值。此处，加速度校正值用于对当前角速率进行校准。

122，将磁场强度误差值输入至基于磁场自适应参数设置的PI控制器，获取磁场校正值。

具体地，磁场自适应参数为将磁场强度误差值代入磁场强度自适应函数获取的，磁场自适应参数包括磁场积分参数和磁场比例参数，将磁场积分参数设置为PI控制器的积分参数，将磁场比例参数设置为PI控制器的比例参数，得到基于磁场自适应参数设置的PI控制器。将磁场强度误差输入上述PI控制器，得到磁场校正值。此处，磁场校正值用于对当前角速率进行校准。

需要说明的是，本发明实施例不对步骤121和步骤122的执行顺序作具体限定，步骤122可以先于步骤121执行，也可以与步骤121同步执行。

123，将当前角速率与加速度校正值和磁场校正值相加，得到校准角速率。

基于上述任一实施例，图2为本发明实施例提供的单兵测姿方法的控制框图，如图2所示，单兵测姿方法包括如下步骤：

首先，启动单兵设备的测向部分。随后，分别读取单兵设备中加速度计、磁传感器和陀螺仪输出的数字信号，并将数字信号转换为实际对应的物理量，得到当前加速度、当前磁场强度和当前角速率。

其次，判断当前加速度是否处于预设加速度区间内。

如果当前加速度处于预设加速度区间内，则根据上一次测算得到的姿态角，以及当前加速度和当前磁场强度，求取加速度误差值accerror和磁场强度误差值magerror。此处，姿态角包括俯仰角、横滚角和航向角。

得到accerror和magerror后，基于accerror、magerror和自适应函数，获取对应的自适应参数。此处，自适应参数包括加速度自适应参数和磁场自适应参数，进一步地，加速度自适应参数包括加速度比例参数

和加速度积分参数

磁场自适应参数包括磁场比例参数k_p2和磁场积分参数k_i2，图2中的

将accerror输入至基于加速度自适应参数设置的PI控制器k_p1+k_i1/s中，得到加速度校正值。将magerror输入至基于磁场自适应参数设置的PI控制器k_p2+k_i2/s中，得到磁场校正值。将上述加速度校正值和磁场校正值与当前角速率相加，得到校准角速率

随后，将

输入至

中，得到

并对

进行积分和归一化，得到姿态角。此处，

为解算的姿态四元数表示形式，Q为上一次解算得到的姿态四元数表示形式，

为

如果当前加速度不处于预设加速度区间内，则直接将当前角速率输入至

得到

并对

进行积分和归一化，得到姿态角。

基于上述任一实施例，单兵设备中包括加速度计、磁传感器和陀螺仪。由于单兵测姿方法的测量效果直接受到陀螺仪的校准部分的影响，而单兵设备的实际使用环境较为复杂，加速度计测得的当前加速度中包含有单兵设备所在载体的运动加速度和震动噪声，磁传感器测量值含有单兵设备工作环境周围的磁干扰，如果参数

k_p2和k_i2均保持固定，则单兵测姿方法所解算的姿态角误差较大。

单兵测姿方法中，参数

k_p2和k_i2较小时，测姿方法输出姿态角准确性主要来源于陀螺仪的测量数据，若设备处于颠簸或者加速运动状态，则测姿方法求取的姿态误差主要来自于陀螺仪。当参数

k_p2和k_i2较大时，软锁相方法求取的姿态误差主要取决于加速度计和磁传感器，因加速度计和磁传感器短期稳定性较好，则测姿方法求取姿态误差主要来自于加速度计和磁传感器。

因此，为了增强单兵设备的适应能力，减小环境因素对单兵测姿误差的影响，本发明实施例中，首先判断当前加速度是否处于预设加速度区间内。如果当前加速度不处于预设加速度区间内，确认加速度计测得的当前加速度中包含有的干扰加速度较大，会影响测姿精度，因此短时间内仅通过陀螺仪测得的当前角速率解算姿态角，以减小姿态误差。

如果当前加速度处于预设加速度区间，则结合自适应函数对测姿过程进行优化。因为加速度计和磁传感器在短期内较为稳定，则加速度误差值accerror能够反映干扰加速度的大小，磁场强度误差值magerror能够反映磁干扰的大小，通过accerror、magerror结合自适应函数，自适应调节如下参数

k_p2和k_i2。

进一步地，当||accerror||和||magerror||较小时，加速度计和磁传感器受外界干扰较小，此时姿态应主要取决于加速度计和磁传感器所测量的数据，故应增大

k_p2和k_i2；反之，当||accerror||和||magerror||较大时，加速度计和磁传感器受外界干扰较大，此时姿态应主要取决于陀螺仪测量的当前角速率，故应减小

k_p2和k_i2。具体增大或减小

k_p2和k_i2，根据自适应函数以及||accerror||和||magerror||的值而定。

基于上述任一实施例，预设加速度区间为[0.985g,1.015g]，其中g为重力加速度常量。

基于上述任一方法实施例，图3为本发明实施例提供的单兵测姿装置的结构示意图，如图3所示，单兵测姿装置包括自适应单元310、校准单元320和第一测姿单元330；

其中，自适应单元310用于若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；

校准单元320用于基于自适应参数对当前角速率进行校准，获取校准角速率；

第一测姿单元330用于基于校准角速率，获取单兵设备的姿态角。

本发明实施例提供的装置，通过加速度误差值、磁场强度误差值和自适应函数获取的自适应参数对当前角速率进行校准，得到校准角速率，并基于校准角速率获取姿态角，实现单兵设备的测姿，降低了环境因素对单兵测姿误差的影响，提高了单兵设备的环境适应能力。此外，该方法无需额外增加硬件设备，实用性强。

基于上述任一实施例，该装置还包括第二测姿单元；第二测姿单元用于若当前加速度不处于预设加速度区间内，则基于当前角速率获取单兵设备的姿态角。

基于上述任一实施例，该装置还包括误差计算单元；误差计算单元用于将当前加速度与重力加速度在单兵设备上的投影的向量积作为加速度误差值；将当前磁场强度与地磁场在单兵设备上的投影的向量积作为磁场强度误差值。

基于上述任一实施例，该装置还包括读取单元；读取单元用于分别读取单兵设备中加速度计、磁传感器和陀螺仪输出的数字信号，并将数字信号转换为当前加速度、当前磁场强度和当前角速率。

基于上述任一实施例，自适应函数包括加速度自适应函数和磁场强度自适应函数；

对应地，自适应单元310具体用于：

若当前加速度处于预设加速度区间内，将加速度误差值和/或磁场强度误差值代入加速度自适应函数，获取加速度自适应参数；将加速度误差值和/或磁场强度误差值代入磁场强度自适应函数，获取磁场自适应参数。

基于上述任一实施例，校准单元320具体用于：

将加速度误差值输入至基于加速度自适应参数设置的PI控制器，获取加速度校正值；将磁场强度误差值输入至基于磁场自适应参数设置的PI控制器，获取磁场校正值；将当前角速率与加速度校正值和磁场校正值相加，得到校准角速率。

图4为本发明实施例提供的电子设备的实体结构示意图，如图4所示，该电子设备可以包括：处理器(processor)401、通信接口(Communications Interface)402、存储器(memory)403和通信总线404，其中，处理器401，通信接口402，存储器403通过通信总线404完成相互间的通信。处理器401可以调用存储在存储器403上并可在处理器401上运行的计算机程序，以执行上述各实施例提供的单兵测姿方法，例如包括：若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；基于自适应参数对当前角速率进行校准，获取校准角速率；基于校准角速率，获取单兵设备的姿态角。

此外，上述的存储器403中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

本发明实施例还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各实施例提供的单兵测姿方法，例如包括：若当前加速度处于预设加速度区间内，则根据加速度误差值、磁场强度误差值和自适应函数，获取自适应参数；其中，加速度误差值是基于当前加速度获取的，磁场强度误差值是基于当前磁场强度获取的；基于自适应参数对当前角速率进行校准，获取校准角速率；基于校准角速率，获取单兵设备的姿态角。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。