機体のアルゴリズム検討

[masato1004]

ARマーカーの認識に基づいた制御アルゴリズムの検討　3/31迄

@suzukiYU000

本issueに検討内容（iPadで描いた絵やスライドなど）を投稿すること

---

検討内容

• マーカー付近のある一点（目標空間）に移動するための移動アルゴリズム
• マーカーが二つあるときの適切な機体位置の判断基準検討
  • ＋移動アルゴリズム（おそらく上記と同様のアルゴリズムになる）

[masato1004]

マーカーに基づく機体制御アルゴリズム検討

[suzukiYU000]

モジュール放出後の移動

• アニメーションは横から接近の場合
• 一周回るより所望の位置に到達しやすそう。

---

モジュールの正面から接近

• モジュールの面の向き、機体の位置に寄らず、マーカーが認識できる範囲であれば、到達できそう。
• 正面からの位置を遠くしておけば、後から微調整もできそう。

---

もう少し賢く接近

• 先の移動では法線上に垂直に移動していたが、この案では直進しながら法線上に乗るため、ロスが少ない
• 面が機体側を向いているときのみ可能

---

2つのモジュールと機体の適切な位置関係と移動

• その場回転による配線移動を想定した場合、アーム先端が移動する円周上にモジュールがあると、接続が可能と判断できる。
• 斜辺をアーム長とする二等辺三角形の頂点に移動する。
• 線を持った状態で回るために、接近時は配線のある方向を向きながら、頂点に移動する必要がある。

---

メモ

• 配線の移動アルゴリズムに適した接近方法を考える必要あり。
• 配線を持ったままどのくらい移動していいのか。

[masato1004]

FB

to: @suzukiYU000 Cc: @OtaKo50 （上記検討内容について意見あれば是非！）

モジュール放出後の移動について

• 一周するより短く済みそうでよい！が，これを曲線軌跡によって実現出来たらさらによい？

  モジュールの正面から接近について

• 「モジュール放出後の移動」のどの部分で入ってくる動作になる？
  • 一旦横からの接近を終わらせてしまってからずれを考えるのか，その手前で考えるのか
• 絶対にカメラにマーカーが映らない時間があるけど，その間マーカー位置の推定などはしない？完全に手前で計算しきってから動く？
  • この案ではおそらくマーカーを全く参考にしない期間があるけど，マーカーが映っている場合と映っていない場合の処理を連続的に切り替えられたら面白い ➡if分による切り替えでマーカーが映っていない瞬間のみ時刻歴情報から位置を推定する等

    もう少し賢く接近について

    【高難易度】

• 曲線軌跡によって，最終的な目標位置・目標座標に到達するアルゴリズムができれば面白い ➡一つ思ったのはベジェ曲線等の離散点から算出する曲線に基づく経路なら最終的な位置・角度を満たせそう？

  2つのモジュールと機体の適切な位置関係と移動について（メモを含む）

• ハード的なアームの詳細検討が現在進行形なので，一旦これがベストな気がします！

  ---

  追記 4/3

  機体の回転について

• 機体の向きを変更する際，左右の車輪の出力を前後にしてその場で回転することは可能
  ➡機体全体を後進させることが難しい（スタビライザがないため）

[masato1004]

アルゴリズム検討現状まとめ

[masato1004]

スライド作成について

@suzukiYU000 検討してもらった動画をパワポのアニメーションで作ってくれた（らしい）ので，うまい具合にテキストの説明と対応させながら図示してもらえれば！

[suzukiYU000]

モジュール放出後の移動について

前方のカメラからでは放出直後はモジュールは見えないので、あらかじめプログラムされた動きになります。 空転の可能性が高い路面では、設定する走行距離が長いほど、ズレが生じやすくなる可能性が上がりそうです。 解決案簡単なフローにする（→①）か、自己位置推定的なことをする（→②）

①簡単なフロー

• 直進とその場回転

  直進はできそう＆回転は方位で簡単そうという仮定の下。

• その場回転（180度）

  遠くなければ、回転中に見つかる可能性が高いので、探索すること考えると一番安全かつ省エネです。

②自己位置推定

マーカーを認識するまでの間で自己位置推定に使えそうな情報は今のとこは以下の2つ？

• 方位センサ

  ○機体の向いてる方向がわかる ×磁石が近くにあると微妙かも

• 加速度センサ

  ○移動距離わかる（誤差すごそう） ×ガタガタな路面での誤差凄そう 他のステートでも使えそうなので、使えそうであればやりたいです。

モジュールの正面から接近について

横からの場合、認識できた時点で法線考えるフローに移っても問題なさそうです。 見えない走行区間で自己位置推定やモーター入力に使える情報はやはり方位とかですかね。

もう少し賢く接近について

マーカー位置の位置によって限られてしまう。使えるのは、所望の位置から大きくずれていた場合。 となるとあまり使う機会がないかもしれないです。 曲線的に移動できるようにした方が移動距離を見ても省エネなのが明らかなのでできるようにしたいです。

