ロボティクスにおける角度情報の取り扱い方法

確率ロボティクス (プレミアムブックス版)

posted with カエレバ

Sebastian Thrun,Wolfram Burgard,Dieter Fox マイナビ出版 2016-09-21

Amazonで探す

楽天市場で探す

Yahooショッピングで探す

はじめに

ロボティクスにおいて角度情報の表現方法や取り扱い方法は重要です。

ロボットの姿勢(オイラー角におけるロールやピッチ、ヨー）や、

センサの観測値には、必ず角度の情報が含まれますが、

この情報の表現方法や取り扱い方法に気をつけないと、

ソフトウェアにバグが混入し、

最悪の場合、ロボットが暴走します(笑)

この記事では自分のこれまでの経験における

角度情報の取り扱いの注意点をまとめておきたいと思います。

角度の単位

ロボティクスのソフトで主に使用する角度の単位は

radianだと思います。

ラジアン - Wikipedia

なぜなら、三角関数を利用したい時に、

直接角度情報を利用できるからです。

一方、ラジアンは人間にとってはあまり直感的な数値ではないので、

GUIに表示する角度の値には度数(Degree)を使うことが多いです。

また、センサから取得する角度データは

それぞれのセンサの通信仕様によって異なります。

一般的にセンサのデータは符号あり整数型で送信され、

それらをある定数で掛け算することで、

DegreeやRadianに変換することが多いです。

例えば、符号あり2byte整数(sint16)で角度情報が送信される場合、

[-180,180]をsint16の最小値最大値[-32768,32767]で

割った定数0.0055を受信データと掛け算することで

度数の角度情報を復元できます。

以上のように角度の単位は様々であるため、注意しましょう。

角度範囲

一般的にロボティクスで利用する角度情報の範囲は、

[-180度,180度]([-π,π])であるべきです。

また、角度の符号に関しては、

冒頭の絵のように、各座標軸を+から-の方向に見た場合、

反時計周りの方向を正、時計回りの方向を負とするのが一般的のようです。

(ただセンサによっては逆方向の場合もあります。)

角度は座標値などと異なり、循環型の値であるため、

角度の値そのものが異なっていても、

幾何学的には同じ角度を表していることがあります。

(例 +90度と-270度は同じ方向を表している)

単純に三角関数を使用する場合は問題ないことも多いですが、

以前紹介したカルマンフイルタによる位置計測を実施した場合、

拡張カルマンフィルタを使用した自己位置推定MATLABサンプルプログラム - MY ENIGMA

運動モデルによる推定値と観測値による推定値の差(イノベーション）

を使用して誤差を補正するため、角度の範囲が守られていない場合、

同じ方向を向いているのに、イノベーションの差が大きくなり

計算が発散してしまうことがあります。

(逆に角度範囲を守っても、-179度と180度は数値的に不連続なので

　その部分が悪さをする場合もあるのですが。。。)

従って、ある座標値の角度を

[-180度,180度]の角度範囲で計算したい場合、

使用する三角関数をきちんと選ぶ必要があります。

C言語の場合は、atan2を使う必要があります。

C言語関数辞典 - atan2, atan2f, atan2l

だいぶ慣れてきた - trinoの走り書き

それ以外に、カルマンフイルタの運動モデルのように、

角度を角速度で積算して計算する場合、

逐次的に角度範囲を補正する必要があります。

逐次的に角度範囲を計算する方法は

主に下記の2つの方法があります。

角度補正方法1

1つ目の補正方法は、単純に角度の値が範囲を超えた場合、

角度範囲内に戻すために360度を足したり引いたりする方法です。

下記はC,C++における角度補正用のプログラムです。

この関数に角度値(radian)を入れると、

角度範囲内の場合はそのままの値が返り値となり、

範囲外の時は補正されて出力されます。

float PI2PI(float angle){
  while(angle>=M_PI){
    angle-=2.0*M_PI;
  }
  while(angle<=-M_PI){
    angle+=2.0*M_PI;
  }
  return angle;
}

角度補正方法2

二つ目の方法は、三角関数を利用する方法です。

一度sin,cosで距離情報に変換して、

atan2で角度情報を計算することにより、

角度範囲を補正することができます。

下記のプログラムは角度範囲を補正するMATLABプログラムです。

function angle=PI2PI(angle)
    angle=atan2(sin(angle),cos(angle));
end

角度の平均の計算方法

恥ずかしい話なのですが、複数の角度の情報の平均を取りたい時って、角度の不連続点があるので、色々面倒なんですが、ベクトルを考えると当たり前なのですが、こんなに簡単に計算できる方法があるのですね。。知りませんでした。。 Averages/Mean angle - Rosetta Code https://t.co/zpjaGUcssT pic.twitter.com/QJGCTxJB0l
— Atsushi Sakai (@Atsushi_twi) October 19, 2019

角度の差の計算方法

ちなみに、２つの角度の差は、同じ考えでこれで計算できるのか。 pic.twitter.com/OQlWXYgrCf
— Atsushi Sakai (@Atsushi_twi) October 20, 2019

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com

MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone