惯性技术和超声波相结合的触控笔位姿检测方法及系统与流程

本发明属于人机交互领域，涉及一种触控笔位姿检测方法及系统，具体涉及一种惯性技术和超声波相结合的触控笔位姿检测方法及系统。

背景技术：

近年来，随着人机交互技术的不断发展，人机交互已不再局限于以往单一的二维人机交互，而是逐渐将三维笔式交互技术引入到人机交互中，是一种高效自然的交互方式。

在人机交互的过程中，用户常需要进行打开各种文件、浏览各种网页等操作，然而在教室或会议等场景使用白板时，距离白板较远的用户，由于二维(平面内)受到距离和笔的姿态的限制无法完成的操作。增大用户的操作距离，同时使操控具有指向性是解决该问题的方法之一，这就需要解决笔的远距离位置和姿态检测问题。

目前已有一些关于解决笔的姿态和位置问题的方法。

中国专利“基于六轴传感器的角度测量装置”(申请号201620079085.1)公开了一种基于六轴传感器的角度测量装置，提高了测量角度信息的准确性和稳定性。中国专利“基于mpu6050六轴传感器的悬空鼠标系统”(申请号201620067175.9)公开了一种mpu6050六轴传感器的悬空鼠标系统摆脱桌面鼠标的限制,实现在远距离的三维空间内获得鼠标的操作功能。中国专利“一种基于重力向量和角速度向量的汽车变道识别方法”(申请号201610087916.4)公开了一种基于重力向量和角速度向量的汽车变道识别方法，利用姿态角信息选择可能的变道组合。但这些方法所述的检测出的姿态信息，不能矫正航向角出现的漂移，且不能准确检测出距离信息，一旦涉及三维空间的笔式交互，就会因距离因素及交互方式的改变带来操作角度受限和触控笔姿态识别不清等问题。

中国专利“加速度、陀螺仪和磁场九轴传感器的空间轨迹定位系统”(申请号201310452478.3)公开了一种加速度、陀螺仪和磁场九轴传感器的空间轨迹定位系统，提到利用九轴传感器定出姿态角，其中利用三轴地磁矫正陀螺仪的航向角，实现了姿态角的精确测量，但未考虑到三轴地磁容易受周围物体磁场的影响，从而影响测量精度。中国专利“基于九轴mems传感器的农业机械全姿态角更新方法”(申请号201510664990.3)公开了一种基于九轴mems传感器的农业机械全姿态角更新方法，提到利用九轴传感器测量出姿态角，利用加速度计测出的数据经过二次积分计算出位置信息，但未考虑加速度计噪声比较大，对运动极为敏感，在短时间内可靠性不高，因此在定出位置信息时误差较大。中国专利“基于九加速度敏感单元的六轴加速度传感器的布局方法”(申请号200810237023.9)公开了一种九加速度敏感单元的六轴加速度传感器的布局方法，提到利用加速度传感器获取角速度信息，但未考虑加速度计噪声比较大，在短期使用时误差较大，可靠性差。

技术实现要素：

本发明提供一种惯性技术和超声波相结合的触控笔位姿检测方法及系统，基于陀螺仪获取姿态信息，利用加速度计获取的加速度值修正陀螺仪获取的姿态信息，通过超声波定位出前一时刻的坐标值和当前时刻的坐标值，利用获得的三维坐标信息修正航向角等方法最终计算出触控笔的姿态和坐标值。

本发明中触控笔的超声波定位精度达到毫米量级，而且解决了触控笔在六轴传感器单独定位的情况下容易在0°和180°出现角度突变的技术问题同时还可以实现修正航向角的功能，达到实现笔以任意姿态、任意角度、任意距离与界面进行交互的目的。

本发明采取的技术方案是，包括如下步骤：

(1)采集mems六轴加速度计及陀螺仪传感器获取的角速度与加速度数据，测量超声波从触控笔上超声波发射传感器到接收传感器上的传播时间，并对采集和测量到的信息进行处理，得到输出的数据信息；

(2)由mems六轴加速度计及陀螺仪传感器输出的数据信息，用处理器计算触控笔的姿态角信息，所述触控笔的姿态角信息是触控笔在三维坐标系下的俯仰角θ、航向角φ和横滚角

(3)由三路超声波传播的时间信息，计算触控笔的坐标信息，所述触控笔的坐标信息是指触控笔在三维坐标系下的三维坐标(x,y,z)；

(4)由获取的触控笔的位置信息获取航向角φ'，对mems六轴加速度计及陀螺仪传感器获取的航向角和超声波获取的航向角进行互补融合，计算出修正后的航向角φ″；

(5)输出坐标信息和修正后的姿态角信息，确定触控笔当前的位姿。

上述方法描述的采集mems六轴加速度计及陀螺仪传感器上的角速度和加速度值，其特征在于采集mems六轴加速度计及陀螺仪传感中三轴陀螺仪和三轴加速度计测出的触控笔的角速度值及加速度值；

上述方法描述的对信号进行处理包括a/d转换，是为了将模拟信息中的的角速度值和加速度值由a/d转换器转换成数字信息的角速度值(wx,wy,wz)和加速度值a＝(ax,ay,az)；

上述方法描述的对信号进行处理包括增益控制，是为了将接收信息幅值调整至适于定位装置动态范围，以保持信息的稳定性；

上述方法描述的对信息进行处理包括滤波，滤波是为了滤除噪声信号；

上述方法描述的对采集到的加速度值进行归一化处理，是为了将加速度计的三维向量转化为三维单位向量；

上述方法描述的触控笔的姿态角信息计算步骤如下：

1)初始化四元数：q0＝1，q1＝0，q2＝0，q3＝0，四元数表达式：q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k，所述四元数代表多维实数空间的一个四维空间，同时也可以代表一个复数的二维空间，i、j、k分别代表x轴、y轴、z轴上的虚数单位；

2)利用四元数根据欧拉定理计算出姿态矩阵a：

3)利用互补滤波算法，实现用加速度计测得的重力向量和陀螺仪获取的角速度信息推算出的重力向量的叉积来修正陀螺仪获取的角速度值的的零偏误差：

①用陀螺仪获取的角速度信息(wx,wy,wz)推算出陀螺仪三轴的重力向量(即重力单位向量在机体坐标系中的重力加速度)，g＝(vx,vy,vz)＝a*[0,0,1]^t；

②测出加速度计的重力向量为a＝(ax,ay,az)^t，对加速度值进行规一化处理：

③用向量积来表示加速度计测量出的重力向量和用陀螺仪获取的角速度值(wx,wy,wz)推算出三轴的重力向量之间的误差：e＝(ex,ey,ez)＝a×g，用加速度计测量出的重力向量和用陀螺仪获取的角速度信息推算出三轴的重力向量之间的误差就是陀螺仪测量的姿态和加速度计测量的姿态的误差；

④用上述求出来的叉乘来纠正陀螺零偏，避免累积误差；

4)由修正后的角速度(w'x,w'y,w'z)值，利用一阶龙格库塔算法更新四元数值：

对四元数进行规一化处理，得到更新后的四元数值(q'0,q1',q'2,q'3)；

5)由更新后的四元数值(q'0,q1',q'2,q'3)更新姿态矩阵，得到更新后的姿态矩阵a'：

6)由姿态矩阵a’计算出姿态角：

θ＝-π·sign(a'23)+arctan(a′23/a′33)

式中a′kl表示姿态矩阵a’的第k行第l列的元素；

7)由计算出的姿态角构造方向余弦矩阵g，触控笔按不同的坐标轴顺序旋转，得到不同的方向余弦矩阵，以触控笔绕z-y-x为例，则方向余弦矩阵g：

上述方法描述的触控笔的位置计算步骤如下：

1)测量出触控笔上超声传感器发送的信号到主机三个超声波接收器[3021]、[3022]和[3023]的时间分别为△t1、△t2、△t3,主机上三个超声波接收器[3021]、[3022]和[3023]的坐标值分别为：(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)，超声波传播速度为c；

2)计算出触控笔到各个超声波接收器的距离：li＝△t·c，i＝1,2,3；

3)利用定位原理公式：

解出触控笔的各个时刻的坐标(x,y,z)。

上述方法描述的由三维坐标信息修正姿态角计算步骤如下：

1)利用定位原理确定触控笔运动前一时刻和当前时刻的坐标分别为(x,y,z)和(x1,y1,z1)

