確率・統計ロボティクス学習キット MZIP-01
統計処理で姿勢の真値を推定するカルマン・フィルタ搭載の高安定倒立振子

講師実演ワークショップ・セミナ   

ZOOMオンライン・セミナ枠を用意しました(2020/11/23)

確率・統計処理&真値推定!カルマン・フィルタ入門 (2日間コース,700頁テキスト/弁当付き)
~高校数学から倒立振子の設計まで!システムの数理モデルで雑音を取り除く~
Z-IP01-WS

立たぬなら立たせて見せよう倒立振子

●物理法則にもとづくモデルをマイコンに組み込んで操縦させる

 1969年,アメリカ航空宇宙局 NASAは,38万kmのかなたにある月に,宇宙船「アポロ11号(写真1)」をコンピュータ誘導し,着陸させることに成功しました.

 アポロ11号のコンピュータ(写真2)は,宇宙船の位置や姿勢を正確に割り出すわずか71行のフィルタリング・プログラムを搭載していました.「数理モデル」と「実測値」を利用して,確率統計処理によって雑音(誤差)を最小化し,最も確からしい値を短時間で推定するカルマン・フィルタです.

 どんなセンサの値にも「誤差」や「ゆらぎ」といったものが含まれています.センサの出力値の統計をとると,おおよそ図1のような「正規分布」が得られます.ほとんどの場合,正規分布の中心が「本当の値」すなわち「真値」であると考えられます.

 実際のリアルタイム処理では,図1のようなたくさんの実測データを集めることが困難です.しかし,確率・統計の考え方にもとづいて生み出されたカルマン・フィルタを利用すれば,少ないデータをもとにして短時間で「真値」を推定することができます.

 最近のAI(人工知能)では,教師データを読み込ませることでニューラル・ネットワーク上に一種のモデルを構築するという方法が用いられています.アポロ11号が搭載したのは,人工知能の原型といえるものです.これに対して,アポロ11号で用いられたのは機械的に生成されたモデルではなく,科学者が自ら用意した「物理モデル」でした.

 1969年当時のコンピュータは,何億円もした大がかりなものでしたが,今のワンチップ・マイコンなら,カルマン・フィルタを動かすことなどいとも簡単です.
 

  • 写真1 1969年,NASAは,コンピュータ誘導によって,有人宇宙船「アポロ11号」38万km先にある月まで正確に誘導し着陸させた

  • 写真2 アポロ11号に搭載された誘導用コンピュータ“AGC(Apollo Guidance Computer)”

  • 図1 センサによる測定を何度も繰り返すと正規分布が得られる

●カルマン・フィルタの応用

 有名な応用先としては,GPSを利用したナビゲーション・システムが挙げられます.自動車,船舶,飛行機など,自動運転(ADAS)の機能をもつ装置にはカルマン・フィルタが使われています.また,金融の分野における株価の予測や,土木の分野における自然災害の予測,天気予報などにも使われています.

 最近は,「真値を推定する」という機能を利用して「手元の実測データから,実際には取得できないデータを求める」という使い方もされているようです.これは,いわゆるデータ・サイエンスや人工知能と呼ばれる分野と深い関わりがあります.

 カルマン・フィルタは我々の生活のいたるところで利用されており,ほとんどすべての人が何らかの形でその恩恵を受けています.

  

本キットの仕様

●数学の力で一発倒立

 本倒立振子キット MZIP-01(写真3)の制御には,「最適制御」を利用します.また,倒立振子の姿勢推定には「カルマン・フィルタ」を使います.

 この倒立振子を設計・製作する一連の流れをたどれば,ロボット制御のための基本的な考え方をひととおり身に着けることができます.完成した倒立振子が動作する様子を見れば,「理論とはここまで強力なものなのか!」と感動できるはずです.

 倒立振子ロボットの設計方針を以下に示します.

 (1)理論を学び理解するための道具として設計・製作を行う
 (2)最低限の機能にしぼり,シンプルな構成とする
 (3)部品は通販で入手しやすく,できるだけ安価なものを選ぶ
 (4)「むやみやたらなチューニング」は極力避け,「設計」による一発動作を目指す

 本倒立振子キットは,とてもシンプルなものです.派手で奇抜な動作はしません.しかし,この倒立振子を設計・製作するノウハウを十分に理解すれば,自力でより複雑かつ多機能なものへ拡張できるはずです.
  


