これは、ロボットを最初から実現し、未知の環境で自律的にナビゲートする機能を提供する方法に関する詳細なチュートリアルです。
ロボット工学に関する一般的な議論はすべて網羅されています。 力学 , エレクトロニクス そして プログラミング .
ロボット全体は、専門的な（すなわち高価な）道具や設備なしで、誰でも自宅で作ることができるように設計されている。
ブレインボード（dsNav ）は、エンコーダおよびモータコントローラ機能を備えたMicrochip社のdsPIC33 DSCに基づいています。位置は、オドメトリ（エンコーダ）によって外部参照なしで計算されます（推測航法）。
最終バージョンでは、他のいくつかのコントローラーがセンサーの制御（Arduino）とアナログセンサーの管理（PSoC）に使用されます。
用品：
ステップ1：基本プラットフォーム
プロ用の道具や機器を必要とせず、そして機械的な作業に関する特別なスキルを必要とせずに、どこにでも見つけやすい部品や部品で非常に単純なロボットプラットフォームを構築する方法の例。
ベースのサイズは、ロボットコンテストの多くの異なるカテゴリーでの使用を可能にします：Explorer、Line Follower、Can Collectorなど。
ステップ2：何を手に入れたいですか？そしてどうやって？
このロボットは、趣味で作られたロボットのほとんどがディファレンシャル・ステアリング・システムをベースにしているため、いつでもロボットの位置座標を知ることができ、各車輪が占めるスペースを十分な精度で定期的に知ることができます。
この推測航法ナビゲーションシステムは累積誤差の影響を受けます。長いパスの後に小さな誤差円を保証するためには、測定精度が高くなければなりません。そこで、自家製のエンコーダでいくつか良い結果を得た後、私はもっと良いものを使うことに決めました。12V-200rpmのギア付きモーター2つを300のCount Per Revolution（cpr）エンコーダに接続しました。
基本理念
4倍のデコード方法（120 kHz）で3000 rpmモーターで300 cprエンコーダによって生成されたすべてのパルスをキャッチするには、各エンコーダ専用のハードウェア（QEI = Quadrature Encoder Interface）が必要です。二重のPIC18F2431を試した後、正しいアップグレードはdsPICへのアップグレードであると判断しました。最初は、2つのdsPIC30F4012モーターコントローラーで、車輪の位置と速度を制御し、走行距離測定を行い、2つのモーターのデータをdsPIC30F3013に提供していました。この汎用DSCは、データを取得し、位置座標を計算するためにいくつかの三角法を実行し、フィールドのマップを取得するためにカバーされたパスに関連するデータを格納するのに十分強力です。
ボードとプログラムがほぼ完成したとき、Microchip社は、dsPIC33Fシリーズで、モータコントローラ（MC）と汎用（GP）の両方のバージョン用に、新しい強力な28ピンSPDIPを発表しました。 dsPIC30Fよりもはるかに高速で、はるかに多くの利用可能なプログラムメモリとRAMがあり（フィールドマッピングに便利）、必要な電力が少なく（バッテリ駆動のロボットに適しています）、DMA機能により多くのI / O操作が簡単になります。
最も重要なのは、これらが同じチップ上に2つのQEIを持つ最初のMicrochip社のモーターコントローラです。もう一度新しいポートを始めましょう！の 論理ブロック図は前のボードのものと似ています しかし、ハードウェアとソフトウェアはそれ以降ずっと単純になりました。 私は1つのDSCを3つだけのインスペクションで使うことができます 。ナビゲーションパラメータを交換するために、スーパーバイザとモータコントローラ間の高速通信は不要です。すべてのプロセスは同じチップ上にあるため、同期は簡単です。 dsPIC33Fシリーズのペリフェラルピン選択機能により、PCBがさらに簡素化され、ペリフェラルの内部接続とより高い柔軟性が可能になります。
これにより、「dsPICベースのナビゲーションコントロールボード」にアクセスできます。 dsNavCon 略して。このボードは、より複雑なシステムの一部として設計されています。完全な探査ロボットでは、他のボードが音、光、ガスセンサー、さらにバンパーや超音波距離計を制御して目標を見つけ、障害物を避けます。
スタンドアロンボードとして、 dsNavCon また、単純な「ラインフォロワ」ロボット、オドメトリや推測航法コンテストのためのロボットのようなより複雑なもの、または（「競技を集めることができる」ための）「キャン缶ロボット」にも使用できます。そのようなタスクのためのコードを追加するための空きプログラムメモリはまだたくさんあります。ソフトウェアを少し変更するか、まったく変更することなく、双方向RFモデムとある種のスマートリモートコントロールを使用して、遠隔操作車両用にスタンドアロンで使用することもできます。このリモコンは、「FWDを1m移動する」、「15°左に曲げる」、「50 cm / sでFWDを実行する」、「X、Y座標に移動する」などの複雑なコマンドを送信できます。
ボードとロボットも、プロの道具や道具を使わずに、誰でも家で作ることができるように設計されています。
ステップ3：オープンソースハードウェア
ブロック図
ナビゲーション制御サブシステムは、 dsNav システムの「スマート」ボード、およびギア付き12Vモーターを制御するL298ベースのデュアルHブリッジボード（Hsiang Neng HN-GH12-1634TR）。モーションフィードバックは、300個のcprエンコーダ（米国のデジタルe4p-300-079-ht）からのものです。
外界との通信は、2つのUARTシリアルインタフェースを介して行われます。 1つはテレメトリ用、もう1つはセンサーボードからデータを取得するためです。 XBeeモジュールは、JP1とJP2のジャンパを介してUART1またはUART2に接続できます。 J1およびJ16ソケットは他の種類の接続に利用できます。 dsPIC33Fシリーズのペリフェラルピン選択機能により、COMM1ポート（J16）もI2C通信に使用できます。
Eagleフォーマットの元の回路図は、ここにあります。
http://www.guiott.com/Rino/dsNavCon33/dsNavCon33_Eagle_project/DsPid33sch.zip
ご覧のとおり、回路図は非常に単純なので、私が行ったようにperfboardに実装できます。あなたがこのシステムを使いたくないし、あなた自身のPCBを実現したくないのであれば、私のオリジナルの作品に基づいていて、私のオープンソースソフトウェアと完全に互換性のある市販のボードが入手可能です。 .com / product_info.php？products_id = 1564
ステップ4：オープンソースソフトウェア
このソフトウェアはMPLAB®free IDEを使って開発され、MPLAB®C30コンパイラを使って書かれています。
http://www.microchip.com/stellent/idcplg?IdcService=SS_GET_PAGE&nodeId=81
プロジェクト全体がGoogle Codeのオープンソースとして公開されています。
http://code.google.com/p/dspid33/
最新版、コメント、説明などについては、こちらを参照してください。
プログラムはコード内で段階的に記述されています。高レベルのコメントとより読みやすいコードを持つために、MPLABプロジェクトの外部ファイル（すなわち：descrEng.txt）への参照として、重要な箇所に括弧で囲まれた番号（例：7）があります。 。
この図は、dsNavボードの制御手順の全体的なアーキテクチャとプロジェクトに基づいて適用されるナビゲーション戦略を示しています。
モーターコントローラは、車輪の速度を考慮するブラックボックスとして見ることができます。プログラムのスーパーバイザー部分はそれらに基準速度（VeldDesX：希望速度）を送る。マイクロコントローラのインプットキャプチャモジュールは、モータ軸に接続されているエンコーダからパルスを取得して、モータの回転速度（VelMesX：測定速度）を導き出します。 1msごとにPID制御「Speed PID」でこの値を組み合わせて、各単一ホイールの希望の速度を維持するために正しいPWM値を取得します。
QEI（Quadrature Encoder Interface）モジュールはエンコーダからAとBの両方のパルスを受け取り、4xモードでの進行方向とパルス数をスーパーバイザ機能に返します（信号Aと信号Bの立ち上がりエッジと立ち下がりエッジをカウントします）。 ２×２ ＝ ４）。
各エンコーダパルスごとに移動したスペースを示すKをパルス数に掛けて、10msごとに左右の車輪が移動した距離を求めます。スーパーバイザは、この移動情報を組み合わせて、ボットの測定位置座標を得るために推測航法手順を適用する：Ｘｍｅｓ、Ｙｍｅｓ、θＭｅｓ（方位角）。
スーパーバイザは、シリアルインタフェース（テレメトリ）によって外部からナビゲーションコマンドを受信します。
さまざまな戦略を適用できます。
A - 一定の速度で一定の方向（VelDes、θDes）に移動します。
B - 座標XDes、YDesで与えられた点に向かって移動します。
C - 所与の方向に所与の距離だけ移動する（ＤｉｓｔＤｅｓ、θＤｅｓ）。
モードA ：ポジション1の「論理制御スイッチ」の場合、スーパーバイザ機能ではPID制御「角度PID」のみが使用されます。これは、方位誤差を補正するのに必要な、車両のその垂直軸の周りの回転角速度ωの値を得るために、所望の角度θＤｅｓを走行距離測定法によって計算された測定角度θＭｅｓと組み合わせる。
DeltaVの値はωに比例します。ロボットの中心がまだVelDesの速度で移動している間、θDes値に対応する方位を維持するために、左車輪の速度を取得するためにVelDesに加算され、右車輪の速度を取得するためにVelDesに減算されます。
モードB ：ポジション2の「論理制御スイッチ」で、目標速度VelDesはPID制御「Dist PID」によって計算され、モードAと同様に使用されます。このPIDの測定入力（DistMes）は、次の関数として計算されます。現在の座標と目的地の座標所望の配向角θＤｅｓも同じ手順に由来し、それは「角度ＰＩＤ」に対する基準入力として使用される。 「Dist PID」の参照入力は0です。これは、宛先に到達したことを意味します。 ωとVelDesが利用可能な場合、車輪の速度制御はモードAのように動作します。
モードC 位置２の「論理制御スイッチ」を用いて、宛先座標Ｘ ｄｅｓ、Ｙ ｄｅｓは、入力パラメータＤｉｓｔＤｅｓ、θＤｅｓの関数として最初に一度計算される。その後はすべてモードBのようになります
ステップ5：ソフトウェアの詳細：速度制御とその他の基本機能
プログラムがいっぱいです 割り込み駆動 。起動時に、初期化後、プログラムはステートマシンとして機能する非常に単純なメインループに入ります。メインループでは、プログラムは外部イベントによって有効にされたフラグをチェックし、それらの値に従って相対状態に入ります。
とても単純な協同組合だから」リアルタイムオペレーティングシステム msgstr "" "各ルーチンは可能な限り最短時間で実行する必要があり、非常に頻繁な割込みに対処するためにシステムを解放します。
「待つまで」も、コードに遅延もありません。特に文字列の送信や受信のような遅い操作のために、可能な割り込みが使用されるときはいつでも。 UART通信は、dsPIC33FのDMA機能を利用して、ハードウェアですべての「ダーティ」な作業を行うCPU時間を節約します。
dsPIC33FJ128MC802で使用される周辺機器：
- 移動経路を計算するためのQEI。
- 速度を計算するためのインプットキャプチャ（IC）。
- モータ電流を読み取るためのA / Dコンバータ。
- モーターを駆動するための強化されたPWM。
- 外界と通信するためのUART
QEIモジュール 車輪がどれだけ移動したか、そしてどの方向に進んだかを知るために使われます。この値は、1msごとに変数に代数的に累積され、要求に応じてスーパーバイザ機能に送信されます。値が送信された後、変数はリセットされます。
下記のように、速度はエンコーダのパルスごとに測定されます。サンプルを平均化することによって平均速度を計算し、PIDアルゴリズムを実行し、その結果に応じてモーター速度を補正し、PWMデューティサイクルを変更します。 C30 PIDライブラリアプリケーションの詳細については、「マイクロチップコード例：CE019 - 閉ループ制御システムで比例積分微分（PID）コントローラーを使用する」を参照してください。 http://ww1.microchip.com/downloads/en/DeviceDoc/CE019_PID.zip
走行距離の誤差を引き起こす可能性のある大きな機械的歪みおよび車輪の滑りを回避するために、モータの速度変化は滑らかに実行され、上昇または下降傾斜ランプで加速または減速する。減速時は加速時よりも加速時の方が速く、制動中に障害物が衝突するのを防ぎます。
IC すなわち、入力捕捉モジュールは、エンコーダによって生成された２つのパルス間の経過時間を測定するために使用され、ｓは、車輪が周知の固定量の空間を走行したときを意味する（定数）。 SPACE_ENC ） QEAと並列に（dsPIC33Fのペリフェラルピン選択機能によりDSCの内部で）接続され、エンコーダ信号の立ち上がりエッジで経過時間をキャプチャします。 TIMER2はフリーランモードで使用されます。 ICが割り込みを受けるたびに、TMR2の現在の値が保存され、その前の値が減算されます。これがパルス周期です。その後、現在の値が前の値になり、次の割り込みを待ちます。 TMR2のフラグをチェックして、16ビットレジスタでオーバーフローが発生したかどうかを確認する必要があります。もしそうであれば、０ｘＦＦＦＦと前のサンプルとの間の差が現在の値に加えられなければならない。サンプルはに代数的に追加される IcPeriod に応じて可変 _UPDN 速度の方向も決定するためのビット。これは、で提案されている方法の1つです。 マイクロチップアプリケーションノートAN545 .
変数 IcIndx 追加されたサンプルの数を含みます IcPeriod .
1msごとに平均速度は次のように計算されます。 V =空間/時間
どこで スペース= SPACE_ENC•IcIndx
（= 1エンコーダパルスでカバーされるスペース•パルス数）
そして 時間= TCY•IcPeriod
（=単一のTMR期間•期間の合計が発生した）。
Single_TMR_period = TCY = 1 / FCY （クロック周波数）
そう V = Kvel•（IcIndx / IcPeriod）
どこで Kvel = SPACE_ENC•FCY 速度をm / sにします。
左に15ビットシフト クベル const（ KvelLong = Kvel << 15 ）速度はすでにPIDルーチンで使用できるように分数フォーマットで（整数変数のみが使用されている場合も）計算されています。より詳細な説明についてはMPLABプロジェクトの「descrEng.txt」ファイルを参照してください。
A / Dコンバータ モータ電流を連続的に測定し、16ポジションADCBUFバッファに値を保存します。バッファがいっぱいになると、割り込みが発生し、平均値が約1msごとに計算されます。
UART 外部からのコマンド受信や測定結果の返信に使用されます。プログラムの通信部分はステートマシンとして動作します。ステータス変数は、アクションを順番に実行するために使用されます。非常に単純で高速な割り込みサービスルーチン（ISR）は、バッファから1バイトごとに1バイトずつ取得または書き込みを行い、正しいフラグを設定して適切な機能を実行させます。
受信中に何らかのエラー（UART、チェックサム、構文解析エラー）が発生した場合、ステータス変数は負の数に設定され、赤いLEDが点灯してこの障害状態を外部に通知します。考えられるエラーの完全なリストはMPLABプロジェクトの“ descrEng.txt”ファイルを見てください。
ハンドシェイクに使用されるプロトコルは 物理層に依存しない また、I2CまたはRS485バスと通信するためにも使用できます。
の 第一層 dsPICペリフェラルインタフェースによって制御されます。フレームエラーまたはオーバーランエラー（UART）または衝突（I2C）がハードウェアによって検出され、適切なフラグが設定されます。
の 第二層 ISRルーチンによって処理されます。それらはインターフェイスから受信したバイトでRXバッファを埋めます。また、バッファオーバーフローとコマンドオーバーランも検出します。
UartRxまたはUartRx2関数は 第三層 。すでに説明したように（フローチャートも参照）、これらのルーチンはステートマシンとして機能し、バッファからバイトを取得してコマンド文字列をデコードします。
バイトは、循環バッファを介して2層目と3層目（ISRおよびUartRx関数）の間で交換されます。 ISRはバイトを受け取り、それを配列に格納し、配列へのポインタをインクリメントします。ポインタが配列の末尾に達すると、先頭から再開されます。 UartRx関数は、同じ配列を読み込むための独自のポインタを持ち、現在のRXステータスでバイトがデコードされるとすぐに（循環的にも）インクリメントされます。メインループは、 "in"ポインタが "out"ポインタと異なるときはいつでもUartRx関数を呼び出します。
各コマンドパケットは次のものから構成されます。
0 - ヘッダー@
1 - ID 0-9 ASCII
2 - コマンドA-Z ASCII
3 - CmdLen N = 1 - MAX_RX_BUFF後に続くバイト数（チェックサムを含む）
4 - データ…
…
N-1 - データ
N - メッセージを構成するすべてのバイトを8ビットの変数に単純に加算することによって得られるチェックサム0〜255（チェックサム自体は除外）。
この層はタイムアウトとチェックサムエラー、およびパケットの一貫性（正しいヘッダ、正しい長さ）を制御します。すべて問題なければ、Parserルーチンを有効にします（第四層 ）メッセージをデコードし、必要なアクションを実行します。受信したメッセージコードが不明の場合、このルーチンは適切なエラーフラグを設定します。
TMR1 プログラムのハートビート - 1000 Hzのタイミングクロックを生成します。各TMR1の割り込み時に、内部タイマーが更新され、ウォッチドッグがクリアされ、移動したスペース値を要求する機能を有効にするためのフラグが設定されます。 10msごとの“ All_Parameters_Ask”機能（速度、位置、電流）が有効になります。
ステップ6：ソフトウェアの詳細：オドメトリとフィールドマッピング=私はどこにいるの？
離散時間更新（走行距離計）で各車輪が移動した距離についての情報を得たら、外部参照なしで同じ周期性でロボットの位置座標を推定できます（推測航法）。
走行距離法による推測航法に関するいくつかの理論的背景は、Johann Borensteinの本にあります。
「私はどこにいるのか - 移動ロボット位置決めのためのセンサと方法」
そして、以下のWebページにあります。
http://www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html
数学的背景および使用される一般的方法の詳細な説明は、G.W。に見出すことができる。 Lucasの論文インターネットで入手可能な、ロボットホイールアクチュエータの差動操舵システムのためのチュートリアルと基本軌道モデル：
http://rossum.sourceforge.net/papers/DiffSteer/DiffSteer.html
同じドキュメントにも単純化されたアルゴリズムがいくつかあります。そのため、dsPIC33Fシリーズの数学的（三角法）機能を使用して、精度と精巧な速度の間で正しい妥協点を見つけることができます。
位置の計算に使用される数学の説明は、このステップに添付されている図にあります。最初の記号は記号の意味を示し、2番目の記号はそれらの記号で使用される式を示します。各計算ステップの横にあるボックスをクリックすると、簡単な説明が表示されます。
最後に、デカルト基準フィールドの方向のデルタ、X軸のデルタ、およびY軸のデルタとして、ロボットがその時間間隔でどれだけ移動したかがわかります。
各デルタ値をそれ自身の変数に累積すると、プラットフォームの現在の座標（位置と方向）がわかります。
計算誤差（ゼロ除算）とコントローラ時間の浪費を避けるために、SrとSlの両方の変数について事前にチェックを行う必要があります。最小の機械的および計算上の近似値を考慮した準ゼロ値を定義すると、ロボットが直線で移動している場合は式を単純化できます（右輪がカバーする空間は左輪が移動するスペースとほぼ同じです）。最後の図に示されているフローチャートで詳しく説明されているように、それが垂直軸の周りを旋回している場合（右の車輪が占める空間は左の車輪が移動する空間とほぼ同じですが反対方向）。
このビデオでは、どの精度が得られるかの例を示しています。
http://www.youtube.com/watch?v=d7KZDPJW5n8
フィールドマッピング
前の関数によって計算されたデータを使用して、フィールドマッピングが実行されます。
現在位置の精緻化の後、Ｘｍｓ毎に、未知のフィールドを１０×１０ｃｍのセルグリッドに分割してフィールドマッピングが行われる。 5×5mの最大フィールド寸法を定義すると、50×50 = 2500セルの行列が得られます。各セルはニブルで定義され、合計メモリ使用量は1250バイトです。各セルに16の異なる値を割り当てることができます。
n = 00未知セル
n = 01 - 10セルがn回訪問しました
n = 11の障害物が見つかりました
n = 12のガスターゲットが見つかりました
n = 13の軽いターゲットが見つかりました
n = 14のサウンドターゲットが見つかりました
ロボットはフィールド内のどの位置からでも起動できます。これらはその参照系の参照座標（0,0）になります。
フィールドマッピングは、未知のフィールドで最適な探索戦略を見つけるために役立ちます。ロボットは、探索されていないフィールドの部分（nの値が小さい）に移動したり、既に発見されたターゲットで2回停止しないことで時間を節約したり、特定の座標に到達するための最良の経路を見つけることができます。
ステップ7：リモートコンソール
開発を容易にし、どのオペレーティングシステムでも実行できるようにするために、 処理 言語：
http://www.processing.org/
このプログラムのソースコードもGoogle Codeのオープンソースとして入手できます。
http://code.google.com/p/dsnavconconsole/
とともに メインパネル （最初の写真）我々は設定可能なサイズフィールドでロボットがたどる経路をグリッド上で見ることによって遠隔測定を実行することができます。 dsNav .
ゲージは測定値を示します。
- MesSpeedは、+ / - 500 mm / sの範囲で、2つの車輪の速度の平均値（プラットフォームの中心の速度）です。
- 測定電流（mA）（2つのモーターからの電流の合計）。
- オドメトリーによって推定されるように、測定角度。
他のパネルを使用して、ロボットのパラメータを設定し、必要に応じて追跡するために定義されたパスをロボットに保存します。少なくともロボットの開発において重要なパネルは 詳細パネル （2枚目の写真）は各ホイールの速度をリアルタイムで表示します。すべてのパラメータの校正に非常に役立ちます。
中央のグリッド表示はWebカメラの表示で切り替えることができ、たとえ表示によっても、ロボットがたどる経路を制御できます。
このコンソールの実用的な使用例は、このビデオに示されています。
http://www.youtube.com/watch?v=OPiaMkCJ-r0
