Rover製作13 Ardupirotで自己位置・速度制御の設定をする

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| OS | Windows10 |
| MissionPlanner | v4.1.0 |

| 項目 | 値 |
|:--|:--|
| Board | pixhawk(v2.4.8) |
| Ardupilot | ArduRover(V4.1.5) |

# 概要
自己位置の設定をします。また自己位置をフィードバックして速度制御されるAcroモードを使用します。

# 予備知識

## EKF(拡張カルマンフィルタ)
カルマンフィルタは複数のセンサー情報を統合して、総合的な自己位置を取得するアルゴリズムです。ArdupilotのEKF3モジュールがこの機能を持ちます。IMUの情報をベースに、ホイールエンコーダー、RangeFinderの情報を統合します。
Roverは地上平面しか動けないのでz位置が固定になるのは自明に思えますが、Droneを念頭に作られているEKF3はこのような2次元モードはなく、Z位置も加速度センサーなどを使って推定します。
しかしROSなどの処理のためにZ位置を固定する方が便利です。手っ取り早い方法が今回使う下向きの距離センサーを使う方法です。

## Acroモード
速度制御をしながら機体の制御をするモードがAcroモードです。外部から速度制御をするGUIDEDモードでも同じ制御系を使うために今回設定をします。
本来はArdupilotの機能で速度PIDを行うのですが、今回はRoboclawで速度制御をするため基本FF項のみ使用します。

## Ardupilotのチューニング機能
Ardupilot&MissionPlannerにはチューニング機能があり、指定した項目について制御目標と制御結果などをプロットで表示できます。

# 事前条件

* WindowsPC
    * MissionPlanenr導入済み
* 走行可能な状態のRover

# 手順

## EKF3の設定
 
* MissionPlannerにてArdupilotに接続して、以下のパラメーターを設定する。

|パラメーター|値|説明|
|:--|:--|:--|
|AHRS_EKF_TYPE|3(default)|EKF3を使用|
|EK2_ENABLE|0(default)|EKF2を無効|
|EK3_ENABLE|1(default)|EKF2を有効|
|EK3_SRC1_POSXY|0|None(GPSを使用しない)|
|EK3_SRC1_POSZ|2|高度にRangeFinderを使用|
|EK3_SRC1_VELXY|7|WheelEncoderを使用|
|EK3_SRC1_VELZ|0|None(GPSを使用しない)|

* Pixhawkに電源を再投入する。

## EKF3の動作確認

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 「初期設定」->「Advanced」を選択、中央画面の中から「MAVLink Inspector」を選択する。
* ツリーを開いて「LOCAL_POSITION_NED」の値を確認する。
* 走行をして、各情報が想定通り表示されるか確認する。

## Acroモードの設定(回転方向)

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のようにパラメーターを設定する。
    * 一部項目は「設定/調整」->「ベーシックチューニング」でも選択できる。

|パラメーター|値|説明|UIでの項目名|
|:--|:--|:--|:--|
|ACRO_TURN_RATE|120|最大のyaw回転速度[deg/s]|--|
|ATC_STR_RAT_FF|0.20|回転制御のFFゲイン|FF|
|ATC_STR_RAT_P|0|回転制御のPゲイン|P|
|ATC_STR_RAT_I|0|回転制御のIゲイン|I|

![acro_turn1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/b521d7b9-a2be-ccb2-43a8-0ea22c6bd26a.png)

## Acroモードのチューニング(回転方向)

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のようにパラメーターを設定する。
    * ダイアログが出現するので、チューニングしたい対象を1つのみ選択する（1つだけにチェックをします）

|パラメーター|値|説明|
|:--|:--|:--|
|GCS_PID_MASK|1|Steering:回転方向のチューニング|

![mp_tune.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/fe8360f0-dfba-dbc0-1894-fc5772769666.png)
※上記スクショではThrottleを選択してますが、ここではSteeringを選択する。

* 「フライト・データ」画面で、画面下部の「チューニング」にチェックを入れる。
* 「Tuning」画面上でダブルクリックして「Display This」ウィンドウを開く。この画面で「piddesired」と「pidachived」をチェックして画面を閉じる

![mp_tune3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/6183b76d-54ee-49f0-a838-f42a6e965169.png)

* Roverを走行可能な状態にする。
    * 左肩のスイッチを奥に倒してフライトモードを「Hold」にする。
    * 右スティックを右いっぱいに数秒倒してARMする。
    * 左肩のスイッチを手前に倒してフライトモードを「Acro」にする。
* 右スティックを左右に倒して、2つのプロットがイッチするか確認する。

![pid_check.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/97d82031-6989-69ef-3f29-4237b94a251c.png)

## Acroモードの設定(前後方向)

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のようにパラメーターを設定する。
    * 一部項目は「設定/調整」->「ベーシックチューニング」でも選択できる。

|パラメーター|値|説明|UIでの項目名|
|:--|:--|:--|:--|
|CRUISE_SPEED|0.4|設定する最大速度[m/s]|クルーズ速度|
|CRUISE_TUROTTLE|50(default)|最大速度に対応するthrottel値[%]|クルーズ|
|ATC_SPEED_P|0.01|Pゲイン|P|
|ATC_SPEED_I|0|Iゲイン|I|
|ATC_SPEED_D|0|Dゲイン|D|
|ATC_ACCEL_MAX|1.0|加速時の最大加速度[m/s]|Accel Max|
|ATC_DECEL_MAX|3.0|減速時の最大加速度[m/s]|--|

![acro_thr_tune1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/9a6323df-391b-dac4-f1d0-986e3d76db69.png)

## Acroモードのチューニング(前後方向)

* WindowsPCでMissionPlannerを起動して、Ardupilotと接続する。
* 以下のようにパラメーターを設定する。

|パラメーター|値|説明|
|:--|:--|:--|
|GCS_PID_MASK|2|Throttle:進行方向のチューニング|

* 「フライト・データ」画面で、画面下部の「チューニング」にチェックを入れる。
* 回転方向と同様に確認する。

# 参考
[Ardupilot WheelEncoder設定](https://ardupilot.org/rover/docs/wheel-encoder.html)
[Ardupilot Acroモード](https://ardupilot.org/rover/docs/acro-mode.html)
[速度のチューニング](https://ardupilot.org/rover/docs/rover-tuning-throttle-and-speed.html)
[回転のチューニング](https://ardupilot.org/rover/docs/rover-tuning-steering-rate.html)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)
[ROS2講座の目次へのリンク](https://qiita.com/srs/items/5f0d765343ca78f524ed)