3シャーシソフトウェアフレームワーク

4参照

このトピックでは、メカナムホイールを使用してROSシャーシを構築する方法を紹介します。標準のROSシャーシを参照すると、ROSシャーシは、ナビゲーションノードによって送信されたトピック「cmd_vel」に関する制御コマンド（X、Y、Z方向の車の速度と3方向の角速度）、およびシャーシホイールの速度を受信できる必要があります。この情報は、シャーシの位置を推定し、オドメトリトピックで公開します。

トピック全体は4つの部分に分かれています。

1.まず、車のハードウェア構造を紹介します。

2.次に、メカナムホイールの運動モデルを紹介します。

3.次に、リモコンを使用してメカナムホイールを制御し、前後左右に移動して、リモートコントロールを実現します。

4.最後に、ROSノードを作成して、「cmd_vel」トピックのデータをシャーシに転送します。

すべてのコードはGithubに配置されています。ダウンロードしてメッセージを残してください。

1.シャーシのハードウェアブロック図

シャーシの主なタスクは、ホストコンピューターからの指示（つまり、ROSノードの指示）を受信し、各モーターの速度を制御し、シャーシ上のIMU、超音波、モーター速度などのセンサーとアクチュエーターの情報をROSのシャーシノードにアップロードすることです。ここでは、それぞれセンサーとアクチュエーターを使用します：2.4Gリモコン受信機、IMU、超音波センサー、4方向DJI M3508モーターとドライバー。それらの中で、リモートコントロールはリモートコントロールシャーシとスイッチ制御方法を支援するために使用され、超音波はライダーの死角を補うために使用され、IMUはシャーシの姿勢と方向角を測定するために使用されます（一部のシャーシでは、IMU情報はコーナーコントロールのクローズドループを作成するためにも使用されます）

図1-1車のハードウェアブロック図

車の実際のハードウェアを下図に示します。左上隅のモーター番号を＃1、左下を＃2、右下を＃3、右上を＃4と定義します。正面はX方向、水平方向はY方向です（ROSの座標系と同じ）。

図1-2車の実際のハードウェア

シャーシの開発タスクは、4つのステップで実現されます。

最初のステップは、センサー（DBUS受信、~~IMUデータ読み取り、超音波データ読み取り~~）およびモータードライバー（C620）データを読み取るために通信リンクを開くことです。

ステップ2：PIコントローラーを使用して、モーターの速度閉ループ制御を実現します。

3番目のステップ：メカナムホイール速度逆解モデルを使用して、X / Y / Wの3方向の速度制御を実現します。

4番目のステップ：命令プロトコルを送受信し、IMU、超音波、モーター速度情報をアップロードするためにホストコンピューターを完成させます。

次に、最初にメカナムホイールの運動学モデルを紹介します。この運動学モデルは、主にX、Y、Wの3方向の速度と4つのモーター（V1、V2、V3、V4）の速度を促進するためのものです間のマッピング関係を確立します。制御時に、ナビゲーションモジュールはXおよびY方向の速度（m / s）と車の回転速度W（rad / s）を提供します。走行距離計を公開するときは、4つのモーターの速度または回転を読み取る必要がありますトロリーの位置座標とヘディング角度を計算します。

2.メカナムホイールモデルの概要

まず、メカナムホイールシャーシが前後左右の回転運動をどのように実現しているかをマクロの視点から分析してみましょう。

a。前方および後方への動きb。左右の動き

c。45度の斜めの動きd。回転の動き

図2-1メカナムホイールの運動学モデルの概略図

図1に示すように、メカナムホイールは「メーター」の形で取り付けられています。車の次の動きを制御する必要があるとします。

前進してから、4つの車輪を前進させる必要があります
左に移動するときは、左前輪と右後輪を後方に、左前輪と右前輪を前に回す必要があります。
右に前進すると、左前輪と右輪が前方に回転します
反時計回りに回転すると、左フロントと左バックが後方に回転し、右フロントと右バックが前方に回転します

