CN115574812A - 姿态数据处理方法、装置、电子设备和存储介质 - Google Patents

姿态数据处理方法、装置、电子设备和存储介质 Download PDF

Info

Publication number
CN115574812A
CN115574812A CN202211272001.2A CN202211272001A CN115574812A CN 115574812 A CN115574812 A CN 115574812A CN 202211272001 A CN202211272001 A CN 202211272001A CN 115574812 A CN115574812 A CN 115574812A
Authority
CN
China
Prior art keywords
measured object
coordinate system
measured
vector
position vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211272001.2A
Other languages
English (en)
Inventor
何风行
李金兰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Lingyu Technology Beijing Co ltd
Original Assignee
Lingyu Technology Beijing Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Lingyu Technology Beijing Co ltd filed Critical Lingyu Technology Beijing Co ltd
Publication of CN115574812A publication Critical patent/CN115574812A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本申请公开了一种姿态数据处理方法、装置、电子设备和存储介质。姿态数据处理方法：根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。通过上述方式，可以处理得到被测物体的姿态数据，实现被测物体处于预设状态时的姿态数据处理。

Description

姿态数据处理方法、装置、电子设备和存储介质
技术领域
本申请实施例涉及通信技术领域，尤其涉及一种姿态数据处理方法、装置、电子设备和存储介质。
背景技术
随着社会各行各业的快速发展，人们对定位与导航的需求日益增大，尤其是在复杂的室内环境，常常需要确定各种设施与物品在室内的位置信息，但是受到定位时间、定位精确度以及复杂室内环境等条件的限制，比较完善的室内定位技术目前还无法很好的利用。因此，业界人士提出了很多室内定位技术解决方案，如超声波定位技术。
超声波定位系统是利用超声波的空间传播特性，来确定目标的具体位置。将超声波发生器置于被定位的目标上面，向周围按照一定的时间间隔发送超声波脉冲，在周围三个固定定位位置上分别接收超声波发射装置发出来的脉冲信号，由于超声波在空间传送速度比较慢，所以通过比较三个接收装置收到信号的时间先后，可以反推出超声波发生器的具体位置，也就是被定位目标的位置，当目标移动时候，可以通过不间断定位，描出目标的运动轨迹。
现有技术中，在单颗超声部署时，初始姿态确定方法有卡尔曼滤波融合方法。然而在被测物体姿态初始化过程中可能由于被测物体处于相对静止状态，而卡尔曼滤波融合方法要求有晃动被测物体的过程，因此，无法对初始姿态为相对静止状态的被测物体进行姿态数据处理。
发明内容
有鉴于此，本申请实施例提供一种姿态数据处理方法、装置、电子设备和存储介质，以至少部分解决上述问题。
根据本申请实施例的第一方面，提供了一种姿态数据处理方法，包括：根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
在本申请的另一实现方式中，所述根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态，包括：获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模；获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模；根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态。
在本申请的另一实现方式中，所述根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态，包括：当所述被测物体的陀螺仪测量值的模处于零值范围内时且当所述被测物体的自身坐标系重力矢量的模处于重力加速度阈值范围内时，确定所述被测物体处于预设状态，否则，确定所述被测物体不处于预设状态。
在本申请的另一实现方式中，所述获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模，包括：获取所述被测物体的陀螺仪三个轴的角速度测量值；利用平方根函数对所述陀螺仪三个轴的角速度测量值进行计算，得到所述被测物体的陀螺仪测量值的模。
在本申请的另一实现方式中，所述获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模，包括：获取被测物体的加速度计三个轴的加速度测量值；利用平方根函数对所述加速度计三个轴的加速度测量值进行计算，得到所述被测物体的自身坐标系重力矢量的模。
在本申请的另一实现方式中，所述当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，包括：根据所述被测物体上的两个超声传感器在世界坐标系中的定位坐标，计算得到所述被测物体的第一位置矢量；根据所述被测物体上的两个超声传感器在自身坐标系中的位置坐标，计算得到所述被测物体的第二位置矢量。
在本申请的另一实现方式中，所述对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据，包括：根据所述被测物体的第一位置矢量、所述被测物体的第二位置矢量、所述被测物体的自身坐标系重力矢量、被测物体世界坐标系重力矢量计算得到所述被测物体自身坐标系到世界坐标系的姿态转移矩阵；对所述姿态转移矩阵进行转置运算，得到所述被测物体的姿态数据。
根据本申请实施例的第二方面，提供了一种姿态数据处理装置，包括：状态确定模块，用于根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；位置计算模块，用于当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；姿态处理模块，用于对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
根据本申请实施例的第三方面，提供了一种电子设备，包括：处理器、存储器、通信接口和通信总线，处理器、存储器和通信接口通过通信总线完成相互间的通信；存储器用于存放至少一可执行指令，可执行指令使处理器执行如第一方面所述的方法对应的操作。
根据本申请实施例的第四方面，提供了一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的方法。
本申请实施例的方案中，在被测物体姿态初始化过程中，由于被测物体可能处于相对静止状态，导致无法对被测物体进行姿态数据处理。通过至少两组超声位置数据确定并得到被测物体的世界坐标系位置矢量和被测物体的自身坐标系位置矢量，结合被测物体的自身坐标系重力矢量和被测物体世界坐标系重力矢量，可以处理得到被测物体的姿态数据，实现被测物体处于预设状态时的姿态数据处理。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
图1为根据本申请的一个实施例的一种姿态数据处理方法的步骤流程图。
图2为根据本申请的一个实施例的一种陀螺仪结构示意图。
图3为根据本申请的一个实施例的一种姿态数据处理装置的示意性框图。
图4为根据本申请的一个实施例的电子设备的结构示意图。
具体实施方式
为了使本领域的人员更好地理解本申请实施例中的技术方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、详细地描述，显然，所描述的实施例仅是本申请实施例一部分实施例，而不是全部的实施例。基于本申请实施例中的实施例，本领域普通技术人员所获得的所有其他实施例，都应当属于本申请实施例保护的范围。
应当理解，本披露的权利要求、说明书及附图中的术语“第一”、“第二”和“第三”等是用于区别不同对象，而不是用于描述特定顺序。本披露的说明书和权利要求书中使用的术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本披露说明书中所使用的术语仅仅是出于描述特定实施例的目的，而并不意在限定本披露。如在本披露说明书和权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。还应当进一步理解，在本披露说明书和权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
现有的姿态数据处理方法是，在单颗超声接收传感器部署时，通过3种公知方法手动标定方法、九轴捷联惯导解算融合方法、卡尔曼滤波融合方法确定初始位姿。其中，九轴捷联惯导解算融合方法利用加速度传感器实现对俯仰角和翻滚角的观测、利用地磁传感器实现的航向角的观测。主要问题点是室内环境地磁存在很多干扰，比如金属管道、金属物体、手机等无线辐射。而卡尔曼滤波融合方法则要求有一个晃动接收传感器的过程，根据运动矢量进行初始化姿态。因此，无法对初始姿态为相对静止状态的被测物体进行姿态数据处理。
图1示出了本申请的一个实施例的一种姿态数据处理方法的步骤流程图。本实施例的一种姿态数据处理方法包括：
S210：根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态。
需要说明的是，此处的被测物体可以是一种VR控制器。此处的自身坐标系指的是以被测物体自身而建立的坐标系，会因被测物体自身发生移动而改变。此处的世界坐标系指的是一种系统的绝对坐标系，在没有建立用户坐标系之前画面上所有点的坐标都是以该坐标系的原点来确定各自的位置的，且不会因被测物体自身发生移动而改变。此处的被测物体的自身坐标系重力矢量指的是被测物体在自身坐标系中加速度计的测量结果。此处的预设状态指的是被测物体在空间中相对静止的状态。
此处的陀螺仪测量值指的是运动被测物体上的陀螺仪所测得的数据值，此处的运动被测物体可以是一种移动控制器，例如虚拟现实设备的手柄类控制器或指环类控制器。此处的陀螺仪指的是用高速回转体的动量矩敏感壳体相对惯性空间绕正交于自转轴的一个或二个轴的角运动检测装置。利用其他原理制成的角运动检测装置起同样功能的也称陀螺仪。此处的陀螺仪测量值指的是测量得到的角速度值。此处的模长即向量的模，数学术语，向量的长度叫做向量的模。由于陀螺仪测量值可以准确反映被测物体运动状态。通过上述方式，可以准确地确定被测物体的运动状态。
S220：当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量。
需要说明的是，被测物体的世界坐标系位置矢量指的是被测物体在世界坐标系中的位置矢量，此处的被测物体的自身坐标系位置矢量指的是被测物体在自身坐标系中的位置矢量。此处的超声位置数据包括被测物体在世界坐标系中的位置数据，被测物体在自身坐标系中的位置数据，由于矢量的计算至少需要两组位置数据，因此，需要根据至少两组超声位置数据确定并得到被测物体的世界坐标系位置矢量和被测物体的自身坐标系位置矢量。此处的超声位置数据可以是一种超声波测得的距离数据，即利用超声波测距原理(根据是接收器接到超声波时的时间差进行距离测量)测量得到的数据。
S230：对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
需要说明的是，此处的姿态是用来描述被测物体自身坐标系和被测物体世界坐标系之间相对关系。此处的被测物体世界坐标系重力矢量指的是被测物体在世界坐标系下的重力加速度，为一种常量，矢量方向竖直向下，可以表示为Gn，具体数值[0 0 9.8]，单位m/s2。
本申请实施例的方案中，在被测物体姿态初始化过程中，由于被测物体可能处于相对静止状态，导致无法对被测物体进行姿态数据处理。通过至少两组超声位置数据确定并得到被测物体的世界坐标系位置矢量和被测物体的自身坐标系位置矢量，结合被测物体的自身坐标系重力矢量和被测物体世界坐标系重力矢量，可以处理得到被测物体的姿态数据，实现被测物体处于预设状态时的姿态数据处理。
在一种可能的实现方式中，所述根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态，包括：获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模；获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模；根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态。
需要说明的是此处的模指的是向量的模，向量长度叫做向量的模。由于被测物体的陀螺仪测量值的模和被测物体的自身坐标系重力矢量的模，可以判断被测物体的运动状态，因此，可以确定所述被测物体是否处于预设状态。通过上述方式，可以准确地判断确定被测物体的运动状态，并判断其是否处于预设状态。
在一种可能的实现方式中，所述根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态，包括：当所述被测物体的陀螺仪测量值的模处于零值范围内时且当所述被测物体的自身坐标系重力矢量的模处于重力加速度阈值范围内时，确定所述被测物体处于预设状态，否则，确定所述被测物体不处于预设状态。
需要说明的是，由于被测物体可能发生较小位移或者测量存在较小误差，因此，可以根据被测物体的陀螺仪测量值的模处于零值范围内，且当被测物体的自身坐标系重力矢量的模处于重力加速度阈值范围内时，确定所述被测物体处于预设状态。具体地，设被测物体的自身坐标系重力矢量为[accx,accy,accz]，单位m/s2，设被测物体的陀螺仪测量值为[Gyrox,Gyroy,Gyroz]。判断条件是，被测物体的自身坐标系重力矢量的模长是否处于重力加速度阈值范围内，即接近9.8，且被测物体的陀螺仪测量值的模长是否处于零值范围内，即接近0。下式中的范围为优选值。
accx^2+accy^2+accz^2∈[9.5,10.1](单位为m/s^2)
Gyrox^2+Gyroy^2+Gyroz^2∈[-0.1,0.1](单位为弧度/s)
优选地，当被测物体的自身坐标系重力矢量的模长处于[9.5,10.1]，且被测物体的陀螺仪测量值的模长处于[-0.1,0.1]时，确定所述被测物体处于预设状态。通过上述方式，可以准确地判断确定被测物体的运动状态，并判断其是否处于预设状态。
在一种可能的实现方式中，所述获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模，包括：获取所述被测物体的陀螺仪三个轴的角速度测量值；利用平方根函数对所述陀螺仪三个轴的角速度测量值进行计算，得到所述被测物体的陀螺仪测量值的模。
需要说明的是，如图2所示，此处的陀螺仪的三个轴指的是空间直角坐标系中的x轴、y轴、z轴，也可以用陀螺仪坐标系的内环轴、外环轴、外框轴来分别表示。当陀螺仪转子高速旋转时，若外力矩作用于外环轴，陀螺仪将绕内环轴转动；若外力矩作用于内环轴，陀螺仪将绕外环轴转动。其转动角速度方向与外力矩作用方向互相垂直。
此处的角速度指的是被测物体做圆周运动时单位时间内转过的角度，用来描述被测物体绕圆心运动的快慢。此处的平方根，又叫二次方根，其中属于非负数的平方根称之为算术平方根，此处的平方根函数即Gyro_mod＝sqrt(Gyrox2+Gyroy2+Gyroz2)，其中，Gyrox、Gyroy、Gyroz为陀螺仪的三个轴的定位数据，单位rad/s。通过上述方式，可以准确计算得出被测物体的陀螺仪测量值的模。
在一种可能的实现方式中，所述获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模，包括：获取被测物体的加速度计三个轴的加速度测量值；利用平方根函数对所述加速度计三个轴的加速度测量值进行计算，得到所述被测物体的自身坐标系重力矢量的模。
需要说明的是，此处的加速度指的是被测物体的速度对时间的变化率，表示被测物体速度变化的快慢。此处的平方根函数即accx_mod＝sqrt(accx2+accy2+accz2)，其中，accx、accy、accz为加速度计三个轴的加速度测量值，单位m/s^2。通过上述方式，可以准确计算得出测物体自身坐标系重力矢量的模。
在一种可能的实现方式中，所述当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，包括：根据所述被测物体上的两个超声传感器在世界坐标系中的定位坐标，计算得到所述被测物体的第一位置矢量；根据所述被测物体上的两个超声传感器在自身坐标系中的位置坐标，计算得到所述被测物体的第二位置矢量。
需要说明的是，由于超声传感器之间的距离越大，进行姿态解算的准确度越高，为了能进行姿态解算，超声传感器之间的距离优选为3-4cm。由于每两个超声传感器之间夹角超过70°会产生定位盲区，因此，每两个超声传感器法线的夹角优选为70°。通过上述方法可以在增加超声传感器的测量范围的基础上，实现位置矢量的准确计算。
此处的超声位置数据包括被测物体在世界坐标系中的位置数据，被测物体在自身坐标系中的位置数据。具体地，设被测物体的自身坐标系为自身系(简称b系)；设被测物体的世界坐标系为惯性系(简称n系)。设超声传感器A、超声传感器B的定位结果A(x1,y1,z1),B(x2,y2,z2)，其中超声传感器A、超声传感器B设置于被测物体上。被测物体的世界坐标系位置矢量设为Pn即[x2-x1,y2-y1,z2-z1]。根据两个点在被测物体结构上的位置坐标A(a,b,c),B(d,e,f)，则被测物体的自身坐标系位置矢量为Pb即[d-a,e-b,f-c]。通过上述方式，可以准确计算得到被测物体的世界坐标系位置矢量和被测物体的自身坐标系位置矢量。
在一种可能的实现方式中，所述对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据，包括：根据所述被测物体的第一位置矢量、所述被测物体的第二位置矢量、所述被测物体的自身坐标系重力矢量、被测物体世界坐标系重力矢量计算得到所述被测物体自身坐标系到世界坐标系的姿态转移矩阵；对所述姿态转移矩阵进行转置运算，得到所述被测物体的姿态数据。
具体地，设被测体自身坐标系重力矢量为Gb，设被测物体自身坐标系到世界坐标系的姿态转移矩阵为Cbn。基于两个坐标系的转换关系，有
Gn＝Cbn*Gb (1)
Pn＝Cbn*Pb (2)
上式中，Cbn为b系到n系的姿态转移矩阵。其转置Cnb记为被测物体在世界坐标系中的姿态。求解过程：联立上面两个方程，将两边做叉乘，得
Gn×Pn＝(Cbn*Gb)×(Cbn*Pb)＝Cbn(Gb×Pb) (3)
联立(1)-(3)，得[Gn Pn Gn×Pn]＝Cbn[Gb Pb Gb×Pb]
所以，Cbn＝[Gn Pn Gn×Pn]*[Gb Pb Gb×Pb]-1
最后对姿态转移矩阵Cbn进行转置运算，得到被测物体的姿态数据Cnb。通过上述方式，可以准确地计算得到被测物体的姿态数据。
图3为根据本申请的另一实施例的姿态数据处理装置的示意性框图。本申请实施例的方案可以应用于电子设备，包括但不限于具有数据处理能力的电子设备等。
本实施例的姿态数据处理装置，包括：状态确定模块310，用于根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；位置计算模块320，用于当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；姿态处理模块330，用于对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
在另一些示例中，状态确定模块310，具体用于：获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模；获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模；根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态。
在另一些示例中，状态确定模块310，具体用于：当所述被测物体的陀螺仪测量值的模处于零值范围内时且当所述被测物体的自身坐标系重力矢量的模处于重力加速度阈值范围内时，确定所述被测物体处于预设状态，否则，确定所述被测物体不处于预设状态。
在另一些示例中，状态确定模块310，具体用于：获取所述被测物体的陀螺仪三个轴的角速度测量值；利用平方根函数对所述陀螺仪三个轴的角速度测量值进行计算，得到所述被测物体的陀螺仪测量值的模。
在另一些示例中，状态确定模块310，具体用于：获取被测物体的加速度计三个轴的加速度测量值；利用平方根函数对所述加速度计三个轴的加速度测量值进行计算，得到所述被测物体的自身坐标系重力矢量的模。
在另一些示例中，位置计算模块320，具体用于：根据所述被测物体上的两个超声传感器在世界坐标系中的定位坐标，计算得到所述被测物体的第一位置矢量；根据所述被测物体上的两个超声传感器在自身坐标系中的位置坐标，计算得到所述被测物体的第二位置矢量。
在另一些示例中，姿态处理模块330，具体用于：根据所述被测物体的第一位置矢量、所述被测物体的第二位置矢量、所述被测物体的自身坐标系重力矢量、被测物体世界坐标系重力矢量计算得到所述被测物体自身坐标系到世界坐标系的姿态转移矩阵；对所述姿态转移矩阵进行转置运算，得到所述被测物体的姿态数据。
参照图4，示出了根据本申请的另一实施例的电子设备的结构示意图，本申请具体实施例并不对电子设备的具体实现做限定。
如图4所示，该电子设备可以包括：处理器(processor)402、通信接口(Communications Interface)404、存储有程序410的存储器(memory)406、以及通信总线408。
处理器、通信接口、以及存储器通过通信总线完成相互间的通信。通信接口，用于与其它电子设备或服务器进行通信。处理器，用于执行程序，具体可以执行上述方法实施例中的相关步骤。具体地，程序可以包括程序代码，该程序代码包括计算机操作指令。
处理器可能是处理器CPU，或者是特定集成电路ASIC(Application SpecificIntegrated Circuit)，或者是被配置成实施本申请实施例的一个或多个集成电路。智能设备包括的一个或多个处理器，可以是同一类型的处理器，如一个或多个CPU；也可以是不同类型的处理器，如一个或多个CPU以及一个或多个ASIC。
存储器，用于存放程序。存储器可能包含高速RAM存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
程序具体可以用于使得处理器执行操作：根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
以上实施方式仅用于说明本申请实施例，而并非对本申请实施例的限制，有关技术领域的普通技术人员，在不脱离本申请实施例的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本申请实施例的范畴，本申请实施例的专利保护范围应由权利要求限定。上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。
为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本申请时可以把各单元的功能在同一个或多个软件和/或硬件中实现。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。
本领域技术人员应明白，本申请的实施例可提供为方法、系统或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定事务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本申请，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行事务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。
本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