写真3 

確率・統計ロボティクス学習キット MZIP-01

(設計:リニア・テック,製造・販売:マルツエレック)

 

 

キー・デバイス

●マイコンはNUCLEO-F401REボードを使う

 本キットは,“STM32F401RET6”というマイコン(以下,STM32F401)が実装されたNUCLEO-F401REボード(写真4)を搭載しています.

 表1に,STM32F401マイコンの仕様を示します.STM32F401には豊富な周辺モジュールが搭載されていますが,いくつかのGPIO(“General Purpose Input/Output”,汎用入出力)とタイマ,UART,I2Cモジュールだけを使います.そのため,違うマイコンを使っても問題なく実験を進められます.

 STM32F401自体の電源電圧は1.7Vから3.6Vとなっていますが,NUCLEOボード上には電圧レギュレータが実装されているので,より広い範囲の電源電圧を印加できます.今回はニッケル水素蓄電池を4本使用して,4.8V(≒5V)を供給することにします.
 

  • 写真4 本キットは,簡単に開発を始められるSTM32マイコン(STM32F401,STマイクロエレクトロニクス製)が実装されたNUCLEO-F401REボードを搭載している

  • 表1 姿勢の計測とカルマン・フィルタによる真値推定,モータ制御に使用したSTM32F401マイコンの仕様

 

●開発環境は“mbed”を使う

 マイコンのプログラム開発環境は,オンラインでソース・コードのコンパイルができる“mbed”(エムベッド)を使います.

 NUCLEO-F401REボードはmbedのプラットフォームに登録されているので,開発環境を構築する手間を一気に省くことができます.NUCLEO-F401REボードとUSBケーブル(mini B-Type A),そしてインターネットに接続されたPCがあればマイコンの開発環境が整います.なお,mbedを利用するには無料の会員登録が必要です.

 

●倒立振子の部品表

 表2に倒立振子キットの部品リストを示します.
 


表2 倒立振子キット MZIP-01の部品表

 

●倒立振子の全システム図

 図2に,倒立振子キットの全システムを示します.太枠のブロックは実体がある部品を,それ以外はプログラム上の処理を表しています.

 本倒立振子には,2つのカルマン・フィルタが搭載されています.1つめは加速度センサとジャイロ・センサの値を統合して,真の車体の傾斜角を推定するためのカルマン・フィルタです.2つめは各センサの値およびモータ電圧にもとづいて,本体の姿勢(本体の角度,本体の角速度,車輪の角度,車輪の角速度)を推定するためのカルマン・フィルタです.

 1つめのカルマン・フィルタは本体の角度をより正確に知るために使い,2つめのカルマン・フィルタは本体を直立させる制御をより安定させるために使います.
 


図2 倒立振子キットのシステム・ブロック図

 

●プログラミング言語について

 本製品のマイコン用プログラムはC言語(厳密にはC++)で記述しています.詳細はソース・コードをご覧ください.

 

組み立て方

 ここからは,倒立振子ロボット・キット MZIP-01(図3図4)を組み立てて,動作させるまでの手順を解説します.

図3 前から見たようす

図4 横から見たようす



■STEP1 工具をそろえる

 ・はんだごて
 ・はんだ
 ・フラックス(例えば図5のようなもの)

図5 フラックスの例
HAKKO "FS-200"

 ・ピンセット(例えば図6のようなもの)

図6 ピンセットの例
HOZAN "P-891"


 ・プラスドライバー
 ・ペンチ
 ・ニッパ
 ・1.5mm あるいは 2mm の六角レンチ(シャフト・カプラのイモネジに合わせる.例えば図7のようなもの)

