CN111024122A - 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法 - Google Patents

基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法 Download PDF

Info

Publication number
CN111024122A
CN111024122A CN201911308832.9A CN201911308832A CN111024122A CN 111024122 A CN111024122 A CN 111024122A CN 201911308832 A CN201911308832 A CN 201911308832A CN 111024122 A CN111024122 A CN 111024122A
Authority
CN
China
Prior art keywords
data
gyroscope
axis
bluetooth
ultrasonic pen
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.)
Granted
Application number
CN201911308832.9A
Other languages
English (en)
Other versions
CN111024122B (zh
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.)
Shanghai Yuhao Information Technology Co Ltd
Original Assignee
Shanghai Yuhao Information Technology 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 Shanghai Yuhao Information Technology Co Ltd filed Critical Shanghai Yuhao Information Technology Co Ltd
Priority to CN201911308832.9A priority Critical patent/CN111024122B/zh
Publication of CN111024122A publication Critical patent/CN111024122A/zh
Application granted granted Critical
Publication of CN111024122B publication Critical patent/CN111024122B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Position Input By Displaying (AREA)
  • Navigation (AREA)

Abstract

本发明公开了基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其方法包括以下步骤：A、蓝牙传输：通过超声波笔上的九轴陀螺仪模块，采集的加速度、陀螺仪以及磁力计的数据，通过蓝牙模块发送给主机端。本发明采用蓝牙加九轴陀螺仪的偏差补偿方法，修正了超声波笔倾斜所带来的误差，提高了超声波笔的精度以及用户的书写体验，且其足够好用，通过卡尔曼滤波数据融合算法，能够快速精准的估算当前超声波笔的姿态，并且消除了超声波笔在书写过程过中的自然晃动所引入的噪声，并且成本低，只需要增加蓝牙和九轴陀螺仪硬件模块以及相应的软件程序，无需对原有的超声波笔定位模块做任何改动，即可实现倾斜校准的方案。

Description

