倒立振子の研究

これまで、Arduinoを使ったシンプルな倒立振子の作り方を、instructablesで2つ紹介しました。

　　「半日で作る倒立振子」
　　「もう一つの倒立振子（ デジタル版）」

今回は、完成した倒立ロボットを使って、その動きをパソコンでリアルタイムに再現します（以下では「仮想振子」と呼びます）。 そして、その記録を使ってロボットの動作改善を検討します。これらの具体的な内容は紹介動画の下の手順一覧をご覧ください。

なお、紹介動画の「仮想振子」を実現するだけなら、手順の（1）と（2）だけで十分です。この場合、ステップ1を飛ばして、ステップ2～4の作業を実行し、ステップ7の解説（8）の修正を行ってください。

紹介動画

【手順】

（1）状態変数のキャプチャ
倒立ロボットの動作の様子（制御に使う4つの状態変数の値）をリアルタイムで取得する

（2）可視化Ⅰ･･･ロボットの動きをリアルタイムで描画
取得したデータをパソコンで即時処理して、その動きをバーチャルに再現する（仮想振子）

（3）可視化Ⅱ･･･ログの分析
取得したデータを使って、倒立ロボットの動作を事後的に分析する

（4）可視化Ⅲ･･･補完的な情報の取得
倒立ロボットのブレッドボードに抵抗と加速度センサを追加して、モータを流れる電流と倒立ロボットにかかる力の情報を新たに取得する

（5）倒立ロボットの改善の検討
以上の分析と新たな情報を使って、倒立ロボットの安定性改善を検討する

上記（4）では、0.5Ωの抵抗（2個）と加速度センサ・モジュール（LIS3DH：1個）を用意する必要がありますが、（1）～（3）は、追加の材料は不要です。

なお（4）と（5）では、次の2つの理由により、主にデジタル版の倒立ロボットを想定して話を進めます。オリジナルのアナログ版でも同様の作業は可能ですが、デジタル版に換装した方が面倒が少ないと思います。

・デジタル出力のジャイロは、出力値の初期測定が不要
・L298N（モータ・ドライバIC）は、電流の計測が容易

Step 1: 問題意識の共有

倒立振子の制御は、モータの回転力の調整で行います。この調整は、振子の状態を表す4つの変数を使った簡単な数式で表されます。

モータの回転力＝　k1 × 躯体の傾き（角度）
　　　　　　　　　　＋k2 × 躯体の傾きの変化率（角速度）
　　　　　　　　　　＋k3 × 車輪軸の移動速度
　　　　　　　　　　＋k4 × 車輪軸の移動距離

ここでk1～k4は、これら4つの変数の重視の程度を表す定数です。

倒立振子がバランスするには、これら4つの変数をリアルタイムで評価する必要があります。ジャイロ・センサは、この式の2番目の変数（躯体の傾きの変化率）を計測し、これをArduinoで積算して1番目の変数（躯体の傾き）を評価します。

これに対し、3つ目の変数と4つ目の変数は、ジャイロ・センサでは評価困難です。 このため、通常は専用の計測手段を別途用意します。しかし、先に紹介した倒立ロボットは、ジャイロ・センサしか使っていません。 このため、3番目と4番目の変数の評価が問題になります。

この問題について、これらの倒立ロボットは、Arduinoの出力電圧（PWM）の履歴を用いて、これらの評価を行っています（*）。

・車輪軸の移動速度･･･出力電圧の積算値で評価
・車輪軸の移動距離･･･上記の積算値をさらに積算した値で評価

（*）2変数（角度と移動距離）のPD制御を、1変数（角度）のPID制御で代替している、と言えるのかもしれません

経験上、上記4つの定数（k1～k4）の値を上手く選べば、これらの倒立ロボットは、長時間（1時間以上）バランスを保ち続けます（外生ショックがない場合）。同時に、これらの定数の内、最後の定数（k4）については、ロボットの動作にほとんど影響を与えないことも分かりました。

例えば、最初の3つの定数の値が適当ならば、k4=0にしても、ロボットの安定性はほとんど悪化しません。これらのことから次の2つの疑問が生じます（2つ目の疑問については下の解説（1）を参照）。

（1）4番目の変数の評価値（出力電圧の積算値の積算）は、ロボットの動作に寄与していないのではないか？

（2）3番目の変数の評価値（出力電圧の積算値）は、倒立ロボットの移動「速度」ではなく移動「距離」を表しているのではないか？

これら2つの疑問については、ステップ6～8で検討します。またこれとは別に、ステップ9～11では、これら2つの変数の評価方法を変更して、より直接的な情報の利用を考えます。その目的はロボットの動作の改善です。

（3）3番目の変数（移動速度）の評価を「電圧」ではなく「電流」の積算値に代えると安定性が高まるのではないか？

（4）加速度計を使って、4番目の変数（移動距離）を評価すると安定性が高まるのではないか？

これら4つの疑問に答える準備として、ステップ2～5では、倒立ロボットの動作の様子を取得、確認、記録する方法を紹介します。