図7 六角レンチの例


 ・テーパ・リーマ(小さい穴を広げる工具なら何でも可.例えば図8のようなもの)

図8 テーパ・リーマの例


 ・スズメッキ線
 ・単3形 Ni-MH充電池(ニッケル水素充電池) 4本
 ・USB mini B  type A ケーブル

■STEP2 基板を作る

●モータ・ドライバICの取り付け
 基板上のU4およびU5の位置に,モータ・ドライバIC“BD6212HFP”をはんだ付けします.表面実装部品なので,フラックスがあると作業が楽です.
 まず,パッド(電極部分)にフラックスを塗り,図9のように部品の位置を合わせて置きます.

図9 パッドに合わせて部品を置く



 次に,図10のように1つのピンだけをはんだ付けします.

図10 1つのピンだけはんだ付けする

 この状態で位置がずれていたら,はんだを溶かして再度位置合わせをします.位置が決まったら,図11のようにすべてのピンとフィンの部分をはんだ付けします.はんだ付けする場合,あるいは間違って付けたはんだを取り除く場合は,適宜フラックスを塗りながら作業を行うようにすると簡単です.

図11 すべてのピンをはんだ付けする

●センサ・モジュールの組み立て
 センサ・モジュールを組み立てます.基板を裏返して“JP4”,“JP5”,“JP7”,“JP8”の4カ所をはんだでショートします(図12).

図12 センサ・モジュールのJP4,JP5,JP7,JP8をはんだでショートする


 続いて,図13のようにICソケットを利用してピン・ヘッダを取り付けます.

図13 ICソケットを利用して,ピン・ヘッダをはんだ付けする

●各部品の取り付け
 抵抗,キャパシタ,ピン・ソケット,LED,スイッチという具合に,背が低い順に部品を取り付けていきます.電解コンデンサは向かって左側がプラス,右側がマイナスです.センサ・モジュールは,図14のようにx,y,z軸の向きが基板側のシルクと一致するように取り付けます.

図14 センサ・モジュールの取り付け方向

●端子台をつくる
 図15のようにスズメッキ線をアーチ状に取り付けて,ロータリ・エンコーダ,電源,モータの端子台を作ります.切り取った部品のリードを使っても構いません.
 ケーブルを直接基板にはんだ付けしても構いませんが,端子台を用意しておいたほうがデバッグの際に便利です.

図15 スズメッキ線で端子台を作る

●NUCLEOボードとセンサ・モジュールの取り付け
 すべての部品をはんだ付けしたら,“NUCLEO-F401RE”マイコン・ボードを取り付けます(図16).

図16 すべての部品を基板に取り付けたようす
ジャンパ・ピン(赤)は"J1"に取り付けておく


 マイコン・ボード用のピン・ソケットは,一番上のピンが余ります(図17).
また,ジャンパ・ピン(赤)はJ1の“5V Ni-MH”と書かれている箇所に取り付けておいてください.

図17 ピン・ソケットの一番上のピンは余る(NC)

■STEP3 車体を作る

●ハイパワーギヤーボックスHE
 「ハイパワーギヤーボックスHE」を2つ組み立てます(図18).ギア比は“64.8:1”にします.シャフトは上側の穴を通すようにします.

図18 ハイパワーギヤーボックスHEを2個組み立てる


 左側のモータのケーブルは長さが足りなくなるので,別途20 cmの長いケーブルに付け替えてください.このとき,ケーブルの色はもともと付いていたケーブルと一致させるようにします.
 図19のように,モータの端子間にセラミック・コンデンサを取り付けます.

図19 モータの端子にセラミック・コンデンサを取り付け

●タイヤ
 「スポーツタイヤセット」を組み立てます(図20).ホイールハブは,ハイパワーギヤーボックスHEと組み合わせるために,溝がある方を使います.
 ホイールハブは,ホイールの浅いほうに取り付けることをおすすめします.完成したら,ギアボックスとタイヤを接続しておきます.

図20 スポーツタイヤセットを組み立てる