2)利用已经修正的俯仰角和航向角，根据欧拉定理：(x1,y1,z1)＝(x,y,z)*g计算出航向角φ'，式中前一时刻和当前时刻的坐标值是给定的，俯仰角和横滚角是给定的，只有一个未知参量航向角，所以由上式可解出航向角φ'。由超声波计算出的航向角是利用触控笔的物理位置不会发生突变的这个条件可以连续平滑的判别上述角度突变是真实的还是测量本身带来的。有效地解决了六轴传感器单独作用的情况下容易出现0°和180°的角度突变问题；

3)利用互补滤波算法，实现对陀螺仪获取的航向角和超声波获取的航向角进行数据融合：

其中需要用到的参数：t为滤波器时间常数，dt为滤波器采样时间；

其中俯仰角θ取值范围在-180°到-90°或90°到180°，航向角φ取值范围在-180°到180°，横滚角的取值范围在-180°到180°；

一种惯性技术和超声波相结合的触控笔位姿检测系统，包括以下部分：

触控笔部分：包括微处理器、mems六轴加速度计及陀螺仪传感器、pvdf超声传感器、红外发射管以及蓝牙芯片。

1)微处理器：主要完成超声波信号、红外信号的发射，加速度计和陀螺仪传感器信号的采集以及按键功能的处理；

2)mems六轴加速度计及陀螺仪传感器：用于获取陀螺仪和加速度计上的数据信息，通过蓝牙传输给主机；

3)pvdf超声传感器和红外发射管分别发射超声信号及红外信号；

4)蓝牙芯片：用于将触控笔获取的三维姿态信息发送到主机上。

主机部分：包括平板电视屏幕或白板和超声三维定位模块，超声三维定位模块：包括3个在同一个平面但不在一条直线上的超声传感器构成的三维超声定位阵列及2个红外传感器，用于实现对触控笔的三维定位。

本发明中触控笔的超声波定位精度达到毫米量级，利用pvdf超声传感器和mems六轴加速度计及陀螺仪传感器共用一个坐标系，坐标原点重合的这一特点解决了触控笔在六轴传感器单独定位的情况下容易在0°和180°出现角度突变的技术问题同时还可以实现修正航向角的功能，达到实现笔以任意姿态、任意角度、任意距离与界面进行交互的目的。

附图说明

图1a是本发明结合触控笔姿态角和坐标信息的一种示意图；

图1b是本发明触控笔姿态角应用示意图；

图2是本发明触控笔结构示意图；

图3是本发明主机组成框图；

图4是本发明触控笔坐标系校准示意图；

图5a是本发明笔式交互实现方法流程图；

图5b是本发明三维坐标计算流程图；

图5c是本发明加速度矫正角速度计算流程图；

图5d是本发明三维坐标修正航向角示意图；

图6是本发明三维超声波定位原理示意图；

图7是本发明触控笔发生角度突变示意图。

具体实施方式

包括如下步骤：

(1)采集mems六轴加速度计及陀螺仪传感器获取的角速度与加速度数据，测量超声波从触控笔上超声波发射传感器到接收传感器上的传播时间，并对采集和测量到的信息进行处理，得到输出的数据信息；

(2)由mems六轴加速度计及陀螺仪传感器输出的数据信息，用处理器计算触控笔的姿态角信息，所述触控笔的姿态角信息是触控笔在三维坐标系下的俯仰角θ、航向角φ和横滚角

(3)由三路超声波传播的时间信息，计算触控笔的坐标信息，所述触控笔的坐标信息是指触控笔在三维坐标系下的三维坐标(x,y,z)；

(4)由获取的触控笔的位置信息获取航向角φ'，对mems六轴加速度计及陀螺仪传感器获取的航向角和超声波获取的航向角进行互补融合，计算出修正后的航向角φ”；

(5)输出坐标信息和修正后的姿态角信息，确定触控笔当前的位姿。

上述方法描述的采集mems六轴加速度计及陀螺仪传感器上的角速度和加速度值，其特征在于采集mems六轴加速度计及陀螺仪传感中三轴陀螺仪和三轴加速度计测出的触控笔的角速度值及加速度值；