基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法
技术领域
本发明涉及超声波笔技术领域，具体为基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法。
背景技术
随着互联网技术的发展，以及优质师资和内容的涌入，在线教育也被越来越多的用户所接受，然而目前在线学习场景的交互体验还很差，老师和学生之间的交流还只能通过声音和视屏，学生想要表达疑问或是解题思路，最好的方式应该是一边写一边说，如果缺乏这种白板交互，教学体验和学习质量也会大打折扣。
桌面投影加智能笔的产品也因此出现，桌面投影仪解决了屏幕输出的问题，而智能笔解决了输入的问题，目前市面上成熟的智能笔解决方案有三种，分别是点阵笔，电磁笔和超声波笔，经过分析，最终选用了超声波笔的方案，原因是点阵笔需要在特殊的点阵纸上书写，而电磁笔也需要在电磁板上书写，而超声波笔可以在任意的白纸上书写，能够完美还原书写体验，更加契合桌面投影的使用场景，也更能提升在线学习场景的交互体验。
传统的超声波笔，是利用两点超声定位技术定位笔的位置，从而获取到用户的书写信息，但是超声波笔的缺陷是书写精度很差，原因是超声发射器的位置离笔尖有一段距离，当笔发生倾斜的时候，超声接收器接收到的位置与实际书写的位置会有偏差，造成书写不准的情况，如果将超声波笔作为桌面投影的输入设备，由于书写区域和显示区域完全重合，所以超声波笔倾斜所带来的偏差会严重影响用户输入的准确度以及手写输入的体验，为此，我们提出基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法。
发明内容
本发明的目的在于提供基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，以解决上述背景技术中提出的问题。
为实现上述目的，本发明提供如下技术方案：基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其方法包括以下步骤：
A、蓝牙传输：通过超声波笔上的九轴陀螺仪模块，采集的加速度、陀螺仪以及磁力计的数据，通过蓝牙模块发送给主机端；
B、姿态角估算：主机端蓝牙模块接收到数据后，发送给数据解算单元，解算出笔的姿态角；
C、偏差补偿计算：主机端超声接收模块接收到笔的位置信息后，发送给数据解算单元，根据当前的姿态角，解算出倾斜偏差补偿的值和方向，最终计算出修正后的点坐标。
优选的，所述步骤A中，选用的是蓝牙BLE（蓝牙4.0，低功耗蓝牙），具体的实现方法是，超声波笔作为从机，作为数据的发送端，桌面投影作为主机，作为数据的接收端，从机在发送数据的时候要用notify的指令代替indicate指令，主机需要将连接间隔设置为10ms，连接延迟设置为0。
优选的，所述步骤B中，姿态角用于描述一个物体在三维空间中的方位，且陀螺仪能够提供沿X轴、Y轴和Z轴转动的瞬时角速度，加速度计能够提供沿X轴、Y轴和Z轴方向的加速度，地磁计能够提供沿X轴、Y轴和Z轴的磁场强度。
优选的，所述步骤B中的数据解算单元为卡尔曼滤波数据融合算法，其实现原理是将陀螺仪作为预测模型，而加速度计和地磁计作为观察模型，用加速度计和地磁极的数据更新陀螺仪的数据，加速度计和地磁计作为观察者，周期性的对陀螺仪的数据进行补偿，以修正陀螺仪长时间出现的漂移误差。
优选的，所述卡尔曼滤波算法的具体实现分为两个过程，第一个过程是预测过程，第二个过程是更新过程，总共包含5个方程式，前2个是预测过程的方程式，后3个是更新过程的方程式，五个方程式的具体实现如下：
1.状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：
Figure 142342DEST_PATH_IMAGE001
，其中，
Figure 646136DEST_PATH_IMAGE002
当前的姿态角的四元数，
Figure 682225DEST_PATH_IMAGE003
为状态转移矩阵，计算出新的姿态角四元数
Figure 308378DEST_PATH_IMAGE004
，
Figure 695497DEST_PATH_IMAGE005
为状态转移矩阵如下：
Figure 737403DEST_PATH_IMAGE006
，传入的陀螺仪数据的四元数为：
Figure 893578DEST_PATH_IMAGE007
，
Figure 690632DEST_PATH_IMAGE008
，其中，
Figure 237151DEST_PATH_IMAGE009
为陀螺仪数据的单位向量，
Figure 410644DEST_PATH_IMAGE010
为陀螺仪数据的原始向量：
Figure 421325DEST_PATH_IMAGE011
，
Figure 389281DEST_PATH_IMAGE012
表示陀螺仪的旋转弧度，
Figure 420166DEST_PATH_IMAGE013
陀螺仪数据采样间隔,
Figure 866191DEST_PATH_IMAGE014
角计算如下：
Figure 996958DEST_PATH_IMAGE015
；
2.协方差方程:
Figure 135816DEST_PATH_IMAGE016
，其中，
Figure 656927DEST_PATH_IMAGE017
预测的协方差，
Figure 906642DEST_PATH_IMAGE018
为上一时刻的协方差，
Figure 891916DEST_PATH_IMAGE019
为协方差迁移矩阵，
Figure 139358DEST_PATH_IMAGE020
为协方差的噪声，
Figure 210082DEST_PATH_IMAGE021
状态迁移矩阵如下：
Figure 263489DEST_PATH_IMAGE022
，
Figure 103269DEST_PATH_IMAGE023
，
Figure 521612DEST_PATH_IMAGE024
，协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，我们假定陀螺三个方向的误差都相同的，如果采样频率是100Hz的话，
Figure 814053DEST_PATH_IMAGE025
的误差为
Figure 671150DEST_PATH_IMAGE026
，
Figure 365437DEST_PATH_IMAGE027
协方差噪声的方程式如下：
Figure 954681DEST_PATH_IMAGE028
，
Figure 999997DEST_PATH_IMAGE030
，
Figure 395207DEST_PATH_IMAGE032
，
Figure 881683DEST_PATH_IMAGE033
；
3.卡尔曼系数方程：
Figure 969725DEST_PATH_IMAGE034
，其中，
Figure 236758DEST_PATH_IMAGE035
是公式
Figure 107762DEST_PATH_IMAGE036
得出的协方差，S残差方程式如下：
Figure 776641DEST_PATH_IMAGE037
，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵：
Figure 35584DEST_PATH_IMAGE038
，加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量
Figure 462017DEST_PATH_IMAGE039
四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量
Figure 464608DEST_PATH_IMAGE040
四元数转化为初始值为Y轴旋转后的向量；
4.状态更新方程：
Figure 987993DEST_PATH_IMAGE041
，
Figure 104590DEST_PATH_IMAGE042
由公式
Figure 346215DEST_PATH_IMAGE043
给出，A为四元数更新方程：
Figure 886918DEST_PATH_IMAGE044
，
Figure 264810DEST_PATH_IMAGE045
，其中，
Figure 803239DEST_PATH_IMAGE046
表示偏差向量，z为观测数据向量，b为
Figure 266581DEST_PATH_IMAGE039
转换为观测数据后的向量，K为卡尔曼系数；
5.更新协方差方程：
Figure 610975DEST_PATH_IMAGE047
，其中，
Figure 46635DEST_PATH_IMAGE048
由公式
Figure 552703DEST_PATH_IMAGE049
得出，K由公式
Figure 768920DEST_PATH_IMAGE050
得出,R为观测数据的方差。
与现有技术相比，本发明的有益效果如下：
本发明采用蓝牙加九轴陀螺仪的偏差补偿方法，修正了超声波笔倾斜所带来的误差，提高了超声波笔的精度以及用户的书写体验，且其足够好用，通过考尔曼滤波数据融合算法，能够快速精准的估算当前超声波笔的姿态，并且消除了超声波笔在书写过程过中的自然晃动所引入的噪声，并且成本低，只需要增加蓝牙和九轴陀螺仪硬件模块以及相应的软件程序，无需对原有的超声波笔定位模块做任何改动，即可实现倾斜校准的方案。
附图说明
图1为本发明方法原理示意图；
图2为本发明超声波笔倾斜示意图；
图3为本发明姿态角示意图；
图4为本发明超声笔倾斜几何示意图；
图5为本发明超声笔倾斜三维坐标示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-5，基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其方法包括以下步骤：
A、蓝牙传输：通过超声波笔上的九轴陀螺仪模块，采集的加速度、陀螺仪以及磁力计的数据，通过蓝牙模块发送给主机端；
B、姿态角估算：主机端蓝牙模块接收到数据后，发送给数据解算单元，解算出笔的姿态角；
C、偏差补偿计算：主机端超声接收模块接收到笔的位置信息后，发送给数据解算单元，根据当前的姿态角，解算出倾斜偏差补偿的值和方向，最终计算出修正后的点坐标。
采用蓝牙加九轴陀螺仪的偏差补偿方法，修正了超声波笔倾斜所带来的误差，提高了超声波笔的精度以及用户的书写体验，且其足够好用，通过考尔曼滤波数据融合算法，能够快速精准的估算当前超声波笔的姿态，并且消除了超声波笔在书写过程过中的自然晃动所引入的噪声，并且成本低，只需要增加蓝牙和九轴陀螺仪硬件模块以及相应的软件程序，无需对原有的超声波笔定位模块做任何改动，即可实现倾斜校准的方案。
步骤A中，选用的是蓝牙BLE（蓝牙4.0，低功耗蓝牙），因为超声波笔结构设计上的限制，留给电池的空间很小，使用蓝牙BLE的方案，可以保证超声波笔连续工作10个小时以上，具体的实现方法是，超声波笔作为从机，作为数据的发送端，桌面投影作为主机，作为数据的接收端，从机在发送数据的时候要用notify的指令代替indicate指令，因为notify指令不需要等待对端响应就可以发送下一条数据，主机需要将连接间隔设置为10ms，连接延迟设置为0，虽然会增加数据的传输的不稳定性和功耗，但总体满足了蓝牙传输方案的速率和功耗要求。
步骤B中，姿态角用于描述一个物体在三维空间中的方位，且陀螺仪能够提供沿X轴、Y轴和Z轴转动的瞬时角速度，加速度计能够提供沿X轴、Y轴和Z轴方向的加速度，由于重力沿Z轴指向地面的方向，所以如果静止不动，可以根据重力在X轴、Y轴和Z轴的各个分量，准确的测量出当前的姿态角，地磁计能够提供沿X轴、Y轴和Z轴的磁场强度，通过测量地磁场的方位，能够计算出偏航角。
步骤B中的数据解算单元为卡尔曼滤波数据融合算法，选用卡尔曼滤波数据融合算法的原因有两点：第一是这三组数据线性相关的，容易建立预测和观察模型，符合卡尔曼滤波的使用场景；第二是卡尔曼滤波算法比较成熟，计算量小，在该场景下足够好用，其实现原理是将陀螺仪作为预测模型，而加速度计和地磁计作为观察模型，用加速度计和地磁极的数据更新陀螺仪的数据，加速度计和地磁计作为观察者，周期性的对陀螺仪的数据进行补偿，以修正陀螺仪长时间出现的漂移误差。
卡尔曼滤波算法的具体实现分为两个过程，第一个过程是预测过程，第二个过程是更新过程，总共包含5个方程式，前2个是预测过程的方程式，后3个是更新过程的方程式，五个方程式的具体实现如下：
1.状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：
Figure 589109DEST_PATH_IMAGE051
，其中，
Figure 879276DEST_PATH_IMAGE052
当前的姿态角的四元数，
Figure 556245DEST_PATH_IMAGE053
为状态转移矩阵，计算出新的姿态角四元数
Figure 994180DEST_PATH_IMAGE054
，
Figure 618059DEST_PATH_IMAGE055
为状态转移矩阵如下：
Figure 825049DEST_PATH_IMAGE056
，传入的陀螺仪数据的四元数为：
Figure 938499DEST_PATH_IMAGE057
，
Figure 863730DEST_PATH_IMAGE058
，其中，
Figure 25721DEST_PATH_IMAGE059
为陀螺仪数据的单位向量，
Figure 352797DEST_PATH_IMAGE060
为陀螺仪数据的原始向量：
Figure 574831DEST_PATH_IMAGE061
，
Figure 721778DEST_PATH_IMAGE062
表示陀螺仪的旋转弧度，
Figure 684530DEST_PATH_IMAGE063
陀螺仪数据采样间隔,
Figure 866113DEST_PATH_IMAGE064
角计算如下：
Figure 321365DEST_PATH_IMAGE065
；
2.协方差方程:
Figure 221188DEST_PATH_IMAGE066
，其中，
Figure 724982DEST_PATH_IMAGE067
预测的协方差，
Figure 761071DEST_PATH_IMAGE068
为上一时刻的协方差，
Figure 387224DEST_PATH_IMAGE069
为协方差迁移矩阵，
Figure 446447DEST_PATH_IMAGE070
为协方差的噪声，
Figure 816249DEST_PATH_IMAGE071
状态迁移矩阵如下：
Figure 972423DEST_PATH_IMAGE072
，
Figure 769478DEST_PATH_IMAGE073
，
Figure 378314DEST_PATH_IMAGE074
，协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，我们假定陀螺三个方向的误差都相同的，如果采样频率是100Hz的话，
Figure 223910DEST_PATH_IMAGE075
的误差为
Figure 234592DEST_PATH_IMAGE076
，
Figure 202548DEST_PATH_IMAGE077
协方差噪声的方程式如下：
Figure 298680DEST_PATH_IMAGE078
，
Figure 947967DEST_PATH_IMAGE079
，
Figure 78734DEST_PATH_IMAGE080
，
Figure 217591DEST_PATH_IMAGE081
；
3.卡尔曼系数方程：
Figure 535440DEST_PATH_IMAGE082
，其中，
Figure 50735DEST_PATH_IMAGE083
是公式
Figure 973692DEST_PATH_IMAGE084
得出的协方差，S残差方程式如下：
Figure 283450DEST_PATH_IMAGE085
，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵：
Figure 88595DEST_PATH_IMAGE086
，加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量
Figure 142002DEST_PATH_IMAGE087
四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量
Figure 919465DEST_PATH_IMAGE087
四元数转化为初始值为Y轴旋转后的向量；
4.状态更新方程：
Figure 400125DEST_PATH_IMAGE088
，
Figure 958145DEST_PATH_IMAGE089
由公式
Figure 549664DEST_PATH_IMAGE090
给出，A为四元数更新方程：
Figure 509529DEST_PATH_IMAGE091
，
Figure 98774DEST_PATH_IMAGE092
，其中，
Figure 878511DEST_PATH_IMAGE093
表示偏差向量，z为观测数据向量，b为
Figure 273720DEST_PATH_IMAGE087
转换为观测数据后的向量，K为卡尔曼系数；
5.更新协方差方程：
Figure 88092DEST_PATH_IMAGE094
，其中，
Figure 910555DEST_PATH_IMAGE095
由公式
Figure 118201DEST_PATH_IMAGE096
得出，K由公式
Figure 317101DEST_PATH_IMAGE097
得出,R为观测数据的方差。
图2中，O点表示笔尖的位置，A点表示超声发射器的位置，OA表示了当前笔的倾斜状态，O点是用户书写的真实坐标，A点是通过超声定位读取的坐标，B点是O点在超声发射平面的映射，超声波笔偏差补偿的目的，就是将获得的A点坐标，校正到B点的位置，获取B点坐标的实现原理，是通过获取OA向量在三维空间的姿态角，算出OB与OA之间的夹角α，以及AB的水平偏角β，已知高度OB和A点的坐标，就可以求得B点的坐标；图4中，已知A点的坐标，OA向量的姿态角以及OB的高度，现在只需要求得角α以及角β即可求得B点的坐标，为了便于计算，我们将OA向量放到三维坐标系中；图5中，1.角α的求解，所示的ax、ay、az 为向量OA在X、Y、Z轴上的投影，为了方便计算，我们设OA为单位向量，α角的求解如下：
Figure 985980DEST_PATH_IMAGE098
，
Figure 979344DEST_PATH_IMAGE099
，
Figure 609039DEST_PATH_IMAGE100
，
Figure 611630DEST_PATH_IMAGE101
，2.角β求解，已知偏航角为ψ，但ψ是相对于地磁北向的水平偏角，然而β角为相对纸面方向的水平偏角，所以需要将超声波笔测算的ψ角减去投影仪设备测算的ψ，即可得出最终的β角，3.B点坐标求解，根据图4所示，OB的高度即纸面到超声发射器的距离，是已知量，又已知角α，可以求得AB即倾斜偏差值，A点是超声波定位坐标，又已知角β，可以算出补偿的方位，即可求得B点的坐标。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (5)