●ギアボックスの取り付け
 ギアボックスをシャーシ(ユニバーサルプレートL)に取り付けます.左右とも一番外側の位置で,下から7番目のネジ穴に固定します.ネジはギアボックスに付属している物を使用してください.

●ロータリ・エンコーダの取り付け
 ロータリ・エンコーダとギアボックスのシャフトを,シャフト・カプラ(シャフト・ジョイント)で接続します.
 ロータリ・エンコーダの位置を見積もり,図21のようにスポンジゴムを貼り付けておきます.スポンジゴムの形状は,おさまりが良いように適宜調節してください.

図21 スポンジゴムを貼り付ける


 先にロータリ・エンコーダにシャフト・カプラをはめてから,ギアボックス側のシャフトを挿入するとスムーズです.イモネジは,強く締めすぎないように注意してください.シャフト・カプラは,製品のロットによっては図22の物と形状が異なる場合があります.いずれの物でも問題なく使用できます.シャフト・カプラのイモネジ用の六角レンチのサイズは,実物に合わせて選択してください(1.5mmあるいは2mm).

図22 ロータリ・エンコーダとギアボックスをシャフト・カプラで結合する

●電池ボックスの取り付け
 電池ボックスのネジ穴は小さいので,テーパ・リーマなどで少しだけ穴を広げておきます.また,シャーシ側の穴もテーパ・リーマで少し広げておくと作業が楽です.電池ボックスは,シャーシの下から16番目の穴にネジで固定します.適宜,ワッシャを使ってください.

●基板の取り付け
 基板に長さ15 mmのスペーサを取り付けます.スペーサ用のネジは長さ7 mmのものを使ってください(10mmのネジも入っているので注意).基板は,シャーシの上から2番目の列の穴に固定します.

●配線する
 ロータリ・エンコーダ,電源,モータを基板に接続します.ロータリ・エンコーダはソケットの順番のとおり基板に接続してください.モータは,「青いケーブル」を“M1_1”および“M2_1”側に,「赤いケーブル」を“M1_2”および“M2_2”側に接続してください.もしモータのケーブルの極性がわからなくなった場合は,プログラムを書き込んだ後に実際に動作させながら極性を確認してください.

●車体の完成図
 図23図24にできあがった車体を示します.

図23 車体の完成図(表側)

図24 車体の完成図(裏側)

■STEP4 プログラムを書き込む

●NUCLEOボードのジャンパ確認
 NUCLEOボードにプログラムを書き込むときは,NUCLEOボード上の“JP5”に挿入するジャンパ・ピンが“U5V”側になっていることを確認してください(図25).また,制御基板のスイッチをOFFしてからPCと接続するようにしてください.

図25 プログラムを書き込むときは,"JP5"の"U5V"側をショートする

●4-1 バイナリ・ファイルをそのまま使う場合
 このキットの販売ページから,バイナリ・ファイル“IP-01.bin”をダウンロードしてください.NUCLEOボードとPCをUSB接続すると,フラッシュ・メモリとして認識されます.ここに,“IP-01.bin”をドラッグ&ドロップして書き込んでください.これで作業はすべて完了です.