Claims (10)

1.一种姿态数据处理方法，其特征在于，包括：
根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；
当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；
对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
2.根据权利要求1所述的方法，其中，所述根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态，包括：
获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模；
获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模；
根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态。
3.根据权利要求2所述的方法，其中，所述根据所述被测物体的陀螺仪测量值的模和所述被测物体的自身坐标系重力矢量的模，确定所述被测物体是否处于预设状态，包括：
当所述被测物体的陀螺仪测量值的模处于零值范围内时且当所述被测物体的自身坐标系重力矢量的模处于重力加速度阈值范围内时，确定所述被测物体处于预设状态，否则，确定所述被测物体不处于预设状态。
4.根据权利要求3所述的方法，其中，所述获取所述被测物体的陀螺仪测量值并计算所述陀螺仪测量值的模长，得到所述被测物体的陀螺仪测量值的模，包括：
获取所述被测物体的陀螺仪三个轴的角速度测量值；
利用平方根函数对所述陀螺仪三个轴的角速度测量值进行计算，得到所述被测物体的陀螺仪测量值的模。
5.根据权利要求3所述的方法，其中，所述获取被测物体的加速度计测量值并计算所述加速度计测量值的模长，得到所述被测物体的自身坐标系重力矢量的模，包括：
获取被测物体的加速度计三个轴的加速度测量值；
利用平方根函数对所述加速度计三个轴的加速度测量值进行计算，得到所述被测物体的自身坐标系重力矢量的模。
6.根据权利要求1所述的方法，其中，所述当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，包括：
根据所述被测物体上的两个超声传感器在世界坐标系中的定位坐标，计算得到所述被测物体的第一位置矢量；
根据所述被测物体上的两个超声传感器在自身坐标系中的位置坐标，计算得到所述被测物体的第二位置矢量。
7.根据权利要求1所述的方法，其中，所述对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据，包括：
根据所述被测物体的第一位置矢量、所述被测物体的第二位置矢量、所述被测物体的自身坐标系重力矢量、被测物体世界坐标系重力矢量计算得到所述被测物体自身坐标系到世界坐标系的姿态转移矩阵；
对所述姿态转移矩阵进行转置运算，得到所述被测物体的姿态数据。
8.一种姿态数据处理装置，包括：
状态确定模块，用于根据被测物体的自身坐标系重力矢量和陀螺仪测量值，确定所述被测物体是否处于预设状态；
位置计算模块，用于当所述被测物体处于预设状态时，根据至少两组超声位置数据确定所述被测物体的第一位置矢量和第二位置矢量，其中，所述第一位置矢量为所述被测物体的世界坐标系位置矢量，所述第二位置矢量为所述被测物体的自身坐标系位置矢量；
姿态处理模块，用于对所述第一位置矢量、所述第二位置矢量、所述被测物体的自身坐标系重力矢量以及被测物体世界坐标系重力矢量进行姿态处理，确定所述被测物体的姿态数据。
9.一种电子设备，包括：处理器、存储器、通信接口和通信总线，所述处理器、所述存储器和所述通信接口通过所述通信总线完成相互间的通信；所述存储器用于存放至少一可执行指令，所述可执行指令使所述处理器执行如权利要求1-7中任一项所述的方法对应的操作。
10.一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现如权利要求1-7中任一项所述的方法。
CN202211272001.2A 2022-08-31 2022-10-18 姿态数据处理方法、装置、电子设备和存储介质 Pending CN115574812A (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202211053988 2022-08-31
CN2022110539889 2022-08-31

Publications (1)

Publication Number Publication Date
CN115574812A true CN115574812A (zh) 2023-01-06

Family

ID=84585524

Family Applications (2)

Application Number Title Priority Date Filing Date
CN202211272001.2A Pending CN115574812A (zh) 2022-08-31 2022-10-18 姿态数据处理方法、装置、电子设备和存储介质
CN202211292599.1A Pending CN115639524A (zh) 2022-08-31 2022-10-21 超声定位数据处理方法、装置、电子设备和存储介质

Family Applications After (1)

Application Number Title Priority Date Filing Date
CN202211292599.1A Pending CN115639524A (zh) 2022-08-31 2022-10-21 超声定位数据处理方法、装置、电子设备和存储介质

Country Status (1)

Country Link
CN (2) CN115574812A (zh)

Also Published As

Publication number Publication date
CN115639524A (zh) 2023-01-24

Similar Documents

Publication Publication Date Title
CN102269592B (zh) 基于传感器的定向系统
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN102449436A (zh) 用于通过补偿信号通道之间的相对处理延迟来准确捕获惯性传感器数据的方法和装置
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN107917707B (zh) 一种任意姿态下行人方向的确定方法、装置及电子设备
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
Zhilenkov et al. Based on MEMS sensors man-machine interface for mechatronic objects control
CN106291567A (zh) 一种激光雷达、激光雷达系统及数据处理方法
CN112461269A (zh) 惯性测量单元标定方法、设备及服务器
CN111504314B (zh) Imu与刚体的位姿融合方法、装置、设备及存储介质
CN115164936A (zh) 高精地图制作中用于点云拼接的全局位姿修正方法及设备
CN109677508B (zh) 一种车辆运动数据获取方法、装置、设备和存储介质
CN111272199B (zh) 一种imu的安装误差角的标定方法及装置
CN108592902A (zh) 一种基于多传感器的定位设备及定位方法、系统和机械臂
CN115574812A (zh) 姿态数据处理方法、装置、电子设备和存储介质
CN102306054A (zh) 姿态感知设备及其定位、鼠标指针的控制方法和装置
CN115097379A (zh) 一种定位追踪方法、装置、设备及存储介质
US11620846B2 (en) Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device
CN112649001B (zh) 一种小型无人机姿态与位置解算方法
US10890444B2 (en) System and method for estimating three-dimensional measurements of physical objects
CN114593735A (zh) 一种位姿预测方法及装置
CN113465632A (zh) 传感器的校准方法、装置、设备和介质
He et al. A novel indoor positioning algorithm based on IMU
CN115561708A (zh) 超声定位方法、装置、电子设备和存储介质
CN112533142A (zh) 车辆定位方法、装置、计算机设备和存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination