智能笔：ECE5725 的最终项目

简介

如今，为了获得手写结果，人们往往依赖电容式触摸屏、触控笔或其他类似设备。它们功能齐全且精确，但并不总是灵活。我们的项目将Raspberry Pi与IMU相结合，构建了一个可以跟踪运动并非常快速地生成轨迹的产品。我们设计了一个不需要任何特定表面或其他输入工具来支持的系统，它只响应 IMU 及其传感器。通过将 IMU 与 Raspberry Pi 连接，当 IMU 设备移动时，传感器数据将从 IMU 传输到 Raspberry Pi。然后使用我们的程序和算法，还原 IMU 运动。该运动将被记录并存储在 Raspberry Pi 中。使用 PyGame，也可以在树莓派的 TFT 屏幕上显示运动。通过小巧方便的按钮，录音功能可以非常灵活地开始和结束。用户还可以将我们的设备连接到其他东西上以跟踪他们的动作。因此，我们的设备为手写跟踪、移动跟踪等提供了一个可行的解决方案。

目标

该项目的目标是设计一个单独的模块，可以放在笔、机器人甚至人上来跟踪和记录物体的运动。用户可以使用该模块并将其与其他设备连接，因此当物体移动时，我们的设备可以恢复和跟踪其轨迹，并在屏幕上的世界框架中显示水平面中的移动。一个非常经典的应用是用户可以把它当作笔来使用，这样他们的文字就会被记录下来并保存为图像。这个项目的一个特点是跟踪不会在任何特定的飞机上回复，它可以应用于每一个飞机，甚至在空中。设备的放置或倾斜也不会影响最终结果。

我们使用 Raspberry Pi 和惯性测量单元作为我们项目的主要组件。另外，PyGame 用于在树莓派上显示轨迹。

IMU 校准

由于制造不准确，加速度计的3轴和3轴陀螺仪通常没有对准，导致两个坐标的欧拉角之间存在误差。需要校准。使用6-pos校准技术，我们设置6个校准位置如下：

使用自制的校准表（图XX）将imu固定在6个位置并记录读数，这6个位置采集的校准数据为：

校准后的加速度数据为（Ax、Ay、Az为校准后数据，ax、ay、az为原始数据）：

原始数据的传感器融合

四元数计算：

四元数是物体方向和旋转的表示，与欧拉角相比，计算向量的旋转更容易。四元数与欧拉角的交易如下图所示：

q=cosθ2+sinθ2cosα⋅i+sinθ2cosβ2⋅j+sinθ2cosγ⋅k

q=λ+P1i+P2j+P3k

四元数旋转：

对于坐标系 XYZ 中的固定矢量 V，可以用四元数表示：

V=0+Vxi+Vyj+Vzk

如果坐标系旋转q，变为X’Y’Z’，则V在X’Y’Z’上的坐标可以表示为：

V'=0+V'xi'+V'yj'+V'zk'

那么，V=q∘V′∘q−1

然而，使用陀螺仪产生的原始数据仍然不够，因为校准部分提到的未对准，需要考虑加速度计获得的重力矢量。 “UpdateIMU”函数用于计算陀螺仪计算的重力矢量与加速度计测量的重力矢量之间的误差。该函数在初始静止阶段运行 2000 次，没有移动或旋转，并使用反馈来计算误差。功能如下图所示：

123456789

 def UpdateIMU(self, Gyr, Acc):if np.linalg.norm(Acc) ==0:warnings.warn("加速度计幅度为零。算法更新中止。") return else: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]])

变量“Acc”和“v”都是加速度计和陀螺仪计算的归一化重力矢量，那么这两个矢量之间的角度偏差（误差）可以用它们的叉积来表示：

1

error =np.cross(v,np.transpose([Acc]),axis =0)

计算的误差也可以被积分以使用 PI 负反馈回路更新陀螺仪读数（这就是为什么我们需要运行这个函数 2000 次，所以 PI 回路可以收敛）：

12

self.IntError =self.IntError + errorRef =Gyr - np.transpose(self.Kp*error+self.Ki*self.IntError)

然后可以通过校正后的陀螺仪数据计算四元数：

1234

pDot =np.multiply(0.5 , self.quaternProd_single(self.q, [0, Ref[0,0], Ref[0,1], Ref[0,2]]))self. q =self.q + pDot * self.SamplePeriod; self.q =self.q / np.linalg.norm(self.q); self.Quaternion =self.quaternConj(self.q);

阅读更多：智能笔：ECE5725 的最终项目