上述方法描述的对信号进行处理包括a/d转换，是为了将模拟信息中的的角速度值和加速度值由a/d转换器转换成数字信息的角速度值(wx,wy,wz)和加速度值a＝(ax,ay,az)；

上述方法描述的对信号进行处理包括增益控制，是为了将接收信息幅值调整至适于定位装置动态范围，以保持信息的稳定性；

上述方法描述的对信息进行处理包括滤波，滤波是为了滤除噪声信号；

上述方法描述的对采集到的加速度值进行归一化处理，是为了将加速度计的三维向量转化为三维单位向量；

上述方法描述的触控笔的姿态角信息计算步骤如下：

1)初始化四元数：q0＝1，q1＝0，q2＝0，q3＝0，四元数表达式：q(q0,q1,q2,q3)＝q0+q1i+q2j+q3k，所述四元数代表多维实数空间的一个四维空间，同时也可以代表一个复数的二维空间，i、j、k分别代表x轴、y轴、z轴上的虚数单位；

2)利用四元数根据欧拉定理计算出姿态矩阵a：

3)利用互补滤波算法，实现用加速度计测得的重力向量和陀螺仪获取的角速度信息推算出的重力向量的叉积来修正陀螺仪获取的角速度值的的零偏误差：

①用陀螺仪获取的角速度信息(wx,wy,wz)推算出陀螺仪三轴的重力向量(即重力单位向量在机体坐标系中的重力加速度)，g＝(vx,vy,vz)＝a*[0,0,1]^t；

②测出加速度计的重力向量为a＝(ax,ay,az)^t，对加速度值进行规一化处理：

③用向量积来表示加速度计测量出的重力向量和用陀螺仪获取的角速度值(wx,wy,wz)推算出三轴的重力向量之间的误差：e＝(ex,ey,ez)＝a×g，用加速度计测量出的重力向量和用陀螺仪获取的角速度信息推算出三轴的重力向量之间的误差就是陀螺仪测量的姿态和加速度计测量的姿态的误差；

④用上述求出来的叉乘来纠正陀螺零偏，避免累积误差；

4)由修正后的角速度(w'x,w'y,w'z)值，利用一阶龙格库塔算法更新四元数值：

对四元数进行规一化处理，得到更新后的四元数值(q'0,q1',q'2,q'3)；

5)由更新后的四元数值(q'0,q1',q'2,q'3)更新姿态矩阵，得到更新后的姿态矩阵a'：

6)由姿态矩阵a’计算出姿态角：

θ＝-π·sign(a'23)+arctan(a′23/a′33)

式中a′kl表示姿态矩阵a’的第k行第l列的元素；

7)由计算出的姿态角构造方向余弦矩阵g，触控笔按不同的坐标轴顺序旋转，得到不同的方向余弦矩阵，以触控笔绕z-y-x为例，则方向余弦矩阵g：

上述方法描述的触控笔的位置计算步骤如下：

1)测量出触控笔上超声传感器发送的信号到主机三个超声波接收器[3021]、[3022]和[3023]的时间分别为△t1、△t2、△t3,主机上三个超声波接收器[3021]、[3022]和[3023]的坐标值分别为：(x1,y1,z1)、(x2,y2,z2)、(x3,y3,z3)，超声波传播速度为c；

2)计算出触控笔到各个超声波接收器的距离：li＝△t·c，i＝1,2,3；

3)利用定位原理公式：

解出触控笔的各个时刻的坐标(x,y,z)。

上述方法描述的由三维坐标信息修正姿态角计算步骤如下：

1)利用定位原理确定触控笔运动前一时刻和当前时刻的坐标分别为(x,y,z)和(x1,y1,z1)

2)利用已经修正的俯仰角和航向角，根据欧拉定理：(x1,y1,z1)＝(x,y,z)*g计算出航向角φ'，式中前一时刻和当前时刻的坐标值是给定的，俯仰角和横滚角是给定的，只有一个未知参量航向角，所以由上式可解出航向角φ'。由超声波计算出的航向角是利用触控笔的物理位置不会发生突变的这个条件可以连续平滑的判别上述角度突变是真实的还是测量本身带来的。有效地解决了六轴传感器单独作用的情况下容易出现0°和180°的角度突变问题；

