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

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.状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：，其中，当前的姿态角的四元数，为状态转移矩阵，计算出新的姿态角四元数 ，为状态转移矩阵如下：，传入的陀螺仪数据的四元数为：

，

，其中， 为陀螺仪数据的单位向量，为陀螺仪数据的原始向量：，表示陀螺仪的旋转弧度，陀螺仪数据采样间隔, 角计算如下：；

2.协方差方程: ，其中，预测的协方差，为上一时刻的协方差，为协方差迁移矩阵，为协方差的噪声，状态迁移矩阵如下：，

，

，

协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，我们假定陀螺三个方向的误差都相同的，如果采样频率是100Hz的话，的误差为，协方差噪声的方程式如下：，

，

，

；

3.卡尔曼系数方程：，其中，是公式得出的协方差，S残差方程式如下：，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵： ，加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量四元数转化为初始值为Y轴旋转后的向量；

4.状态更新方程：，由公式给出，A为四元数更新方程：，，其中，表示偏差向量，z为观测数据向量，b为转换为观测数据后的向量，K为卡尔曼系数；

5.更新协方差方程：，其中，由公式得出，K由公式得出,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.状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：，其中，当前的姿态角的四元数，为状态转移矩阵，计算出新的姿态角四元数，为状态转移矩阵如下：，传入的陀螺仪数据的四元数为：，，其中，为陀螺仪数据的单位向量，为陀螺仪数据的原始向量：，表示陀螺仪的旋转弧度，陀螺仪数据采样间隔, 角计算如下：；

2.协方差方程:，其中，预测的协方差，为上一时刻的协方差，为协方差迁移矩阵，为协方差的噪声，状态迁移矩阵如下：

，

，

，

协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，我们假定陀螺三个方向的误差都相同的，如果采样频率是100Hz的话，的误差为，协方差噪声的方程式如下：，

，

，

；

3.卡尔曼系数方程：，其中，是公式得出的协方差，S残差方程式如下：，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵： ，加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量四元数转化为初始值为Y轴旋转后的向量；

4.状态更新方程：，由公式给出，A为四元数更新方程：，，其中，表示偏差向量，z为观测数据向量，b为转换为观测数据后的向量，K为卡尔曼系数；

5.更新协方差方程：，其中，由公式得出，K由公式得出,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为单位向量，α角的求解如下：，，，，2.角β求解，已知偏航角为ψ，但ψ是相对于地磁北向的水平偏角，然而β角为相对纸面方向的水平偏角，所以需要将超声波笔测算的ψ角减去投影仪设备测算的ψ，即可得出最终的β角，3.B点坐标求解，根据图4所示，OB的高度即纸面到超声发射器的距离，是已知量，又已知角α，可以求得AB即倾斜偏差值，A点是超声波定位坐标，又已知角β，可以算出补偿的方位，即可求得B点的坐标。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (1)

1.基于蓝牙和九轴陀螺仪的超声波笔倾斜偏差补偿方法，其特征在于：其方法包括以下步骤：

A、蓝牙传输：通过超声波笔上的九轴陀螺仪模块，采集的加速度、陀螺仪以及磁力计的数据，通过蓝牙模块发送给主机端，选用的是蓝牙BLE，蓝牙4.0，低功耗蓝牙，具体的实现方法是，超声波笔作为从机，作为数据的发送端，桌面投影作为主机，作为数据的接收端，从机在发送数据的时候要用notify的指令代替indicate指令，主机需要将连接间隔设置为10ms，连接延迟设置为0；

B、姿态角估算：主机端蓝牙模块接收到数据后，发送给数据解算单元，解算出笔的姿态角，姿态角用于描述一个物体在三维空间中的方位，且陀螺仪能够提供沿X轴、Y轴和Z轴转动的瞬时角速度，加速度计能够提供沿X轴、Y轴和Z轴方向的加速度，地磁计能够提供沿X轴、Y轴和Z轴的磁场强度，数据解算单元为卡尔曼滤波数据融合算法，其实现原理是将陀螺仪作为预测模型，而加速度计和地磁计作为观察模型，用加速度计和地磁极的数据更新陀螺仪的数据，加速度计和地磁计作为观察者，周期性的对陀螺仪的数据进行补偿，以修正陀螺仪长时间出现的漂移误差，卡尔曼滤波算法的具体实现分为两个过程，第一个过程是预测过程，第二个过程是更新过程，总共包含5个方程式，前2个是预测过程的方程式，后3个是更新过程的方程式，五个方程式的具体实现如下：(1).状态预测方程式，主要用陀螺仪的数据预测当前的姿态角：q_k|k+1＝O_ωq_k|k，其中，q_k|k当前的姿态角的四元数，计算出新的姿态角四元数q_k+1|k，o_ω为状态转移矩阵如下：传入的陀螺仪数据的四元数为： q₄＝cos(θ/2)，其中，为陀螺仪数据的单位向量，为陀螺仪数据的原始向量：θ表示陀螺仪的旋转弧度，δt陀螺仪数据采样间隔,θ角计算如下：(2).协方差方程:P_k+1|k＝ΦP_k|kΦ^T+Q_d，其中，P_k+1|k预测的协方差，P_k|k为上一时刻的协方差，Φ为协方差迁移矩阵，Q_k为协方差的噪声，Φ状态迁移矩阵如下：

协方差所相关的两个变量分别是陀螺仪本身的偏差以及采样频率的噪声，陀螺三个方向的误差都相同的，采样频率是100Hz，θ的误差为10^-6，Q_d协方差噪声的方程式如下：

(3).卡尔曼系数方程：K＝PH^TS^-1，其中，P是公式P_k+1|k＝ΦP_k|kΦ^T+Q_d得出的协方差，S残差方程式如下：S＝HPH^T+R，其中，R为观测数据的方差，观测数据为加速度数据和地磁数据，它们分别独自参与更新过程，也就是卡尔曼滤波算法的后三个方程式，设定加速度的标准差为0.05，地磁计的标准差为0.1，H为预测量到观测量的转换矩阵：加速度和地磁计的转换矩阵稍有不同，在加速度数据更新过程中，预测量q_k+1|k四元数需要转换为初始值为Z轴旋转后的向量，在地磁数据更新过程中，预测量q_k+1|k四元数转化为初始值为Y轴旋转后的向量；(4).状态更新方程：q_k+1|k+1＝q_k+1|k+A，q_k+1|k由公式q_k+1|k＝O_ωq_k|k给出，A为四元数更新方程：δq＝(z-b)K，其中，δq表示偏差向量，z为观测数据向量，b为q_k+1|k转换为观测数据后的向量，K为卡尔曼系数；(5).更新协方差方程：P_k+1|k+1＝(I_6×6-KH)P_k+1|k(I_6×6-KH)^T+KRK^T，其中，P_k+1|k由公式P_k+1|k＝ΦP_k|kΦ^T+Q_d得出，K由公式K＝PH^TS^-1得出,R为观测数据的方差；

C、偏差补偿计算：主机端超声接收模块接收到笔的位置信息后，发送给数据解算单元，根据当前的姿态角，解算出倾斜偏差补偿的值和方向，最终计算出修正后的点坐标。