1.基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：其方法包括以下步骤：
A、蓝牙传输：通过超声波笔上的九轴陀螺仪模块，采集的加速度、陀螺仪以及磁力计的数据，通过蓝牙模块发送给主机端；
B、姿态角估算：主机端蓝牙模块接收到数据后，发送给数据解算单元，解算出笔的姿态角；
C、偏差补偿计算：主机端超声接收模块接收到笔的位置信息后，发送给数据解算单元，根据当前的姿态角，解算出倾斜偏差补偿的值和方向，最终计算出修正后的点坐标。
2.根据权利要求1所述的基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：所述步骤A中，选用的是蓝牙BLE（蓝牙4.0，低功耗蓝牙），具体的实现方法是，超声波笔作为从机，作为数据的发送端，桌面投影作为主机，作为数据的接收端，从机在发送数据的时候要用notify的指令代替indicate指令，主机需要将连接间隔设置为10ms，连接延迟设置为0。
3.根据权利要求1所述的基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：所述步骤B中，姿态角用于描述一个物体在三维空间中的方位，且陀螺仪能够提供沿X轴、Y轴和Z轴转动的瞬时角速度，加速度计能够提供沿X轴、Y轴和Z轴方向的加速度，地磁计能够提供沿X轴、Y轴和Z轴的磁场强度。
4.根据权利要求1所述的基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：所述步骤B中的数据解算单元为卡尔曼滤波数据融合算法，其实现原理是将陀螺仪作为预测模型，而加速度计和地磁计作为观察模型，用加速度计和地磁极的数据更新陀螺仪的数据，加速度计和地磁计作为观察者，周期性的对陀螺仪的数据进行补偿，以修正陀螺仪长时间出现的漂移误差。
5.根据权利要求4所述的基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：所述卡尔曼滤波算法的具体实现分为两个过程，第一个过程是预测过程，第二个过程是更新过程，总共包含5个方程式，前2个是预测过程的方程式，后3个是更新过程的方程式，五个方程式的具体实现如下：
1.状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：
Figure 12259DEST_PATH_IMAGE002
，其中，
Figure 882257DEST_PATH_IMAGE004
当前的姿态角的四元数，
Figure 337509DEST_PATH_IMAGE006
为状态转移矩阵，计算出新的姿态角四元数
Figure 175015DEST_PATH_IMAGE008
，
Figure 741126DEST_PATH_IMAGE010
为状态转移矩阵如下：
Figure 964165DEST_PATH_IMAGE012
，传入的陀螺仪数据的四元数为：
Figure 590319DEST_PATH_IMAGE014
，
Figure 711859DEST_PATH_IMAGE016
，其中，
Figure 19343DEST_PATH_IMAGE018
为陀螺仪数据的单位向量，
Figure 175518DEST_PATH_IMAGE020
为陀螺仪数据的原始向量：
Figure 972573DEST_PATH_IMAGE022
，
Figure 581409DEST_PATH_IMAGE024
表示陀螺仪的旋转弧度，
Figure 174808DEST_PATH_IMAGE026
陀螺仪数据采样间隔,
Figure 185489DEST_PATH_IMAGE028
角计算如下：
Figure 153445DEST_PATH_IMAGE030
；
2.协方差方程:
Figure 249577DEST_PATH_IMAGE032
，其中，
Figure 898864DEST_PATH_IMAGE034
预测的协方差，
Figure 29631DEST_PATH_IMAGE036
为上一时刻的协方差，
Figure 168488DEST_PATH_IMAGE038
为协方差迁移矩阵，
Figure 673288DEST_PATH_IMAGE040
为协方差的噪声，
Figure 188583DEST_PATH_IMAGE038
状态迁移矩阵如下：
Figure 173856DEST_PATH_IMAGE042
，
Figure 483615DEST_PATH_IMAGE044
,
Figure 226443DEST_PATH_IMAGE046
，协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，我们假定陀螺三个方向的误差都相同的，如果采样频率是100Hz的话，
Figure 968265DEST_PATH_IMAGE048
的误差为
Figure 808045DEST_PATH_IMAGE050
，
Figure 288705DEST_PATH_IMAGE052
协方差噪声的方程式如下：
Figure 784409DEST_PATH_IMAGE054
，
Figure 375927DEST_PATH_IMAGE056
，
Figure 335793DEST_PATH_IMAGE058
，
Figure 174305DEST_PATH_IMAGE060
；
3.卡尔曼系数方程：
Figure 954042DEST_PATH_IMAGE062
，其中，
Figure 286934DEST_PATH_IMAGE064
是公式
Figure 101306DEST_PATH_IMAGE066
得出的协方差，S残差方程式如下：
Figure 552797DEST_PATH_IMAGE068
，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵：
Figure 6781DEST_PATH_IMAGE070
，加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量
Figure 205681DEST_PATH_IMAGE072
四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量
Figure DEST_PATH_IMAGE073
四元数转化为初始值为Y轴旋转后的向量；
4.状态更新方程：
Figure DEST_PATH_IMAGE075
，
Figure 812243DEST_PATH_IMAGE076
由公式
Figure 805607DEST_PATH_IMAGE078
给出，A为四元数更新方程：
Figure 248352DEST_PATH_IMAGE080
，
Figure 250943DEST_PATH_IMAGE082
，其中，
Figure 712011DEST_PATH_IMAGE084
表示偏差向量，z为观测数据向量，b为
Figure DEST_PATH_IMAGE085
转换为观测数据后的向量，K为卡尔曼系数；
5.更新协方差方程：
Figure DEST_PATH_IMAGE087
，其中，
Figure DEST_PATH_IMAGE089
由公式
Figure DEST_PATH_IMAGE091
得出，K由公式
Figure DEST_PATH_IMAGE093
得出,R为观测数据的方差。
CN201911308832.9A 2019-12-18 2019-12-18 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法 Active CN111024122B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911308832.9A CN111024122B (zh) 2019-12-18 2019-12-18 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911308832.9A CN111024122B (zh) 2019-12-18 2019-12-18 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法