■解説（1）
ここで用いる倒立ロボットの計測器はジャイロ・センサだけです。 このため、車輪軸の移動速度と移動距離は、Arduinoの出力電圧（PWM）の履歴を用いて間接的に評価しています。 この時に前提にしたのが、下記の因果関係です。

　　（1）Arduinoの出力電圧（p） → （2）モータを流れる電流（q）
　　　　　　　　　　　　　　　　　 → （3）モータ（→車輪）の回転力（r）
　　　　　　　　　　　　　　　　　 → （4）床から車輪軸への抗力（s）
　　　　　　　　　　　　　　　　　 → （5）車輪軸の加速度（u）
　　　　　　　　　　　　　　　　　 → （6）車輪軸の移動速度（v）
　　　　　　　　　　　　　　　　　 → （7）車輪軸の移動距離（x）

そしてこの関係を基に、（5）の車輪軸の加速度uが、（1）の電圧pに近似的に比例すると仮定して（u＝c*p）、微小時間tにおける車輪軸の速度vの変化（＝u*t）を求め、（6）の速度vと（7）の距離xを評価しています。

　　v＝ Σ(u*t) ＝ c*[Σ(p*t)]

　　x ＝ Σ(v*t) ＝ c*Σ[ {Σ(p*t)} *t ]

ここでcは適当な定数で、Σは総和を表す記号です。この因果関係では、（3）のモータの回転力rが、車輪軸、車輪、床面の順に伝わり、その抗力（反作用）が軸を動かして（結果的に）車輪が回転します。

一方、DCモータの標準的な出力特性の説明（A、B）を見ると、「回転力は一定で、回転数が電圧に比例する」とも考えられます。 したがって、次のような因果関係を考えることもできます。

　　（1）Arduinoの出力電圧（p） → （2）モータを流れる電流（q）
　　　　　　　　　　　　　　　　　 → （A）モータの回転数（m）
　　　　　　　　　　　　　　　　　 → （B）車輪の回転数（n）
　　　　　　　　　　　　　　　　　 → （6）車輪軸の移動速度（v）
　　　　　　　　　　　　　　　　　 → （7）車輪軸の移動距離（x）

最初の因果関係では、モータや車輪の回転数（m、n）は重要性が低いので明記しませんでした。 しかし、この因果関係では、2つの状態変数（v、x）に直接影響する重要な変数になっています。また、車輪軸の速度vはモータの回転数mに比例します。したがって、この回転数mが電圧pに近似的に比例すると仮定すると、（6）の速度vと（7）の距離xの評価は、次のように書き換えられます。

　　v＝c*p

　　x＝Σ(v*t) ＝c*[Σ(p*t)]

ここで重要なのは、出力電圧の積算値のc倍（ c*[Σ(p*t)] ）の位置付けです。この積算値は、最初の因果関係では車輪軸の移動速度vの評価値ですが、下の因果関係では、車輪軸の移動距離xの評価値になります。

なお上述のように、電圧の2重積算（ Σ[ {Σ(p*t)} *t ] ）は、経験的に倒立ロボットの動作にほとんど影響を与えません。 これは、最初の因果関係よりも、下の因果関係の妥当性を示唆しているのかもしれません。 この疑問は、モータの回転数mと回転力rの関係を加味した運動方程式を解けば答が出るように思えます。 しかしここでは、動作するロボットが手元にあるので、計測によって答を出します。

Step 2: 倒立ロボットのスケッチ修正

本ステップ～ステップ4では、倒立ロボットの動作の様子を表す諸変数をリアルタイムでパソコンに送り、その動作を再現、保存する方法を紹介します。 ロボットからパソコンへのデータ送信は、シリアル通信を利用します。

その準備としてここでは、倒立ロボットを制御しているArduinoのスケッチを修正します。 なお、サンプル・スケッチの入手、修正、およびこれらのスケッチの行数表示については、「もう一つの倒立振子（デジタル版）」のイントロ末尾の注記と同版ステップ5下段の「サンプル・スケッチ（プログラム）」を参照してください。

【作業手順】

• 倒立ロボットのArduinoをUSBケーブルでパソコンに繋ぐ
• 倒立ロボットの加筆済みのサンプル・スケッチに下記の5行を追加し、Arduinoにアップロードする
• IDEでArduinoのコム・ポートの番号を確認する（ステップ3で利用）

＜倒立ロボットのスケッチへの追加コード＞

オリジナル版（アナログ版）：
ベースは、同版ステップ5の修正サンプル・スケッチ（ver.2.0）です。 これに同版ステップ8の修正を加えると倒立ロボットが上手くバランスをとるはずです。 この加筆修正スケッチの「NL8」行目と56行目（コメントアウト済み）の間に、下記の5行を追加してください。

Serial.print( millis() ); Serial.print(",");
Serial.print( kAngle * thetaI / 200 ); Serial.print(",");
Serial.print( kOmega * omegaI / 78 ); Serial.print(",");
Serial.print( kSpeed * vE5 / 1000 ); Serial.print(",");
Serial.println( kDistance * xE5 / 1000 );

デジタル版：
ベースは、同版ステップ5のサンプル・スケッチ（ver.2.0.d）です。 これに同版ステップ8の修正を加えると倒立ロボットが上手くバランスをとるはずです。 この加筆スケッチの「DL32」と「56」行目（コメントアウト済み）の間に、下記の5行を追加してください。

Serial.print( millis() ); Serial.print(",");
Serial.print( kAngle * thetaI / 100 ); Serial.print(",");
Serial.print( kOmega * omegaI / 100 ); Serial.print(",");
Serial.print( kSpeed * vE5 / 1000 ); Serial.print(",");
Serial.println( kDistance * xE5 / 1000 );

Step 3: Processingの導入と準備

ロボットの状態を、Arduinoからパソコンに送るためのシリアル通信に関して、今回3つの方法を試しました。

・ArduinoのIDEのシリアル・モニタを使用
・Excelのスプレッドシートにリアルタイムに取り込む（PLX-DAQを使用）
・Processingでリアルタイムに動作再現しながらデータ保存（csvファイル）

この内、技術的に最も容易なのはIDEのシリアル・モニタを使う方法ですが（*1）、毎回の作業が煩雑な上、リアルタイムでの動作確認ができません。一方、「PLX-DAQ」は便利なアプリケーションですが（*2）、上手く動作するには一定の条件が必要です（*3）。このため以下では、フリーソフトの「Processing」を使った方法を紹介します。

（*1）具体的な作業の方法は、オリジナル版に寄せられたtokumikatuさんのコメントへの回答を参照してください
（*2）PLX-DAQは、Arduinoの競合ボード用のアプリケーションです。Arduinoでの使用方法はこちらを参照してください
（*3）パソコンの処理能力が高くないとグラフが更新されません。また使用できるコム・ポートは15番以下に限られます

【作業手順】

• Processingのサイトから「ver.2.X.X」を収めたzipファイルをパソコンにダウンロードして解凍する（レジストリの書き換えは不要）
• 解凍したフォルダ内にある「processing.exe」をダブル・クリックして統合エディタ（PDE）を起動する
• Processingのサンプル・スケッチを収めたpdfファイル「virtualPendulum_processing.pdf」のアイコンが、本ステップの末尾にあることを確認（確認できない時は下記の「トラブルシューティング」を参照）
• このpdfファイルをパソコンにダウンロードして、Adobe Reader等の専用ビューアで開く
• ビューアの内容を全てコピーして、Processingの統合エディタ（PDE）に貼り付ける
• ミスプリを修正する（詳細は「半日で作る倒立振子」のステップ5を参照）
• サンプル・スケッチの24行目のポート番号（COM14）を、倒立ロボットのArduinoのコム・ポート番号に書き換える（本ステップ2枚目の画像参照）
• 修正したProcessingのスケッチに適当な名前を付けて、適当なディレクトリに保存する

■解説（2）
ArduinoからProcessingに送られたデータは、Processingのスケッチ（プログラム）と同じフォルダ内に、「log.csv」として自動的に保存（もしくは上書き）されます。 なお、pdfファイルのダウンロード、閲覧、コピー、ミスプリ修正（*）は、倒立ロボットの製作でArduinoのサンプル・スケッチを入手したのと同様です。 詳しくは「半日で作る倒立振子」のイントロ末尾の注記とステップ5を参照してください。

（*）典型的なミスプリは、「//」の間にスペースが入る（「/ /」）ことです

■トラブルシューティング
本ステップの末尾にpdfファイルのアイコンが表示されない場合、まずアドレス・バーのURLの最後の「?lang=ja」を削除してページを更新してください。それでも上手くいかない時は、本記事で使うpdfファイルのリンクを最初の倒立ロボットの記事のステップ12に追加したので、そちらを参照してください。

Step 4: 仮想振子の動作確認と倒立ロボットのデータ取得

ここでは、前ステップで用意したProcessingのスケッチを実行して、仮想振子がパソコン上でリアルタイムに動く様子をまず確認します。 次に、Arduinoから送られた倒立ロボットのデータが、csvファイルに格納されているのを確認します。

【作業手順】

• ステップ2で用意した倒立ロボット（Arduino）とパソコンをUSBケーブルでつなぐ
• 前ステップで用意したProcessingのスケッチを開く
• PDEの「Run」ボタンを押すと、描画画面が開く
• 簡単化された「仮想振子」の車輪から、頭と胴体が出てくるのを確認する（描画画面の上段）
• ArduinoにUSBケーブルをつないだまま、倒立ロボットを倒立させる
• 描画画面の仮想振子が、現実の倒立ロボットと同じ動きをするのを確認する
• 十分観察したら、ロボットが自立している間に描画画面をクリックし、適当なキーを押す
• 描画画面が消える
• 修正スケッチが収められたフォルダにCSVファイル「log.csv」が出来ているのを確認する

作業手順の解説動画（ステップ4～5）

■解説（3）
「log.csv」には、描画画面が表示されている間に取得したデータが2次元配置で収められています。 データは左から、「経過時間（ミリ秒）」、「角度の寄与」、「角速度の寄与」、「移動速度の寄与」、「移動距離の寄与」の順です。 ちなみにここで「寄与」とは、モータに加える電圧のPWMスケール（-255～255）での寄与です（*）。また各行は、時系列順に下に並んでいます。ただし最初の約10～20行は、記録開始の初期ノイズが記録されているので削除してください。

