スマートペン：Raspberry PiとIMUで手書きと動きをリアルタイム追跡

はじめに

デジタル時代において、手書き入力を取得するには通常、静電容量式タッチスクリーンやスタイラスペンが必要です。これらは精密ですが、使用面に制約があり、真の空間自由度に欠けます。本プロジェクトのスマートペンは、Raspberry Piと慣性計測装置（IMU）を組み合わせ、動きを追跡して軌跡を即座に再構築することで、それらの制約を取り払います。システムはIMUのジャイロスコープと加速度計のみに依存し、データを無線でPiに送信。独自アルゴリズムでデバイスの姿勢と位置を復元し、データを保存、PiのTFTディスプレイ上でpygameを使ってリアルタイム描画します。単一のプッシュボタンで記録の開始/停止を切り替え、ペン、ロボット、ウェアラブルなど多用途に対応。このアプローチは、手書き取得、動作解析などに使える汎用的で面非依存のソリューションを提供します。

目的

核心的な狙いは、ペン、ロボット、さらには人体の四肢に取り付け可能なモジュラー追跡ユニットを開発することです。ホストが動くと、モジュールはその軌跡を記録し、画面上の水平ワールド座標系にパスを投影します。従来のスタイラスシステムとは異なり、本設計は所定の平面に依存せず、空中や不規則な表面での動きも追跡可能。モジュールの姿勢非依存性により、傾きや配置に関わらず一貫した結果が得られます。

IMUキャリブレーション

製造公差によりIMUの加速度計とジャイロスコープ軸がズレ、計算されるオイラー角に誤差が生じます。これを補正するため、6姿勢キャリブレーションルーチンを採用。専用ジグ（図XX）でIMUを各姿勢に固定し、生センサ値を記録。この6サンプルから導かれるキャリブレーション行列を、以降のすべての加速度計測定に適用します：

\[\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ルーチンはセンサ静止時に2,000回反復し、比例積分（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）が四元数更新に入力され、安定した姿勢推定が得られ、Piに表示される軌跡再構築を支えます。

詳細：スマートペン - ECE5725最終プロジェクト