Publications (2)

Publication Number Publication Date
CN111024122A true CN111024122A (zh) 2020-04-17
CN111024122B CN111024122B (zh) 2024-01-19

Family

ID=70210357

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911308832.9A Active CN111024122B (zh) 2019-12-18 2019-12-18 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法

Country Status (1)

Country Link
CN (1) CN111024122B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060279549A1 (en) * 2005-06-08 2006-12-14 Guanglie Zhang Writing system
CN101872260A (zh) * 2010-06-03 2010-10-27 张通达 一种远程交互笔及书写笔迹检测方法
US20120013578A1 (en) * 2010-07-15 2012-01-19 Avermedia Information, Inc. Pen-shaped pointing device and shift control method thereof
CN102650527A (zh) * 2012-05-25 2012-08-29 北京航空航天大学 一种基于时间序列分析消噪的光纤陀螺温度补偿方法
CN103019414A (zh) * 2012-11-02 2013-04-03 浙江大学 基于imu的电子手写笔的笔迹估计方法
CN105159450A (zh) * 2015-08-25 2015-12-16 中国运载火箭技术研究院 一种便携式可交互桌面级虚拟现实系统
CN105738905A (zh) * 2016-05-16 2016-07-06 北京信息科技大学 一种减少盲区的室内定位系统及方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060279549A1 (en) * 2005-06-08 2006-12-14 Guanglie Zhang Writing system
CN101872260A (zh) * 2010-06-03 2010-10-27 张通达 一种远程交互笔及书写笔迹检测方法
US20120013578A1 (en) * 2010-07-15 2012-01-19 Avermedia Information, Inc. Pen-shaped pointing device and shift control method thereof
CN102650527A (zh) * 2012-05-25 2012-08-29 北京航空航天大学 一种基于时间序列分析消噪的光纤陀螺温度补偿方法
CN103019414A (zh) * 2012-11-02 2013-04-03 浙江大学 基于imu的电子手写笔的笔迹估计方法
CN105159450A (zh) * 2015-08-25 2015-12-16 中国运载火箭技术研究院 一种便携式可交互桌面级虚拟现实系统
CN105738905A (zh) * 2016-05-16 2016-07-06 北京信息科技大学 一种减少盲区的室内定位系统及方法
CN106933390A (zh) * 2017-03-08 2017-07-07 吉林大学 惯性技术和超声波相结合的触控笔位姿检测方法及系统
CN109001787A (zh) * 2018-05-25 2018-12-14 北京大学深圳研究生院 一种姿态角解算与定位的方法及其融合传感器

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
封家鹏: "双目标三维超声定位笔式人机交互装置", 《中国优秀硕士学位论文全文数据库信息科技辑》, no. 09, pages 140 - 1 *
曹丽等: "利用加速度计和角速度仪的笔杆运动姿态检测", 《仪器仪表学报》, vol. 29, no. 4, pages 831 - 835 *