3)利用互补滤波算法，实现对陀螺仪获取的航向角和超声波获取的航向角进行数据融合：

其中需要用到的参数：t为滤波器时间常数，dt为滤波器采样时间；

其中俯仰角θ取值范围在-180°到-90°或90°到180°，航向角φ取值范围在-180°到180°，横滚角的取值范围在-180°到180°；

一种惯性技术和超声波相结合的触控笔位姿检测系统，包括以下部分：

触控笔部分：包括微处理器、mems六轴加速度计及陀螺仪传感器、pvdf超声传感器、红外发射管以及蓝牙芯片。

1)微处理器：主要完成超声波信号、红外信号的发射，加速度计和陀螺仪传感器信号的采集以及按键功能的处理；

2)mems六轴加速度计及陀螺仪传感器：用于获取陀螺仪和加速度计上的数据信息，通过蓝牙传输给主机；

3)pvdf超声传感器和红外发射管分别发射超声信号及红外信号；

4)蓝牙芯片：用于将触控笔获取的三维姿态信息发送到主机上。

主机部分：包括平板电视屏幕或白板和超声三维定位模块，超声三维定位模块：包括3个在同一个平面但不在一条直线上的超声传感器构成的三维超声定位阵列及2个红外传感器，用于实现对触控笔的三维定位。

为了使本技术领域的人员更好的理解本发明，下面结合附图和实施方式对本发明作进一步的详细说明。

1、姿态角结合坐标值的应用，参照图1a和图1b：

图1a是结合触控笔姿态角和坐标信息的一种示意图，需要用到如下参数：d1,d2分别表示触控笔到主机的距离，且d2>d1，α1,α2，表示触控笔旋转的角度，且α2>α1，参照图1a[100]，启动三维超声定位和六轴传感器三维姿态定位测出触控笔的位置信息和姿态信息，根据获得的触控笔的坐标值，就可以计算出触控笔相对于主机[1001]的距离，当用户[1005]需要调用主机上[1001]的文档时，如图示出：当用户距离主机位置为d1时，用户需要旋转角度α1使位于b点处[1002]的光标移动到需要打开的文档[1004]，按下按键打开文档[1003]，当用户距离主机位置为d2时，用户需要旋转角度α2使位于b点处[1002]的光标移动到需要打开文档[1005]，按下按键打开文档[1003]。

图1b触控笔姿态角应用示意图的说明，需要用到如下参数：β1,β2分别表示触控笔旋转的角度。参照图1b，启动三维超声定位和六轴传感器三维姿态定位测出触控笔的位置信息和姿态信息，当用户[1017]距离主机[1014]位置没有发生变化时，用户需要打开文档e[1012]时，只需旋转角度β1[1016]使位于b’点处[1013]的光标移动到文档e处[1012]，按下按键打开文档；用户需要打开文档d[1011]时，只需旋转角度β2[1015]使位于b’点处[1013]的光标移动到文档d处[1011]，按下按键打开文档。

2、图2为本发明应用的三维大尺度书空模式人机交互系统的触控笔部分组成示意图，触控笔[200]主要包括笔尖[201]、超声传感器[202]，本发明中使用的超声传感器[202]是pvdf超声传感器，mems六轴加速度计及陀螺仪传感器[203]、红外发射管[204]、蓝牙通信芯片[205]、按键[206]和处理器[208]，pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]在触控笔[200]中放置重合，确定pvdf超声传感器[202]和电路板[207]上的mems六轴加速度计及陀螺仪传感器[203]共用一个坐标系，坐标原点重合，如步骤四中，当pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]在触控笔[200]中放置重合时，可以解决笔容易在0和180出现角度突变问题，。由于触控笔的设计是圆柱形的，在进行笔式交互时，只需用到俯仰角和航向角。

在进行三维坐标定位时，红外发射管[204]和pvdf超声传感器[202]同时发射超声信号和红外信号，超声信号以声速传播，红外信号以光速传播，以红外信号作为测距同步信号，测出超声信号和红外信号分别到达超声波接收器和红外传感器的时间，超声信号和红外信号分别到达超声波接收器和红外传感器的时间差为超声波发送一次信号所用的时间。