●4-2 自分でコンパイルしてバイナリを生成する場合
▲mbedに登録する
 自分でプログラムを変更したい場合は,以下の手順にしたがってください.ここでは,NUCLEOボードのソフトウェア開発に“mbed”(エムベッド)プラットフォームを利用する方法を紹介します.
 mbedのページ(https://os.mbed.com)にアクセスし,“Sign up free”をクリックしてアカウントを作成します(図26).

図26 mbedのトップページ"Sign up for free"をクリック


▲コンパイラ画面を開く
 サイン・インしたら,画面右上の“Compiler”ボタンをクリックします.初めてコンパイラを使う場合は,図27のような画面が表示されます.“Personal projects”を選択して,“Next”をクリックします.

図27 初めてコンパイラを使うときに表示される画面"Personal projects"を選択する


 続いて図28のような画面が表示されるので,使用するマイコンを選択します.検索入力欄に“STM32F401”と入力すると“NUCLEO-F401RE”ボードが候補に挙がるので,これをクリックします.“Finish”をクリックすると,コンパイラのメイン画面に遷移します.

図28 "NUCLEO-F401RE"ボードを選択する


▲新しいプログラムを作成する
 以下,mbedのコンパイラ画面(https://ide.mbed.com/compiler)で作業を進めます.
 開発環境のページへ移動すると,図29のように新しいプログラムを作成するダイアログが出ます(出ない場合は,左側の列に表示される「マイプログラム」を右クリックして「新しいプログラム」をクリックします).ここで,「プラットフォーム」が“NUCLEO-F401RE”になっていることを確認してください.「テンプレート」は,デフォルトの“Nucleo board with Seeed bot and bluetooth shields demo”を選びます.「プログラム名」は好きに決めて構いませんが,今回は“test”としておきます.これで「OK」ボタンをクリックします.

図29 mbedで新しいプログラムを作る


 図30のようにプロジェクトに登録されているすべてのソース・コードおよび設定ファイルが表示されます.ここで“main.cpp”をダブルクリックすると,ソース・コードの編集画面が開きます.今回はプロジェクトの枠組みを利用するだけなので,もともと記述されているソース・コードは消去してください.
 このキットの販売ページから,C++ソース・コードの“IP-01.cpp”をダウンロードします.テキスト・エディタなどで開き,すべての内容をmbed上の“main.cpp”にコピー&ペーストします.“ctrl + d”キーを押すと,コンパイルが実行されます.
 必要に応じてソース・コードの内容を編集し,実験などにご利用ください.

図30 プログラムの管理画面
"main.cpp"をダブルクリックするとソース・コードを編集できる

■STEP5 動作確認

●Ni-MH充電池を使う場合(5V電源)
(1)NUCLEOボード上の“JP5”のジャンパ・ピンを“U5V”側にしてください.
(2)制御基板上の“JP1”(5V Ni-MHと書かれている)にジャンパ・ピンを取り付けてください.
(3)本体を垂直に立ててください.
(4)NUCLEOボード上のリセット・ボタン(黒)を押した状態で,電源スイッチをONしてください.
(5)数秒待ってから,NUCLEOボードのリセット・ボタンをはなします.
(6)リセット・ボタンをはなしてから1秒後に黄色LEDが点灯します.黄色LEDが点灯している間は,センサの雑音を測定しています.できるだけ本体を動かさないようにします(およそ2秒間).
(7)黄色LEDが消灯したら,通常動作モードに移ります.本体からそっと手をはなしてください.無事に直立すれば成功です.

 もし安定して直立しない場合は,黄色LEDが点灯している間に本体を動かしてしまった可能性があります.もう一度(6)の手順をやり直してください.それでも改善し無い場合はシャフト・カプラのイモネジが締まっているか,センサ・モジュールの取り付け向きは正しいか,モータの配線は正しいかなどの点をチェックしてください.

●アルカリ乾電池を使う場合(6V電源)
(1)NUCLEOボード上の“JP5”のジャンパ・ピンを“E5V”側にしてください.
(2)制御基板上の“JP2”(6V Alkalineと書かれている)にジャンパ・ピンを取り付けてください.
(3)以降は,上記と同様です.
 
 NUCLEOボードにプログラムを書き込む場合はNUCLEOボード上の“JP5”のジャンパ・ピンを“U5V”側に挿入する必要があります.ご注意ください.

 図31に倒立振子MZIP-01の全回路図を示します.

図31 倒立振子の全回路図




 

関連資料

●組み立てマニュアル
 こちらからダウンロードしてください

●STM32マイコン用のプログラム
 こちらからダウンロードしてください

●キットの購入
 こちらからお求めください

●本キットに関する問い合わせ先
 https://www.marutsu.co.jp/pc/static/contact/index

<文責 リニア・テック代表 別府 伸耕>

アクセスカウンターアクセスカウンタ

ページトップへ