Also Published As

Publication number Publication date
CN111024122B (zh) 2024-01-19

Similar Documents

Publication Publication Date Title
Azuma et al. A motion-stabilized outdoor augmented reality system
CN110375738A (zh) 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
US8552978B2 (en) 3D pointing device and method for compensating rotations of the 3D pointing device thereof
CN104620203A (zh) 交互式显示系统中的绝对与相对定位传感器融合
CN111091587B (zh) 一种基于视觉标志物的低成本动作捕捉方法
CN102192742B (zh) 一种导航方法和装置
US20110199298A1 (en) Pointer with motion sensing resolved by data merging
CN103136912A (zh) 运动姿态捕捉系统
CN103512584A (zh) 导航姿态信息输出方法、装置及捷联航姿参考系统
CN102575933A (zh) 地图图像综合数据库生成系统以及地图图像综合数据库生成程序
US11698687B2 (en) Electronic device for use in motion detection and method for obtaining resultant deviation thereof
CN110388919B (zh) 增强现实中基于特征图和惯性测量的三维模型定位方法
CN102750020A (zh) 获取空中鼠标位移的方法、空中鼠标及空中鼠标控制系统
CN110186478A (zh) 用于捷联式惯导系统的惯性传感器选型方法及系统
CN110487269A (zh) Gps/ins组合导航方法、装置、存储介质与电子设备
CN111932637A (zh) 一种车身相机外参自适应标定方法和装置
CN111024122B (zh) 基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法
CN109029499A (zh) 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN117232499A (zh) 多传感器融合的点云地图构建方法、装置、设备及介质
CN108932353A (zh) 应用混合现实技术实现建筑模型与实体间的定位方法
CN110030991B (zh) 融合陀螺和磁强计的飞行物高速旋转角运动测量方法
Wang et al. Uav attitude measurement based on enhanced mahony complementary filter
CN114777773A (zh) 摄像头位姿补偿方法、装置、电子设备和可读存储介质
CN111324136B (zh) 一种微型飞行器位置与距离复合作用导引方法
CN103034345B (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
GR01 Patent grant
GR01 Patent grant