---

一度近づいたら後退できないので、接近しすぎる前に二つのマーカーを見つけて、二等辺三角形の頂点の位置に移動するのが良さそうです。 ➩ 「頂点の位置に所望の向きで到着する」 が課題になりそうです。（←苦戦してます。）

[masato1004]

モジュール放出後の移動について

@suzukiYU000

フローについて

• 直進 & その場回転は一番省エネ感あってよきだと思う！！ ➡ 角度指定というよりAR基準にすると確実だったりするかな？
  • 画角内にARが入るまでその場回転
  • ARが価格中心にくるまで徐々に回転

    自己位置推定について

• 方位センサ・加速度センサに関しては，言ってくれている通りほかのステートでも使うのでやってみる価値はあると思う
• 一方で，どちらのセンサを使用した場合にもARマーカーとは完全に独立した自己位置推定になる ➡ ミッションで大事になるのはマーカーと機体の位置関係なので，自己位置推定≒マーカー位置推定ともいえる...？ ➡ であればマーカー座標のカルマンフィルタ（推定）みたいなことも考えられたりするかな

  モジュール正面からの接近について

• 直線 & その場回転ができそうであればあまり問題なさそうな気もするね
• 自己位置推定に関しては上記の通り

  賢い接近

• 近くなりすぎてしまう場合はたしかにあるね そもそも大きく位置も角度もずれてしまうことがなければ必要ないのか...？ ➡これに関しても直進 & その場回転で解決してしまう内容？
• おそらく直進のみを使用した移動アルゴリズムは一度ずれたら弱い気がするので，できれば曲線を描けるようにしたいよね ➡ 「曲線的な経路計画」みたいなワードでロボットの移動アルゴリズム関連の論文とかあされるといいね

  ---

  頂点の位置に所望の向きで到着する

• おそらく上記の曲線的な経路での走行に近しい部分もあると思う ➡ 経路の途中で扇形の経路を入れ込むなど これに関しても何か論文ベースで考えられるといいかもしれない（今後井上も調べていきます）

[suzukiYU000]

フローについて

"AR基準にする" 　➩これで行けそうです！

自己位置推定について

カルマンフィルタについて、勉強不足すぎるので、調べてみます！

賢い接近 & 頂点の位置に所望の向きで到着する

一つ見つけたので、張っておきます。

PythonによるDubinsパスプランニングサンプルプログラム

位置と角度を指定してコースを作るアルゴリズムが紹介されてました。

• サンプルコードあり
• 計算負荷小さい
• 横の移動に対する制約もつけられそう

おそらくこれで作成されるコースでも、マーカーを見失いそうな区間がありそうなので、 これも見える区間と見えない区間の移動方法の切り替えをすると良さそうです。

スライドについて

機体移動のスライドは、上記URLの方法の方が学術的で良さそうですかね...

[masato1004]

FB

@suzukiYU000

位置と角度を指定した接近について

• Dubins pathについて，サンプルコード実装してみたけどよさそうだね！ ➡ 指摘してくれている通り，マーカーが見えなくなる区間が出てくることもあるけど，このdubins pathは2点がわかった段階で事前に経路を計画することになりそうだよね ➡ 見えない間は計画した経路通りに進む・見える場合には経路を数秒に一回算出しなおすとかかな

  スライドについて

  上記の理論を説明的に情報として載せられるといい！ 現在考えてくれている内容に対してスライドがやや単調なので，より理論的な内容に踏み込んでスライドに記載してくれてもいいかも！

[masato1004]

移動アルゴリズムの検討・実装検証

[OtaKo50]

Dubins path について

サンプルコードを用いて、経路途中で再びdubins [pathによる経路計算を行った。 直線の途中で再び計算すると、ほとんど同じ経路(赤)となった。 ただし少しずれている？ように見えるのでどのくらい誤差が出ているかはまだ不明。

サンプルコードの経路

途中計算あり

[OtaKo50]

また、近すぎると(指定した半径と関係してそう)大きく経路が変更されてしまうので、経路計算の更新はRSLやRSRなどによる3つの流れのうち二つ目までに行えば良い？

[suzukiYU000]

Dubinsパスの追従方法の検討　

前に載せたサイト：Dubinsパス

作戦①

Dubinsパスを計算後、現在位置からゴールまでのモーターの出力を算出 ➩ Dubinsパスの円の半径は一定のため、円周を回るような左右のモーターの出力バランスは既知 　➩Dubinsパスの計算からカーブ、ストレートの長さは既知 　　➩所望の距離の分だけモーター出力

作戦②

2つのマーカーが見えたらその都度再計算 ➩経路追従の誤差をその都度補正可能

おそらく、①と②のMIX

経路にチェックポイントを設定し、見えたら補正

---

次回

ロバストな経路追従方法の検討

　磁気センサとか？ 　"センサフュージョン"ってワードがイケてる

作戦①と②のMIXの仕方

　どれくらいの頻度で再計算する？

[OtaKo50]

dubins path の運用アイデアメモ

進んでる最中に停止＆色検出を挟む？

[OtaKo50]

その他運用アイデアメモ

経路生成のための終点の推定のとき、色検出による距離推定はピクセルの面積から算出 ➡色の見えた位置がカメラの画角のどこにあるかによって距離の範囲を制限する！ 　➡モジュールが一部見えず実際より遠い距離が算出されても外れ値として扱えそう

[OtaKo50]

今の近づく方針

1.色認識による始点の修正（回転） 2.dubins pathによる経路計算と走行 （3.計算しなおし、経路修正）

[suzukiYU000]

2つのARマーカーから目標点の算出方法

ARマーカーを読み取り、DubinsPathを生成

https://user-images.githubusercontent.com/126460582/235405116-f86e75b7-eb81-43a2-b04d-04860e86bd98.MOV

＜メモ＞

• 角度計算が変
• カメラが壊れている（3号機） 　⇒DubinsPathを使えるほどの距離で実験できていない

[19238901]

新OS × 旧機体　動作確認　（実施日:2023/05/15）

動作確認動画

連絡事項

• 旧機体（3号機）に新OS（この前いれなおしたSD）を差し込み済み
• 上記のSDはAP化済み
• モータ用電池　フル充電後使用
• ラズパイ用電池　フル充電後使用
• カメラ動作確認→問題なし
• モータ動作確認→問題あり
  • 問題
  • Motor_test.py実行時，モータが回転し続ける
    • やばそうな音が出る
    • Ctrl + Cでも止まらない
  • 原因
  • ターミナルが正常に起動しなかった？
  • 解決策
  • 一時的
    • 回転部分をコメントアウト
    • モータストップ用として保存
  • 未検証
    • ターミナルで実行する
  • タスク
  • [x] モータストップ用のコード作成（非常用）
  • [ ] Motor_test.pyの改善
  • [ ] ターミナルで実行してモータが正常に動作するか検証

環境

• 旧機体（3号機）
• ラズパイ用電池　フル充電済み（最終兵器）
• モータ用電池　フル充電済み

できたこと

• カメラ動作確認
  • AFモード　動作確認
  • ARマーカ検出　動作確認
• モータ動作確認
  • モータ出力　動作確認
  • ARマーカ検出結果に基づいた行動

できなかったこと・要検討事項

• カメラ
  • 行動時，カメラ入力がない
• モータ
  • 行動計画が不完全？
  • 壁に激突
  • Uターン（どこ向いてるの？）
  • 画角からARマーカの消失
  • 最初の検出に依存しすぎ？
  • 処理が重い？
  • モータ出力速度の検討
• 電池
  • 消費電力の測定

[masato1004]

実装について

@KeioTeamWolvez2023/soft 班長がいない中積極的にやっていただきありがとうございます！！ 良い進捗が埋めているように思います．

連絡事項について

モーターストップファイル

これに関しては昨年から同じ問題があって，legacy_code/Motor/motor_stop.py当該ファイルが存在します．今年も流用できます．

モーターの異音

こちらはラズパイを使用している時ならではのエラーで，半分くらい仕方ないです．

• 原因
  • プログラム実行中，エラーにより中断された場合
  • Ctrl+Cによる中断を試みた際，時々エクセプションでバグって爆速で回り始める
• 昨年度の解決方法
  • できるだけモーター以外のエラーが起こりそうな状態でモーターを使用しない
  • スクリプトファイルを開いて実行し，常にmotor_stop.pyを実行できる環境にしておく．
  • どうしても止められない場合，モータードライバーがやられてしまうのでモーターのバッテリーを引っこ抜く（最終手段）

できなかったこと・要検討事項

カメラについて

• 使用した画角・画質設定はいくつ？
• カメラ入力がないというのは制御ループの中で撮影ができていないということ？

  パスプランニング・追従について

• モーター出力は（経験的に）適切な出力を決めることができた？
• おおよそ経路追従はできていた？

[suzukiYU000]

走行中のマーカー認識試験

結果：出力30でゆっくり走行してもブレる。

対策案

• 一時停止しながら接近する方法に変更
• ブレやボケを補正するプログラムの導入
• カメラを支える首の強化
• カメラ位置の変更

https://github.com/KeioTeamWolvez2023/wolvez2023-soft/assets/126460582/601afa76-352e-4b47-a7db-819bdc018fa1

https://github.com/KeioTeamWolvez2023/wolvez2023-soft/assets/126460582/a0ba7912-aa8c-4473-ba39-5f197aac3f64

https://github.com/KeioTeamWolvez2023/wolvez2023-soft/assets/126460582/85e3816f-eeb7-4129-bbd1-fb4649eb7e6c

https://github.com/KeioTeamWolvez2023/wolvez2023-soft/assets/126460582/28338c53-d4d7-46e0-954d-68def973fbbb

https://github.com/KeioTeamWolvez2023/wolvez2023-soft/assets/126460582/53a9b403-7543-4a4f-9ca4-24b308b91913