（*）PWMの上下限は意識していないので、合計の絶対値が255を超えることがあります

■解説（4）
仮想振子の動作は、カメラ等のデータを参照しているわけではありません。ジャイロの出力だけを使って、Arduinoで評価した倒立ロボットの動きを仮想的に再現したものです。 このため、各変数に関して主に次のような誤差が生じると考えられます。

・ジャイロ出力のノイズ・・・角速度の誤差
・ジャイロ出力のバイアス・・・角度、移動速度、移動距離の誤差
・評価の前提の誤り・・・移動速度と移動距離の誤差

■解説（5）
Processingの描画画面の下段は（下から順に）、ステップ1の制御方程式の左辺（出力）と、右辺各項の寄与度の評価値を図示しています。また、評価値なので、解説（4）で述べた誤差も入っています。なお、同式右辺の第4項（移動距離）の寄与は表示していません（理由はステップ7を参照）。

・Speed：車輪軸の移動速度の寄与･･･出力電圧の積算 ×c ×k3
・Omega：躯体の角速度の寄与･･･ジャイロ出力値 ×k2
・Theta：躯体の角度の寄与･･･ジャイロ出力の積算値 ×k1
・Power：出力電圧･･･ArduinoのPWM出力電圧（上下限は考慮しない）

■トラブルシューティング
パソコンとのシリアル通信の追加により、倒立ロボットの安定性が低下するかもしれません。その場合、アナログ版のロボットならば、「NL2」行目の値（3600）を少し小さくする（2000～2600）、デジタル版のロボットならば、「DL30」行目の待機時間（90）を少し小さくする（45～75）ことで解決すると思います。それでも上手くいかない場合、ステップ1の制御の方程式の4つの係数（k1～k4：Arduinoのスケッチの10～13行目）の値を少し変えてみてください。

Step 5: 状態変数の評価値のドリフト確認

前ステップでは、Processingで描いた仮想振子が、現実の倒立ロボットの動きをリアルタイムでトレースする様子を確認しました。 でもよく見ると、時間が経つにつれて、仮想振子の移動スピードがだんだん速くなったり、仮想振子の姿勢（傾き）が不自然になったりしていませんか？（本ステップ冒頭の写真参照、以下同）

また、描画画面の下段に目を転じると、移動速度（Speed）と角度（Theta）が、明らかにゼロと異なる領域で推移していませんか？ よほどの幸運に恵まれない限り（*）、仮想振子の移動速度や姿勢が、現実の倒立ロボットとズレてしまう状況を確認できるはずです。

（*）下記の解説（7）参照

こうしたズレは、倒立ロボットの状態変数の評価値にドリフト（真値との持続的なズレ）が生じていることを意味します。 ここでは、これら倒立ロボットの動作記録を収めた「log.csv」を使って（*）、これらの評価値の推移をグラフ化し、ドリフトの様子を確認します。

（*）log.csvの保存場所はステップ4を参照

【作業手順】

• log.csvをMS社のExcelのようなアプリケーションで開く（Processingでのグラフ化も可能ですがここでは割愛）
• 不要な観測値を削除する（前ステップの解説動画の1:10～、および下記の＜補足＞を参照）
• これにより、ロボットが自立している間の経過時間と状態変数（4つ）の評価値の時系列データが残る
• 4変数の評価値の時系列（2～5列目）を折れ線グラフで表示する
• 各変数のドリフトの有無を目視で確認する
• 時折、ドリフトがリセットされるのを確認する

＜補足＞
・最初の10～20行前後は、記録開始時の取込みノイズなので削除する
・その後、倒立ロボットが自立するまでの観測値も邪魔なので、これも削除する
・Processingの描画画面を閉じる前に、ロボットの自立を止めた時（例えば転倒）は、これらの観測値も削除する

■解説（6）
これらの倒立ロボットは、自立の間に一瞬静止に近い状態になると、全ての状態変数をゼロにリセットするようプログラムされています。 このリセットにより、仮想振子とロボットのズレはキャンセルされます。 リセットの頻度を下げたい場合は、62行目の上限の値「8（もしくは4）」を小さくしてください（たとえば「3（もしくは2）」）。

■解説（7）
ジャイロ・モジュールの個体によっては、こうしたドリフトが見られないかもしれません。 これは、モジュールの出力が（幸運にも）良好であることを意味します。 ドリフトが見られない場合、「NL5」行目（アナログ修正版）もしくは「DL32」行目（デジタル版）の右辺に適当な整数（1～5）を加減してみてください。

Step 6: 分析（1） 評価値のドリフトの原因

前ステップでは、4つの状態変数の評価値の記録（log.csv）を用いて、角速度（Omega）を除く3変数（角度、移動速度、移動距離）にドリフトが生じている様子をグラフで確認しました。 ここでは、これらのドリフトが生じた原因を考えます。

そのためにまず、各変数の評価値の作成方法を振り返り、唯一の外部情報（オリジナルの情報）であるジャイロ出力との関係から、各評価値の独立性の高低を整理します。