3、图3为本发明应用的三维大尺度书空模式人机交互系统的主机部分组成示意图，主机[300]主要由平板电视屏幕或白板[301]和超声三维定位模块[302]组成，超声三维定位模块[302]包括3个在同一个平面但不在一条直线上的超声传感器[3021]、[3022]和[3023]构成的三维超声定位阵列及2个红外传感器[3024]和[3025]，使用2个红外传感器[3024]和[3025]是为了能够接收到不同角度传来的红外信号。

4、具体的笔式交互实现方法可参考图5a[500]，按下按键[206]给触控笔上电[501]，pvdf超声传感器[202]及红外发射管[203]分别发射超声信号及红外信号，通过按键[206]和笔尖[201]在三维空间内无需接触板面即可触发超声波和红外信号，以红外信号作为测距同步信号，通过三维超声定位技术对触控笔进行三维实时定位，计算出超声三维各个时刻的坐标值[502]，启动mems六轴加速度计及陀螺仪完成加速度和角速度信号的提取，利用加速度信息修正角速度信息计算出三维姿态[503]，利用各个时刻的三维坐标再次修正三维姿态[504]，获取修正后的姿态信息[505]，输出笔的三维坐标信息和三维姿态信息[506]经由蓝牙芯片[205]发送到主机。

三维坐标计算步骤，参照图5b[502]，启动三维超声定位[5021]，测量超声波到三个接收器的传播时间△ti[5022]，利用li＝△t×c，i＝1,2,3算出笔到各超声波接收器的距离[5023]，利用超生波定位原理算出各个时刻的坐标值为(x,y,z)[5024]。

其中，超声波定位原理可参照图6进一步说明：触控笔的三维坐标o(x,y,z)[601]，三个超声传感器的物理坐标a(x1,y1,z1)[604]、b(x2,y2,z2)[602]、c(x3,y3,z3)[603]；触控笔到三个超声传感器的距离l1[605]、l2[606]、l3[607]。

利用定位原理公式就可计算出三维坐标值。

加速度矫正角速度计算步骤，参照图5c[503]，启动加速度和角速度三维姿态定位[5030]，利用陀螺仪获取角速度信息[5031]、利用加速度计获取加速度信息[5032]，对加速度信息进行归一化处理[5033]，然后利用互补滤波算法，用加速度值对角速度进行修正[5034]，获得更新后的角速度值，初始化四元数[5035]。获取更新后的角速度值，利用龙格库塔法更新四元数值[5036]，然后对四元数进行归一化处理[5037]，更新姿态矩阵，得到更新后的姿态矩阵a'[5038]，利用姿态矩阵计算出三维姿态角[5039]。

这里需要说明的参数如下：li为各超声波接收器的距离，c为超声波传播速度，△ti为发送超声波的时刻值。

三维坐标矫正航向角计算步骤，参照图5d[504]，利用图5c获得的三维姿态角[5041]和图5b获得的三维坐标值[5042]，(x,y,z)和(x1,y1,z1)分别为超声波前一时刻和当前时刻的坐标值，根据欧拉定理：(x1,y1,z1)＝(x,y,z)·c，利用前一时刻和当前时刻的坐标值修正航向角[8043]。

参照图7，图7是触控笔发生角度突变示意图[700]，触控笔[701]在有六轴传感器单独作用的情况下，容易在0°、180°发生角度突变问题，图中示出，正常旋转情况下触控笔旋转角度χ到达0°[702]，但在到达0°时出现角度突变问题，触控笔突变到180[703]，造成角度测量错误。

如果，pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]在触控笔[200]中放置不重合：首先要对触控笔进行坐标系校准，参照图4[400]，使得触控笔能够精准的获取三维姿态信息；然后在pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]下建立平行坐标系，然后在pvdf超声传感器[202]建立的坐标系下获取触控笔的三维坐标信息，利用pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]在触控笔中放置的位置距离差及三维姿态角信息经过坐标变换，计算得到mems六轴加速度计及陀螺仪传感器[203]下的三维坐标信息。pvdf超声传感器[202]和mems六轴加速度计及陀螺仪传感器[203]在触控笔[200]中放置不重合的状态下，仍可以通过前一时刻和当前时刻的三维坐标，解决笔容易在0°和180°出现角度突变问题，但不能通过互补融合实现修正航向角的功能。