2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

RCJのエンジェルラインの計算

2
Last updated at Posted at 2025-12-23

この記事は、産技品川 Advent Calendar 2025 24日目の記事です!

24日なのは、今回のノード大会で使った機体のラインセンサーの数が24個だからです:sunglasses:
ちなみに、昨日担当だった伊勢えびは同じチームの回路班です。回路がないと制御できないので、いつもありがとう...

エンジェルライン(エンジェルリング?)とは

このように、ロボの底面に円形に配置されている照度センサーのことです。表面実装なので少しわかりにくいですが、ここでは、赤色LEDの間にあるものが照度センサーです。
IMG_5331.jpeg
従来の十字の配置だと、ラインがどこにあるか(4方向)しか取れず、自由度が少なかったり、うまく見分けられなかったりします

本題

では、ここからエンジェルラインの計算について話します。
今回紹介する方法は2つで、ノード大会では2つ目のほうを使いました。

方法1

センサーごとのベクトルを事前に計算しておき、読んだセンサーのベクトルを合成する

例1

まず、青い破線が伸びているセンサーがラインを踏んでいるセンサーとします。
スクリーンショット 2025-12-15 11.39.57.png

あらかじめ計算していたベクトルを可視化するとこうなります。(値がマイナスなものは赤い線にしています)
スクリーンショット 2025-12-15 11.45.34.png

これらのベクトルを合成して読んだセンサーの数で割ると、ラインの方向と距離をとることができます。ここでは、緑が合成されたベクトルです。
スクリーンショット 2025-12-15 18.01.55.png

例2

うまく対角のラインを踏むと合成されたベクトルが0になります。
スクリーンショット 2025-12-15 17.47.38.png

メリット

  • 計算量がO(センサーの数)で、早い
  • わかりやすい

デメリット

  • ロボットの中心がラインに近いとき不安定
詳しく

まず例2を思い出してほしく、ここから追加で$\frac{13}{12}\pi$の位置にあるラインを踏んだとします。
そうするとこのような感じで、ラインの実際の位置は例2とほとんど変わっていないのに、合成したベクトルがずれてしまいます。
(これはゴールキーパーでちょうどラインの上に居たい時などで特に困ると思います)
スクリーンショット 2025-12-15 18.04.19.png

たぶんうまくやれば見分けられると思いますが、
これ↓と合成したベクトルがほぼ同じになってしまうの結構致命的じゃないですか?

おまけ

ここの説明で使ってたdesmosです。配布用に作ってなかったので使いにくいですが一応...
$d_1, d_2, d_3, d_4$の値を反応したセンサーのある角度にするといい感じにベクトルを足してくれます。一応うまく複製すれば増やせます(がんばってください)

方法2

急ですが、センサーのペア(A, B)だけを読んでいるときの計算について話します。

計算

今からする計算では、ライン間の角度がよく出てきます。これをthetaとします。
A個目のセンサーの角度をdegAとすると、これは$A\times\frac{360}{センサーの数}$になります。
同じようにdegBも求め、degAとdegBの差の絶対値がthetaになります。
スクリーンショット 2025-12-21 0.35.55.jpg
この例では、
$degA = 4\times\frac{360}{24} = 60$
$degB = 9\times\frac{360}{24} = 135$
$theta = |60-135| = 75$
です。

また、特殊な例としてthetaが180°を超える(0°をまたぐ)ときは360-thetaの値をthetaとしています。
スクリーンショット 2025-12-21 13.53.03 4.jpg

ラインの方向

緑色の辺がどの向きにあるかを求めます。
スクリーンショット 2025-12-21 0.35.55 5.jpg
基本的には (degA, degBの小さい方)+theta/2 なのですが、気をつけないといけない場合があり、それがthetaがちょうど180度の場合と、0度をまたぐ場合です。

180度の場合

例えばセンサーが(0度, 180度)だったとき、90度の方向にあるとしても270度の方向にあるとしても自然で、2つ存在してしまいます。ややこしいです...
なので最終的な計算からは除外するようにしています。

0度をまたぐ場合

こういう場合があったとします。水色で示した方向(0°)になってほしいですよね?
スクリーンショット 2025-12-21 13.53.03.jpg

しかしなりません。角度を書いたものがこうなっていて、
スクリーンショット 2025-12-21 13.53.03 5.jpg

単純に(degA, degBの小さい方)+theta/2だと、
degA+theta/2 = 30°+60°/2 = 60°
と60°になってしまい、少しずれます。(黄色の矢印)
スクリーンショット 2025-12-21 13.53.03 6.jpg

対策として、今回は0°をまたぐ(dega, degbの差が180以上)ときはdegA,Bの大きい方にtheta/2を加算するということにしました。こうすると大丈夫です。

一応、コードで使う際は360で割ったあまりをとっています。
degA: 45°, degB: 345°(, theta: 60°) の場合、方向は375°となります。
1つだけならそれでもいいのですが、これだと最後に平均を取るときにだめです。

例 375°, 20°があったとき、平均をとると197.5°です。だいぶずれる...

ラインの傾き

この緑色の辺の傾きを求めます。
スクリーンショット 2025-12-21 0.35.55 4.jpg
ラインの方向のベクトルと直行するので、ラインの方向+90度がそれのはずです。
...が、theta=180のとき方向がもとまらないからなのか、実装は90-(dega, degb)/2になっていました
確かめたら想定通りに動くのでいいのですが、どうこれを求めたか忘れました。当時のメモが見つかったら追記します...

戻ります。
ただ、これだとこのオレンジの辺と紫の辺は同じ傾きであってほしいですが、なりません。
スクリーンショット 2025-12-21 14.57.35.jpg

なので180で割ったあまりを取ります。
そうすると330° % 180 == 150° % 180となり、同じになります。

ラインの距離

この水色の辺が距離です。
スクリーンショット 2025-12-21 0.35.55 2.jpg
青破線の長さは同じなので、原点とセンサー2つに囲まれた三角形は二等辺三角形です。さらに、距離の辺とセンサー同士を結ぶ辺は直行しているので、赤で記した角度はちょうどtheta/2になります。
スクリーンショット 2025-12-21 0.35.55 3.jpg
水色の辺、青破線からなる三角形は直角三角形なので、水色の辺の長さはcos(theta/2)になることがわかります。


急に計算の話してすみません。どういう構成にすればいいかかなり悩みました
では、ここからライン検知あたりの話をします

イメージ

ここでも上の例1と同じように青い破線が伸びているセンサーがラインを踏んでいるセンサーとします。
前回はセンサーごとのベクトルを...としましたが、この方法では反応しているセンサーの組み合わせからラインを見いだして計算しています。

例えば、これらのセンサーが反応しているとします。
スクリーンショット 2025-12-19 11.02.42.png

この3つが反応するようなラインのかかり方を考えると、こんな感じ↓になっていそうですよね?
2025-12-19 11.05の画像.jpeg
(水色のものがラインです)

実際にこうなっていることを検知することはできないので、反応しているセンサーのペアを考えて、ラインをこのように読んでいることにします。
スクリーンショット 2025-12-19 11.12.16.png
このとき、隣同士のセンサーは反応していても無視します。


ここから、反応したラインセンサーのペア全てに対して、最初に話した

  • 角度
  • 方向
  • 傾き

などの計算をします。

すべてのペアに対して計算が終わったら平均をとります。
thetaが180のときはラインの方向を使わないようにしたので、そのあたりはうまくやる必要があります。

コード

コードです。
readed[]には予め読んでおいたセンサーの結果が格納されています。readの過去形はread
内側のfor文のjが1ではなく2からなのは、隣接したセンサーのペアをラインとしてカウントしないためです。

    for(int i=0;i<24;i++){
      if(!readed[i]) continue;
      int a = i;
      int dega = a*(360/24);

      for(int j=2;j<22;j++){
        int b = (a+j)%24;
        if(!readed[b]) continue;
        int degb = b*(360/24);
        linecount++;

        // ここの時点で、(a,b)が反応しているセンサーの組み合わせで、
        // このペアを通るラインがあるとする
        
        int theta = abs(dega-degb);
        if(180<theta) theta = 360-theta;

        lineslope += (90-(dega+degb)/2 +360)%180;

        linedist += fabs(cos((theta/2)*DEG2RAD));

        if(theta!=180){
          linecountex++;
          int mi = min(dega,degb);
          if(180<abs(dega-degb)){
        	mi = max(dega,degb);
          }
          linedir += (mi+theta/2)%360;
        }
      }
    }

    if(linecount!=0){ // 平均を取る
      lineslope /= linecount;
      linedist /= linecount;
      if(linecountex!=0){ // すべてthetaが180だった場合にゼロ除算をしないため
        linedir /= linecountex;
      } else {
        linedir = 0;
      }
    }

    // 送信、すきな処理など

メリット

  • ロボットの中心がラインに近くても不安定にならない
詳しく

(方法1のデメリットの詳細を先に読むといいかもしれないです)
方法1では、$\frac{2\pi}{12},\frac{13\pi}{12},\frac{14\pi}{12}$が反応しているとき、$\frac{2\pi}{12}$側が1つしかないのに対し、$\frac{13\pi}{12}$側が2つあるので、$\frac{13\pi}{12}$側が2倍重くなってしまっていました。
スクリーンショット 2025-12-15 18.04.19.png

方法2だと、隣接するセンサーのペア(ここでは$(\frac{13\pi}{12},\frac{14\pi}{12})$のペア)を無視しているので、偏っていてもそちらにひっぱられることは起きません。
スクリーンショット 2025-12-22 13.28.36.png

デメリット

  • 計算量がO(センサーの数^2)なので遅い
    (センサー数は多くても30とかだと思うので、あまり影響は大きくないかも?具体的な計測はできていないです...)
  • マイコンによっては、浮動小数点の限界を超えるかもしれない
    かなりレアケースだとは思いますが、floatが16bitとかだと溢れるかもしれないです
  • ややこしい

あとがき

長かったですがありがとうございました!
表現がわかりにくかったり間違っていたりしたら教えてください...
(特に2つ目は考えたものなので、ぜんぜん最適化できるかもしれません)
では、名古屋で会いましょう〜〜〜:sob:

2
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
2
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?