• 角速度の評価値･･･ジャイロの出力そのものなので、その独立性は最も高い

• 角度の評価値･･･角速度の積算値なので、その独立性も角速度に準じる

• 移動速度の評価値･･･幾つかの仮定の上で、Arduinoの出力履歴から求めたものであり、この評価値の独立性は相対的に低い

• 移動距離の評価値･･･（独立性の低い）移動速度の評価値をさらに積算したものであり、この評価値の独立性は最も低い

4つの変数の評価値の独立性に関するこの整理を踏まえると、各評価値のドリフトについて次の因果関係を考えることができます。

（1）ジャイロの出力に僅かなバイアス（非ゼロの偏り）があり、これが角速度の評価に混入する

（2）角度の評価値は、角速度の評価値の積算なので、このバイアスも積算されて無視できない大きさになる（→角度評価のドリフト）

（3）移動速度および移動距離の評価値は、相対的に従属的であるが故に、「結果的に」、角度評価のバイアスをキャンセルするようなドリフトを有する

この内（3）の仮説は、「倒立振子の制御の方程式に、これら2つの評価値を適用すると、ジャイロの誤差累積を自動的に補正して、バランスを維持するメカニズムが生じる」ということなので、少なからず驚きです。

では、この自律的なバイアス補正効果は、2つの評価値（移動速度、移動距離）の間でどのように分担されているのでしょうか？ これを確かめるため、制御における移動距離の寄与をゼロにして、倒立ロボットの動きを改めて確認します。

【作業手順】

• 前ステップで倒立ロボット（Arduino）にアップロードしたスケッチの13行目「kDistance」の値（60もしくは20）を「0」に変更
• 変更したスケッチをArduinoにアップロード
• ステップ4の作業を実行して、倒立ロボットの動作の様子を目視し、「log.csv」に記録
• ステップ5の作業を実行して、各変数の評価値の推移をグラフで確認

これらの作業を行うと、まず、倒立ロボットの動き（安定性）が、スケッチ変更前と変わらないことが分かります。 次に、スケッチ変更前は、2つの評価値で分担していたドリフト（角度評価の補正に相当）が、移動速度の評価だけで実現されていることが分かります（本ステップ冒頭の画像参照）。 では、ここで無視した移動距離の評価値は、いったい何を表しているのでしょうか？

Step 7: 分析（2） 移動「距離」の評価値が表すもの

前ステップでは、4つの状態変数の評価値の内、移動距離の評価値が実質的な役割を持たない可能性を確認しました。 ここでは、逆にこの変数の係数（k4）を大きくすることで、この可能性を吟味します。

【作業手順】

• 前ステップのArduinoのスケッチの13行目「kDistance」の値（0）を、オリジナルのアナログ版ならば「（200～）250」、デジタル版ならば「（240～）290」に変更
• 変更したスケッチをArduinoにアップロード
• ステップ4の作業を実行して、倒立ロボットの動作の様子を目視し、「log.csv」に記録
• ステップ5の作業を実行して、各変数の評価値の推移を確認

この倒立ロボットにおける移動距離の評価値は、ジャイロの出力値を3回積算した成分を含みます。 したがって、ジャイロの出力が非ゼロのバイアスを持つ場合、評価値のドリフトは、この移動距離の評価値で最大になるはずです。 しかし、この評価値にかかる係数をかなり大きくしても（*）、倒立ロボットの動作はほとんど悪化しません。

（*）上記作業で示した数値を超えると悪化します。なおk4の値が上記作業の数値以下であるにも関わらず、ロボットの安定性が低下する場合、ステップ4の「トラブルシューティング」を参照してください

そこで、上記の作業の分析結果を見ると、この（一見不要な）移動距離の評価値が、（相対的に重要性の高い）角度評価のドリフトを、ほぼ完全にキャンセルしていることが分かります（本ステップ冒頭の画像参照（*））。

（*）この画像と前ステップの画像は、いずれもステップ9で新たに用意するArduinoのスケッチを用いて描いたものです。このスケッチでは「k4＝350」がデフォルト値です

またステップ5の分析では、この評価値とともに、移動速度の評価値も角度評価のドリフトをキャンセルしていましたが、本ステップの分析では、移動速度の評価値には、ほとんどドリフトが認められません。

以上のことから、この移動距離の評価値は、現実の「移動距離」を表しているのではなく、倒立ロボットのバランス維持を妨げるジャイロの誤差の累積をキャンセルする役割を果たしていると考えられます。

なお、この誤差累積をキャンセルする役割は、移動「速度」の評価値でも代替できます。 しかしこの役割を、移動「距離」の評価が専任することで、移動「速度」の評価値に関するステップ1の疑問（2）の答に近づくことができます（ステップ8参照）。

■解説（8）
このように、倒立ロボットが認識する移動距離の評価値（ c*Σ[ {Σ(p*t)} *t ] ）は、現実の倒立ロボットの移動距離ではなく、その角度評価のドリフト補正に相当します。したがって、角度の評価値と移動距離の評価値を合計すると、ドリフトを補正した角度の評価値になります。「仮想振子」でこの補正を行うには、まず本ステップの作業手順に従ってk4（kDistance）の値を変更した上で、Processingのスケッチ（virtualPendulum_processing.pdf）の74、75、77行目（dhx、dhy、theta）の「+data[4] * 0 」を、全て「+data[4] 」に書き換えてください。

Step 8: 分析（3） 移動「速度」の評価値が表すもの

前ステップでは、4つの状態変数の評価値の内、移動距離の評価値が、ジャイロの累積誤差の補正を行うことを確認しました。また、ロボットの制御に用いる係数が適当ならば、移動速度の評価値からドリフト成分が除去されて、周期的な変化だけになることも分かりました。

そこで、倒立ロボットの周期的な動きに着目することにより、移動速度の評価値が実質的に何を表しているのか確認したいと思います。ここではまず、この確認の考え方を簡単に整理しておきます。

もし、この評価値（出力電圧の積算）が、本当にロボットの車輪軸の移動速度を表しているとしたら、その大きさは、倒立ロボットの姿勢が最も悪い（傾きの絶対値が最も大きい）時にゼロになるはずです。また、この周期的な動き（前後動）に対称性があるなら、その絶対値は、姿勢が最も良い（傾きの絶対値がゼロの）時に最大になるはずです。 ここでは、前ステップにおける倒立ロボットの動作記録「log.csv」を用いて、その確認を行います。

【作業手順】

• log.csvをMS社のExcelのようなアプリケーションで開く
• 不要な観測値を削除する（ステップ5を参照）
• スプレッドシートの左から2つ目（角度の評価値）と5つ目（移動距離の評価値）を足した系列を、右側に追加する
• 追加した系列の折れ線グラフを描き、ドリフトがないことを確認する（ドリフトを除去した角度の評価値）
• 追加した系列と、左から4つ目（移動速度の評価値）の系列の折れ線グラフを描く
• 周期性が見えるので、その周期（山の頂点の距離：観測数）を概算する
• 追加した系列（ドリフトを除去した角度の評価値）を横軸、左から4つ目（移動速度の評価値）を縦軸にした散布図を描く
• 参照する観測値を、ロボットの動作が安定していた期間（観測数で言うと周期の2～3倍）に限定して、この散布図を書き直す

倒立ロボットは、周期的に前後しながらバランスをとります。 ただ、前後の折り返しでやや複雑な動きをすることが多いので、散布図がきれいな円、楕円もしくは直線になることは少ないかもしれません。 しかし、左上と右下の部分を除くと、概ね「楕円」に近い形状になると思います。

これは、PWM出力の積算値が、倒立ロボットの現実の車輪軸の速度を「ほぼ」表現していることを意味します。 実際、本ステップ冒頭の画像を見ると、ロボットの動作をタイトに管理したケース（左側の散布図）では、出力電圧の積算による移動速度の評価値が、現実の移動速度を上手く把握できている様子が示されています。

一方、ロボットの動作に遊びを持たせたケース（右側の散布図）では（*1）、楕円の左端と右端、すなわちロボットの前後運動の折り返し部分において、その高さ（速度の評価値）がゼロでないことが分かります。 また、楕円状の軌跡の最高点と最低点についても、横の位置はゼロではありません。 これらの特徴は、PWM出力の積算値が、実際の車輪軸の速度よりも僅かに遅れていることを意味しています（*2）。

（*1）動作をタイトに管理すると、外からのショックに脆弱になります
（*2）ロボットの重量配分にも依存しますが、周期の4/100程度の遅れだと思います

現実の速度に対するこの評価値の遅れは、ステップ1の解説（1）で述べた、2つ目の因果関係に関連すると思われます。すなわち、車輪軸の周期的な移動距離（振幅）が大きくなると、ロボットにかかる摩擦等の抵抗力が無視できなくなり、ロボットの姿勢を制御するモータの回転力の実質的な効果（回転力－抵抗力）が低下します。その結果、PWM電圧による速度評価において、車輪軸の回転数に比例する周期的な距離変動が顕在化するという図式です。

なお、この遅れが、前ステップで述べた自律的な補正効果に準じる役割（ロボットの安定性を改善）を果たすのか（*）、逆に動作改善の余地を示すものなのかはよく分かりません。

（*）モータの回転力（トルク）ではなく、回転そのものによる車輪軸の移動を補正している可能性があります

Step 9: 電流と加速度の計測

DCモータの特性解説を見ると、モータの回転力（トルク）は、電流に基づいて評価する方法が推奨されています。ステップ1で述べたように、モータの回転力は、倒立振子の制御方程式の出力ですが、ここで使っている倒立ロボットは、これが電圧に比例すると仮定しています。

したがって、モータを流れる電流を別途計測して、その計測値をPWMの電圧に代えれば、倒立ロボットの動作を改善できるかもしれません。

また別の改善策として、独立性の高い外部情報の追加利用が考えられます。 ステップ7で見たように、ここで使っている倒立ロボットは、実際の移動距離を参照せずに動作しています（移動距離の評価値は「結果的に」角度評価のドリフト補正になってます）。

一方、ステップ6で見たように、この角度評価のドリフトは、移動速度の評価値だけでもキャンセルできます。したがって、ロボットの動作（安定性）に関する限り、ステップ1の解説（1）に記した移動距離の評価値（ c*Σ[ {Σ(p*t)} *t ] ）は無用だと言えます。（ステップ8の「分析」では有用でした）

逆に言うと、実際の移動距離に関する外部情報を新たに追加すれば、ロボットの動作改善に役立つかもしれません。 方法はいろいろ考えられますが、ここでは、加速度センサを新たに追加して、移動距離に関する独立性の高い情報の利用を検討します。

【作業手順】

• 0.5Ωの抵抗（2個）と加速度センサ・モジュール（LIS3DH：1個）を用意する
• これらの部品を上の配線図を見てブレッドボードに取り付け、追加の配線を行う
• 本ステップ末尾のpdfファイル「invertedRobot_v21d_crrnt_accl.pdf」をダウンロードして、その内容を（ミスプリを修正して）Arduinoにアップロードする
• ステップ3のProcessingのサンプル・スケッチの5行目の「dataDim」の値（5）を「10」に変更する
• さらに、同スケッチの82行目を削除する
• 削除した82行目の下（83行目との間）に、コメント・アウトされた4行があることを確認する
• これら4行の内、最初の1行目を除く残りの3行を有効化する（各行の冒頭の「//」を削除する）

次のステップ以降で使うArduino のスケッチは、ステップ2で用意したスケッチに対して、10箇所余りの変更点があります。このため、Arduino用のサンプル・スケッチを収めたpdfファイル（invertedRobot_v21d_crrnt_accl.pdf）を新たに用意しました（*）。 なお、このサンプル・スケッチはデジタル版のロボット用です。 アナログ版のロボットについては、デジタル版の変更点を確認して、同様の変更を行ってください。

（*）このpdfファイルをArduinoにアップロードする手順は、ステップ3のProcessingのケースと同様です。なお、pdfファイルのリンクが、本ステップの末尾に表示されない場合、ステップ3の「トラブルシューティング」を参照してください

Step 10: 電流の計測と動作改善の検討

前ステップでは、倒立ロボットに2つの計測機能（電流と加速度）を追加しました。ここでは、この機能拡張ロボットを用いて、モータを流れる電流を計測します。さらに、この電流の計測値を用いて、倒立ロボットの動作改善を検討します。

【作業手順】

• 機能拡張した倒立ロボット（Arduino）を、USBケーブルでパソコンにつなぐ
• 前ステップで修正したProcessingのサンプル・スケッチを実行して、倒立ロボットを倒立させる
• 描画画面の左上の観測数が、2000程度になったら描画画面をクリックして適当なキーを押す（描画画面が終了し、記録ファイルが上書きされる）
• 「log.csv」をExcelで開く
• スプレッドシート上で、新たに、5列追加されていることを確認する（全部で10列になる）
• この内、6列目と7列目が、2つのモータを流れる電流の履歴
• スプレッドシートの右側（空白部分）に、2～5列目を合計した新たな系列を作る（PWM出力の履歴に相当）
• その右側に6列目と7列目の数値を足した系列を新たに作る（全電流の履歴に相当）
• これら2系列の散布図を描く（横軸：PWM電圧、縦軸：全電流）

「log.csv」に新たに加わった5系列の内、最初の2列（6列目と7列目）は、各モータを流れる電流の履歴です。電流の計測値はノイズが大きいため（※）、連続20回計測して、その平均値を使っています。本ステップ冒頭の画像（下段）の右側グラフがPWM電圧と全電流の散布図になります（*）。これを見ると、2つのモータを流れる電流の合計が、その時点のPWM電圧にほぼ比例することが分かります。

（*）同画像の左側グラフは、電流の連続計測を行わない時の散布図です。PWM電圧に対する比例関係が見難くなっていることが分かります。（※原因については下記の追記参照）

（※：追記）連続計測をしない時の散布図を見ると、PWM電圧が非ゼロなのに電流がゼロの観測（横軸上の点）が多数認められます。これはON/OFFを高速で切り替えるPWMの電圧制御で、OFF時点の観測に相当すると考えられます。この場合、この観測（およびON時点の観測）をノイズと見なすのは妥当でないと思われます。

さて、この右側グラフでも、計測値（の平均値）は比例直線から上下にブレています。もしこれが平滑化されないノイズにとどまらず、何か実質的なものを含むとしたら、PWM電圧に代えて、この電流の平均値を用いることで、倒立ロボットの動作（安定性）を改善できるかもしれません。

実は、前ステップで新たに用意したArduinoのスケッチは、ロボットの速度の評価を、PWM電圧の積算値ではなく、モータを流れる電流の積算値に代えています（*）。下の動画は、このスケッチを使ったロボットの動作をテストしたものです。その結果、モータの出力の評価を、PWM電圧から電流計測値に代えることで、ロボットの安定性がほんの少し良くなることが分かりました。

（*）ロボットの速度評価を、モータを流れる電流の積算値ではなく、PWM電圧の積算値に戻したい場合、新スケッチの下から6行目 （sumPower）の右辺第二項（(c1+c2) / 20 / 0.552）を「power」に書き換えてください