参考[1]のブログでは、メカナムホイールを説明する古典的なチュートリアルが提供されていますが、未知の理由によりこのチュートリアルの写真を表示できないことが問題です。DJIの公式Webサイトのチュートリアルを参照してください。終了しました。

最初に、トロリーの左側と右側をY方向として定義します。ここで、左方向は正、前と後はX方向、正面は正です。トロリーは正として反時計回りに回転し ${w_v}$ ます。車の速度がであるとすると $\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {\ rm {]}$ 、Mecanumホイールには、左上隅から右上隅にN = 1,2,3,4というラベルが付けられます。（Xn、Yn）は原点を基準としたn番目のホイールの座標であり、速度は ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { }} \; {w_n} {\ rm {]}}$ 次のように表されます。

各ウィートホイールハブはX軸に平行に取り付けられ（通常はメーターの自己形状で取り付けられます）、rはハブの半径であり、反時計回りの方向の偏向は正です。 $\ theta _N$ ウィートホイールnの接地ローラーの軸とX軸の間の角度です（つまり、ローラーのオフセット設置角度。サイズは45°です）。ローラーは、Mecanumホイールで傾いているローラーです。4輪の速度はそれぞれ ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { }} \; {w_n} {\ rm {]}}$ です。これらを定義した後、メカナムホイールの運動学モデルの導出を開始します。

図2-2車のハードウェアブロック図

2.1インバースキネマティクスモデル

$\ Omega {\ rm {= [}} {w_1}、{w_2}、{w_3}、{w_4} {{\ rm {]}} ^ T}$ 車の速度を解くための4つの車輪の速度を知ること、 ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {{\ rm {]}} ^ T}$ つまり、次の条件を満たす4x3行列Rを見つけること。

$\ Omega {\ rm {=}} \ frac {1} {{\ rm {r}}} RV$ （2.1）

車体が剛体であると仮定すると、各ウィートホイールの中心の ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$ 水平速度成分と垂直速度成分は、車の水平速度成分と垂直速度成分に等しくなり ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} {\ rm {]}}$ ます。車体は、回転速度を有するために加えて、 ${w_v}$ それは ${w_v}$ また、各小麦ホイール上（水平および垂直）速度成分を生成します ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$

$\ begin {array} {l} {{\ rm {v}} _ {xrn}} = {\ rm {-Yn}} \ cdot {w_n} \\ {{\ rm {v}} _ {yrn}} = X {\ rm {n}} \ cdot {w_n} \ end {array}$ （2.2）

したがって、一般に、水平方向と垂直方向の各ウィートホイールの速度成分は次のとおりです。

$\ begin {array} {l} {{\ rm {v}} _ {xn}} {\ rm {=}} {{\ rm {v}} _ x} {\ rm {+}} {{\ rm { v}} _ {xrn}} = {{\ rm {v}} _ x} {\ rm {-}} {{\ rm {Y}} _ n} \ cdot {w_n} \\ {{\ rm {v} } _ {yn}} {\ rm {=}} {{\ rm {v}} _ y} {\ rm {+}} {{\ rm {v}} _ {yrn}} = {{\ rm {v }} _ y} {\ rm {+}} {X_n} \ cdot {w_n} \ end {array}$ （2.3）

この時点で、車体の速度 ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {\ rm {]}}$ と各ウィートホイールの速度 ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$ の関係がわかりました。次に、各ホイールの速度 ${w_n}$ と ${v_n}$ 構造が完了したことの関係を見つけるだけです。次の図は、特定の瞬間における右上の車輪の速度ベクトルの関係を示しています。

図2-3車のハードウェアブロック図

図2-3の合成速度ベクトルVnは、速度成分VxnとVynによって合成された小麦ホイールnの現在の実際の移動速度です。この図では、Vnは、ウィートホイールnのハブ中心速度Aî=ωn・rと接地フリーローラーの中心速度Bûから合成されていることもわかります（フリーローラーの中心速度B は、速度ベクトルVn とともに変化します））。だから方程式があります：

