智能笔：基于树莓派与 IMU 的实时手写与运动追踪

简介

数字化时代，捕捉手写输入通常依赖电容触摸屏或触控笔。虽精准，但受限于表面要求，缺乏真正的空间自由度。我们的智能笔结合树莓派与惯性测量单元（IMU），消除这些限制，追踪运动并实时重建轨迹。系统仅依赖 IMU 的陀螺仪和加速度计，无线传输数据到树莓派。自定义算法恢复设备姿态和位置，存储数据，并通过 pygame 实时在树莓派 TFT 显示屏渲染。单个按钮控制录制开始/停止，适用于笔、机器人或可穿戴设备。这种方案提供了一种通用的、与表面无关的手写捕捉、运动分析解决方案。

目标

核心目标是开发一个模块化追踪单元，可固定在笔、机器人甚至人体肢体上。当宿主移动时，模块记录轨迹并将其投影到屏幕上的水平世界坐标系。不同于传统触控笔系统，本设计独立于任何预定义平面，可在自由空间或不规则表面追踪运动。模块的姿态不敏感性能确保无论倾斜或放置方式如何，结果一致。

IMU 校准

制造公差常导致 IMU 加速度计和陀螺仪轴向错位，造成欧拉角计算偏差。我们采用六位置校准程序量化并修正这些偏移。自制校准夹具（图 XX）将 IMU 固定在各方向，记录原始读数。从这六个样本推导出的校准矩阵应用于每次加速度计测量：

\[\mathbf{A}_{\text{cal}} = \begin{bmatrix}A_x\\A_y\\A_z\end{bmatrix}, \quad \mathbf{a}_{\text{raw}} = \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}\]

校准后，修正的加速度向量 \(\mathbf{A}_{\text{cal}}\) 替换原始数据用于后续处理。

传感器融合获取原始数据

为获得稳健姿态估计，我们用四元数数学融合陀螺仪和加速度计输出。四元数提供无奇异性的旋转表示，简化向量变换。四元数 \(q\) 与欧拉角 \((\alpha,\beta,\gamma)\) 转换公式：

\[\displaystyle q = \cos\frac{\theta}{2} + \sin\frac{\theta}{2}\bigl(\cos\alpha\,\mathbf{i} + \cos\beta\,\mathbf{j} + \cos\gamma\,\mathbf{k}\bigr)\]

对固定向量 \(\mathbf{V} = V_x\mathbf{i} + V_y\mathbf{j} + V_z\mathbf{k}\)，四元数旋转为 \(\mathbf{V}' = q \circ \mathbf{V} \circ q^{-1}\)。

因陀螺仪随时间漂移，我们不断用加速度计测得的重力向量校正陀螺仪推导的重力估计。UpdateIMU 例程在传感器静止时执行 2000 次迭代，构建比例-积分（PI）反馈回路，最小化角度误差。核心算法如下：

 def UpdateIMU(self, Gyr, Acc):
    if np.linalg.norm(Acc) == 0:
        warnings.warn("Accelerometer magnitude is zero. Algorithm update aborted.")
        return
    Acc = np.array(Acc / np.linalg.norm(Acc))
    v = np.array([[2*(self.q[1]*self.q[3] - self.q[0]*self.q[2])],
                  [2*(self.q[0]*self.q[1] + self.q[2]*self.q[3])],
                  [self.q[0]**2 - self.q[1]**2 - self.q[2]**2 + self.q[3]**2]])
    error = np.cross(v, np.transpose([Acc]), axis=0)
    self.IntError += error
    Ref = Gyr - np.transpose(self.Kp*error + self.Ki*self.IntError)
    pDot = 0.5 * self.quaternProd_single(self.q, [0, Ref[0,0], Ref[0,1], Ref[0,2]])
    self.q += pDot * self.SamplePeriod
    self.q /= np.linalg.norm(self.q)
    self.Quaternion = self.quaternConj(self.q)

校正后的陀螺仪数据（Ref）送入四元数更新，产出稳定姿态估计，驱动树莓派上显示的轨迹重建。

阅读更多：智能笔 - ECE5725 期末项目