電流計測値を用いた倒立ロボットの動作デモ（ステップ10）

Step 11: 加速度の計測と動作改善の検討

ステップ9で用意した倒立ロボットを使って、倒立ロボットの加速度（正確にはロボットに加わる力）を計測します。なお、使用した加速度モジュールは3軸の加速度を計測できますが（*1）、ここで知りたいのは、倒立ロボットの前後方向の動きです。そこで、加速度センサを車輪軸と同じ高さに取り付けて、ロボットの基部の動きをダイレクトに計測することにしました（*2）。

（*1）加速度モジュールの出力が不自然である場合、Processingの描画画面が閉じていることを確認した上で、Arduinoに接続しているUSBケーブルを抜き差ししてください
（*2）加速度センサをジャイロと同じブレッド・ボードに取り付けたところ、姿勢変化に伴う上下の動きが大きいことが分かりました。これに対し、車輪軸の近くに取り付ければ、車輪軸の前後の動きを直接計測ができます（姿勢変化にともなう重力成分の変化は残ります）

【作業手順】

• ステップ9で修正したProcessingのサンプル・スケッチを実行する
• 描画画面の左上の観測数が、2000程度になったら描画画面をクリックして適当なキーを押す（描画画面が終了し、記録ファイルが上書きされる）
• 3軸方向にかかる力の履歴が、「log.csv」の8～10列目に記録されている
• この内、負値で推移する系列は、ロボットの上下の動き（加速度）に重力加速度を加えた計測値である
• ロボットの前後の動きは、ゼロ付近で推移する系列の内、変化（振れ幅）が大きい方である
• この系列を積算すると、ロボットの前後動に関する「速度」の評価値を得ることができる
• 加速度測定に基づくこの速度の評価値を、PWM出力の積算に基づく速度の評価値と比較する

上の画像の下段左側の3つのグラフは、3軸方向に加わる力の計測値を時系列でプロットしたものです（*1）。これによると、前後の変動（青色）が最も大きいですが、動きが少ないはずの上下（緑色）や左右（赤色）の計測値も相当変動しています（*2）。これは、本来知りたい前後方向の加速度の計測値に、かなりノイズが含まれることを示唆します。

（*1）いずれの計測値も、各系列の平均を控除しています（グラフの平均がゼロになります）。また、右の速度評価のグラフ（赤線）も、この平均控除後の系列を用いています

（*2）倒立ロボットの周期的な姿勢変化により、加速度センサから見た重力の方向が変化します。これにより、上下方向と前後方向の計測値に変化が生じますが、左右方向の計測値には影響ありません

一方、上の画像の下段右側のグラフは、前後方向の「速度」に関する2つの評価値の時系列を比較したものです。これによると、PWM出力の積算による評価（青線）が、周期的な変動を示しているのに対し、加速度の計測による評価（赤線）は、中期的なトレンドの変化が目立ち、周期的な変動はほとんど見えません。現実の倒立ロボットが、ほぼ同じ場所で周期的な前後動をしていることを考えると、加速度の計測に基づくこの速度の評価値は使い物にならない、と言わざるをえません。

なお、この右側のグラフでは、加速度の計測に基づく速度評価のトレンドが時折シフトしています。これは、倒立ロボットが姿勢を乱して、モータから大きな力が加わったタイミングに重なります。逆に言うと、こうしたシフトこそ本来知りたかった情報なのですが、より小さなシフトはノイズの累積に埋もれてしまっていると言えます。

Step 12: まとめ

ここでは、ジャイロ・センサを使ったシンプルな倒立振子ロボットを使って、その動作をパソコン上で再現し、動作記録を分析する方法を紹介しました。 その結果、次のことが分かりました。

• 倒立振子の標準的な制御式で用いる4つの変数のうち、このロボットは3つの変数だけでバランスを維持している
• ジャイロ出力の微小なバイアスにより、ロボットの姿勢（角度）の評価にドリフトが生じる
• 制御電圧の履歴を使ってロボットの移動速度と移動距離を間接的に評価すると、姿勢（角度）のドリフトをキャンセルする逆のドリフトが生じる
• ロボットの移動距離の評価は、実際の移動距離を評価していない
• ロボットの移動距離の評価に十分なウェイトを与えると、移動速度の評価のドリフトを抑えられる
• この時、姿勢（角度）の評価と移動距離の評価を合算すると、実際のロボットの姿勢（角度）を知ることができる
• 制御電圧の履歴を使ったロボットの速度の間接評価は、移動距離の周期的な変化の影響をわずかに受ける

続いて、モータを流れる電流やロボットの加速度を別途計測し、ロボットの動作改善を検討しました。 その結果、次のことが分かりました。

• モータを流れる電流は、制御電圧にほぼ比例する
• 電流の計測値のノイズは大きい
• 電流の計測値のノイズは反復計測で低減できる
• 反復計測した電流の計測値を用いると、ロボットの安定性を少し改善できる
• 加速度の計測値のノイズはとても大きい
• 加速度の計測でロボットの動作を改善するのは難しい