${w_n} r = {{\ rm {v}} _ {xn}} {\ rm {+}} {{\ rm {v}} _ {in}} * \ tan {\ theta _n}$ （2.4）

に変換：

$\ begin {array} {l} {w_n} = \ frac {1} {r} [（{{\ rm {v}} _ x} {\ rm {-}} {{\ rm {Y}} _ n} \ cdot {w_n}）{\ rm {+}}（{{\ rm {v}} _ y} {\ rm {+}} {X_n} \ cdot {w_n}）\ tan {\ theta _n}] \\ \ ; \; \; \; \; = \ frac {1} {r} [{{\ rm {v}} _ x} {\ rm {+}} {{\ rm {v}} _ y} \ tan {\ theta _n} {\ rm {+} }（{X_n} \ tan {\ theta _n}-{{\ rm {Y}} _ n}）\ cdot {w_n}] \ end {array}$ （2.5）

4輪の場合：

$\ left [{\ begin {array} {* {20} {c}} {{w_1}} \\ {{w_2}} \\ {{w_3}} \\ {{w_4}} \ end {array}} \ right] {\ rm {=}} \ frac {1} {{\ rm {r}}} \ left [{\ begin {array} {* {20} {c}} 1＆{\ tan {\ theta _1 }}＆{{X_1} \ tan {\ theta _1}-{{\ rm {Y}} _ 1}} \\ 1＆{\ tan {\ theta _2}}＆{{X_2} \ tan {\ theta _2} -{{\ rm {Y}} _ 2}} \\ 1＆{\ tan {\ theta _3}}＆{{X_3} \ tan {\ theta _3}-{{\ rm {Y}} _ 3}} \\ 1＆{\ tan {\ theta _4}}＆{{X_4} \ tan {\ theta _4}-{{\ rm {Y}} _ 4}} \ end {array}} \ right] \ left [{\ begin {配列} {* {20} {c}} {{{\ rm {v}} _ x}} \\ {{{\ rm {v}} _ y}} \\ {{{\ rm {v}} _ z} } \ end {array}} \ right] \;$ （2.6）

それは： $\ Omega {\ rm {=}} \ frac {1} {{\ rm {r}}} RV$

最初の図のθn の定義によれば、θ1 とθ3 は-45°、θ2 とθ4 は+ 45° であるため、R 行列は次のようになります。

${\ rm {R =}} \ left [{\ begin {array} {* {20} {c}} 1＆{-1}＆{-{X_1}-{{\ rm {Y}} _ 1}} \ \ 1＆1＆{{X_2}-{{\ rm {Y}} _ 2}} \\ 1＆{-1}＆{-{X_3}-{{\ rm {Y}} _ 3}} \\ 1＆1＆{{X_4} -{{\ rm {Y}} _ 4}} \ end {array}} \ right]$ （2.7）

通常、私たちが設計するシャーシは軸対称です。つまり、原点から座標系の4つの車輪までの距離は同じです。したがって、各ホイールの水平座標と垂直座標の絶対値は等しくなります。K= abs（Xn）+ abs（Yn）とします。各ホイール座標の象限の符号によれば、行列は次のように簡略化できます。

${\ rm {R =}} \ left [{\ begin {array} {* {20} {c}} 1＆{-1}＆{-{\ rm {K}}} \\ 1＆1＆{-{\ rm {K}}} \\ 1＆{-1}＆{\ rm {K}} \\ 1＆1＆{\ rm {K}} \ end {array}} \ right]$ （2.8）

さらに、îとofの2つの単位ベクトルの線形結合を考えると、XOY平面にベクトル空間全体を形成するだけです（もちろん、速度は無限ではありませんが、方向は無指向性でなければなりません）。運動の理由。Aîが常にモーターの軸に垂直（Y軸に平行）であるように、ローラーはその回転軸に垂直な方向に回転する必要があるため、Bûは常にローラーの軸に垂直です。

2.2正の運動学モデル

正の運動学とは、各モーターの速度を知り、車体の速度を見つけることです。数学的な形で見つけることである3×4の行列[F]を満足します。

$\; {\ rm {F}} \ Omega {\ rm {r =}} V$ （2.9）

この方法は、逆運動学方程式から始めて、最小二乗法[F] によって最適解の行列を提供することです。

$\ Omega {\ rm {=}} \ frac {1} {{\ rm {r}}} RV$ （2.10）

同時に[R]の転置を両側に掛けて、以下を取得します。

${R ^ {\ rm {T}}} \ Omega {\ rm {=}} \ frac {1} {{\ rm {r}}} {R ^ {\ rm {T}}} RV$ （2.11）

両方を同時に[R] '[R] の逆数で乗算すると、次のようになります。

${（{R ^ {\ rm {T}}} R）^ {-1}} {R ^ {\ rm {T}}} \ Omega {\ rm {=}} \ frac {1} {{\ rm {r}}} V$ （2.12）

右辺は[V]に削減され、次のようになります。

${（{R ^ {\ rm {T}}} R）^ {-1}} {R ^ {\ rm {T}}} \ Omega {\ rm {r =}} V$ （2.13）

これは[F] を与えます：

$\; {\ rm {F}} \ Omega {\ rm {r =}} V \;、{\ rm {F =}} {（{R ^ {\ rm {T}}} R）^ {-1 }} {R ^ {\ rm {T}}} $$ （2.14）

単純化された[R] 行列をインバースキネマティクスで使用すると、[F]を簡単に見つけることができます。

注意：

式2.1の速度 $V$ の単位はm / sで、回転速度 $\オメガ$ の単位はrad / s（ラジアン/秒）です。DJIC620モーターエンコーダーのフィードバック速度はrpm（回転/分）であり、変換する必要があります（ $\オメガ$ * 2 * $\ pi$ = rpm / 60）

3シャーシソフトウェアフレームワーク

前章では、メカナムホイールシャーシの基本的なフレームワークとメカナムホイールの運動モデルを紹介しましたが、ここでは、DJIのリモートコントロールを使用して、シャーシを前後左右に動かすように制御します。

シャーシはSTM32F1に基づいており、シャーシは次の機能モジュールに分割できます。

C620 ESC CANドライブモジュール、CAN割り込みを介してモーターエンコーダーフィードバックデータを受信
DBUSプロトコルのリモコンデータを受信
速度PID閉ループの設定
4つのモーターの回転角度、速度、温度をホストコンピューターにアップロードする

具体的には、まずシリアルポート1を使用してレシーバーからデータを受信し、次にメカナムホイール運動学モデルを使用してX、Y、Wの3方向の速度を4つのモーターの目標速度（RPM）に変換し、PIに送信します目標速度として制御すると、PIコントローラーが4つの車輪の目標速度を受信して速度閉ループ制御を達成し、制御値を計算して、CANバスを介してモーターに送信します。最後に、ステータスデータアップロードモジュールは、モーター、IMU、および超音波データをシャーシ上のホストコンピューターに定期的にアップロードします。

配線手順：

接続するには、C620 ESCのCANバスをSTM32に接続するだけでよく、シリアルポート1の受信ピンがレシーバーに接続され（レシーバーとSTM32の間に外部インバーターが必要です）、シリアルポート2はUSBからシリアルラインに接続されますホストコンピュータに行くだけです。

注：（DJIリモコンのSW1を有効にするには、CLまたはHLに切り替える必要があります。これは、リモコンが制御されていることを意味します。オフの位置は、リモコンをキャンセルし、ホストコンピューターによって制御されます）

コードは投稿されておらず、興味のある友人がそれをダウンロードしています（シャーシのコード）。この時点で、基本的にリモートコントロールを使用してメカナムホイールシャーシをリモート制御し、前後左右の3方向の動きを実現できます。最後に、エフェクト画像を貼り付けます。