fkanehiro commented 10 years ago

From eisoku9...@gmail.com on October 09, 2013 16:33:26

IMUセンサを上下反対につけてしまっているロボットが直立状態（RPYの回転が0）の状態なのにも関わらず，rvizに表示されるロボットはロール方向に-π/2だけ回転してしまっています．上下反対というのは，ロール方向にπだけ回転している状態です．

IMUセンサの向きを直せばいいだけの話かもしれませんが，ロボットが-π傾くのではなく，-π/2傾いているのはカルマンフィルタがおかしいのかなと思いました．

IMUセンサが上下反対の場合，直立状態ではZ-Y-Xオイラー角がα=0，β=π，γ=0となるのが正解だと思いますが，現状，rvizを見る限り，α=0，β=-π/2，γ=0となっています．

hrpsys/build/hrpsys-base-source/rtc/KalmanFilter/KalmanFilter.cppに書かれているカルマンフィルタは， http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data と似たようなことをやっていて，入力が加速度センサの値から算出したRPYとなっていると思います．

加速度センサの値からRPY（Z-Y-Xオイラー角）を算出する部分で，加速度センサの値が sx=0 sy=0 sz=+g（通常ならばsz=-gになりますが，IMUが逆に取り付けられているのでプラスになると思います．）の場合に，現状のカルマンフィルタだとatan2(y,x)のxがルートをとった値になっているため必ず正になり，本来atan2(0,-1)となるべきところもatan2(0,1)になってしまっていると思います． http://en.wikipedia.org/wiki/Atan2 を参考にすると， atan2(0,-1)=π atan2(0, 1)=0 になるので，atan2(0,-1)を使っていれば α=0，β=π，γ=0 となるべきところが，atan2(0,1)を使っていることで， α=0，β=0，γ=0 になってしまっているため，入力値 : α=0，β=0，γ=0 観測値 : α=0，β=π，γ=0　（ジャイロセンサ）としてカルマンフィルタを通し，直立状態でもおかしな姿勢になるのかなと思いました．

プログラム上ではロールがa，ピッチがb，ヨーが0となっていることに注意しつつ， b = atan2( - sx / g, sqrt( sy/g * sy/g + sz/g * sz/g ) ); a = atan2( ( sy/g ) / sqrt( 1 - sx/g * sx/g), ( sz/g ) / sqrt( 1 - sx/g * sx/g) ); となっていたところを， a = atan2( - sy/g , - sz/g ); if ( - sz/g > 0) { b = atan2( ( sy/g ) , sqrt( sy/g * sy/g + sz/g * sz/g ) ); } else { b = atan2( ( sy/g ) , - sqrt( sy/g * sy/g + sz/g * sz/g ) ); } と変えてみましたが，これで合っているか不安ですが，いまから実機て試してみます．

ちなみに，加速度センサからRPYを求める式は http://www.alab.t.u-tokyo.ac.jp/~star/pdf/080603short.pdf の式(2)を参考にしていて， cosα，sinα，cosβ，sinβを求めるときには，『ロボットモーション』41ページによると，Z-Y-Xオイラー角は -π<α≦π cosβ≠0 であることと，上記のatan2のwikipediaによると -π/2 < atan2(y,x) < π/2 であることを用いました．

diffを添付しました．よろしくお願いします．

Attachment: kalman.diff

Original issue: http://code.google.com/p/hrpsys-base/issues/detail?id=142

fkanehiro commented 10 years ago

From eisoku9...@gmail.com on October 09, 2013 01:40:56

if ( - sz/g > 0) { b = atan2( ( sy/g ) , sqrt( sy/g * sy/g + sz/g * sz/g ) ); } else { b = atan2( ( sy/g ) , - sqrt( sy/g * sy/g + sz/g * sz/g ) ); } ではなくて， if ( - sz/g > 0) { b = atan2( ( sx/g ) , sqrt( sy/g * sy/g + sz/g * sz/g ) ); } else { b = atan2( ( sx/g ) , - sqrt( sy/g * sy/g + sz/g * sz/g ) ); } でした． diffも添付し直します．

Attachment: kalman_new.diff

fkanehiro commented 10 years ago

From eisoku9...@gmail.com on October 10, 2013 04:28:29

飛行ロボットをやっている人に聞いてみたところ，磁気センサをつけてYawを取らない限り，RPYでやるなら，裏表がひっくり返った場合は，別途KFの処理を書く + TFの向きを変えるということをしないといけないということでした．ということで，ソフトウェアで仮想的にIMUセンサの向きを正しい向きに変えたところ正常に動きました．

お騒がせしました．

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on October 12, 2013 01:59:40

モデル情報を使うためにKalmanFilterでロボットモデルを追加できるようにしてく準備が必要そうです．

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on October 12, 2013 02:04:23

モデル情報を使うためにKalmanFilterでロボットモデルを追加 r864 で追加しました．

fkanehiro commented 10 years ago

From eisoku9...@gmail.com on October 15, 2013 04:02:49

最初の３つの僕の書き込みは的外れでしたが， KFの中でIMUがどのような姿勢でついているかの情報を取得し，添付した図のように変換をかますことで，IMUがどんな姿勢で取り付けられていようと正しい結果を返してくれるようになると思います．

添付した図は，IMUをYaw方向に0°，Pitch方向に-45°，Roll方向に180°だけ傾けてつけてしまい，かつ，/BODYがPitch方向に30°傾いている場合の例です．

従来のKFでは添付図のB→Dの回転を出力していました（IMUが正しく付けれれていればこれで正しい）が， IMUが正しく付いていないときのことも考えると，添付図のA→Dを変換するのが正しそうです．

以下がdiffです（モデルからIMUの取り付け姿勢を取得する方法が分からなかったので，以下の行列Aが決め打ちになっています）．

ご確認よろしくお願いします．

Index: KalmanFilter.cpp

--- KalmanFilter.cpp (リビジョン 864) +++ KalmanFilter.cpp (作業コピー) @@ -204,6 +204,14 @@ // g = [ 0 0 -g]t // s = G g = [g sinb, -g cosb sina, -g cosb cosa]t double sx = m_acc.data.ax, sy = m_acc.data.ay, sz = m_acc.data.az;

Eigen::Matrix3d A;
A << 1.0, 0.0, 0.0,
0.0, -1.0, 0.0,
0.0, 0.0, -1.0;
Eigen::Vector3d s;
s << sx, sy, sz;
s = A * s;
sx = s(0); sy = s(1); sz = s(2); double g = sqrt(sx * sx + sy * sy + sz * sz); // atan2(y, x) = atan(y/x) double a, b; @@ -240,12 +248,32 @@ // kalman filter // x[0] = m_rpyRaw.data.r; // angle (from m/sec Acceleration3D, unstable, no drift ) // x[1] = m_rate.data.avx; // rate ( rad/sec, AngularVelocity, gyro, stable/drift )
Eigen::Vector3d av;
av << m_rate.data.avx, m_rate.data.avy, m_rate.data.avz;
av = A * av;
m_rate.data.avx = av(0); m_rate.data.avy = av(1); m_rate.data.avz = av(2); r_filter.update(m_rate.data.avx, m_rpyRaw.data.r); p_filter.update(m_rate.data.avy, m_rpyRaw.data.p); y_filter.update(m_rate.data.avz, m_rpyRaw.data.y);
m_rpy.data.r = r_filter.getx()[0];
m_rpy.data.p = p_filter.getx()[0];
m_rpy.data.y = y_filter.getx()[0];
double sina, cosa, sinb, cosb, sinc, cosc;
sina = sin(r_filter.getx()[0]);
cosa = cos(r_filter.getx()[0]);
sinb = sin(p_filter.getx()[0]);
cosb = cos(p_filter.getx()[0]);
sinc = sin(y_filter.getx()[0]);
cosc = cos(y_filter.getx()[0]);
Eigen::Matrix3d M, MA;
M << cosb_cosc, sina_sinb_cosc - cosa_sinc, cosa_sinb_cosc+sina*sinc,
cosb_sinc, sina_sinb_sinc + cosa_cosc, cosa_sinb_sinc-sina*cosc,
-sinb, sina_cosb, cosa_cosb;
MA = M * A; +
// m_rpy.data.r = r_filter.getx()[0];
// m_rpy.data.p = p_filter.getx()[0];
// m_rpy.data.y = y_filter.getx()[0];
m_rpy.data.y = atan2(MA(1,0), MA(0, 0));
m_rpy.data.p = atan2(- MA(2,0), MA(0, 0)_cos(m_rpy.data.y) + MA(1,0)_sin(m_rpy.data.y));
m_rpy.data.r = atan2(MA(0,2)_sin(m_rpy.data.y) - MA(1,2)_cos(m_rpy.data.y), - MA(0, 1)_sin(m_rpy.data.y) + MA(1,1)_cos(m_rpy.data.y));

Attachment: KalmanFilter.jpg

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on October 15, 2013 04:20:43

モデルからリンクの座標を知るには， m_robot->link(m_sensors[sensor_name].parent_link_name)->R としてできると思います．試してみて下さい．

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on October 15, 2013 21:03:43

m_robot->link(m_sensors[sensor_name].parent_link_name)->R をすればいいとのことでしたが， m_robot->link(m_sensors[RATE_GYRO].parent_link_name)->R; としたところ，m_sensorsがありませんと言われました．

linkの引数は名前のstringなので("RATE_GYRO")かなにか自分のロボットに合わせて書いてみて，まずはテストするのがいいともいます．

次は，ロボット毎にセンサの名前が違うだろうからそれをどうやって与えたらいいかを考えるわけですが， /rtc$ less VirtualCamera/VirtualCamera.cpp をみてみると， // look for camera if (it->first == bodyName){ OpenHRP::LinkInfoSequence_var lis = binfo->links(); for (unsigned int i=0; ilength(); i++){ const OpenHRP::SensorInfoSequence& sensors = lis[i].sensors; for (unsigned int j=0; j<sensors.length(); j++){ const OpenHRP::SensorInfo& si = sensors[j]; std::string type(si.type); std::string name(si.name); if (type == "Vision" && name == cameraName){ w = si.specValues[4]; h = si.specValues[5]; break; } } } if (!w || !h){ std::cout << "invalid image size:" << w << "x" << h << std::endl; return RTC::RTC_ERROR; } } みたいにあっているので，こういう感じでGyroの名前を調べるか，あるいは，２台以上ジャイロがあるロボットが出てきそうなら， less VirtualForceSensor/VirtualForceSensor.cpp の // virtual_force_sensor: , , , 0, 0, 0, 0, 0, 1, 0 とかいてあるあたりをみて.confに書いてある名前をかければいいかとおもいますが，まずは，「モデルからリンクの座標を知る」というのが出来るか試すのがいいともいます．

fkanehiro commented 10 years ago

From eisoku9...@gmail.com on October 17, 2013 02:31:10

僕の使っているロボットのwrlファイルを見ると，ジャイロセンサはWaist Joint のchildrenの Body Segment のchildrenの gyrometer という名前だったので， AbsoluteForceSensor.cppを参考にして，

今まで， A << 1.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, -1.0; としていたところを hrp::RateGyroSensor* sensor = m_robot->sensorhrp::RateGyroSensor("gyrometer"); A = sensor->link->R * sensor->localR;

と変えることで，IMUセンサがBodyに対する姿勢を表す回転行列Aを直打ちでなく求めることができたので，

まずは，「モデルからリンクの座標を知る」というのが出来るか試すのがいいともいます．

はクリアしたと思います．

ジャイロセンサの名前がgyrometerでないロボットへの対応もやりたいのですが，

次は，ロボット毎にセンサの名前が違うだろうからそれをどうやって与えたらいいかを考えるわけですが， /rtc$ less VirtualCamera/VirtualCamera.cpp をみてみると， // look for camera if (it->first == bodyName){ OpenHRP::LinkInfoSequence_var lis = binfo->links(); for (unsigned int i=0; ilength(); i++){ const OpenHRP::SensorInfoSequence& sensors = lis[i].sensors; for (unsigned int j=0; j<sensors.length(); j++){ const OpenHRP::SensorInfo& si = sensors[j]; std::string type(si.type); std::string name(si.name); if (type == "Vision" && name == cameraName){ w = si.specValues[4]; h = si.specValues[5]; break; } } } if (!w || !h){ std::cout << "invalid image size:" << w << "x" << h << std::endl; return RTC::RTC_ERROR; } } みたいにあっているので，こういう感じでGyroの名前を調べるか，あるいは，２台以上ジャイロがあるロボットが出てきそうなら，VirtualCamera.cppを見ても分かりませんでした．

が良く分かりませんでした．どうすれば良いのでしょうか？

fkanehiro commented 10 years ago

From eisoku9...@gmail.com on November 07, 2013 03:09:21

次は，ロボット毎にセンサの名前が違うだろうからそれをどうやって与えたらいいかを考えるわけですが，センサの名前を直打ちしていた部分を，AbsoluteForceSensor/AbsoluteForceSensor.cppを参考にしてセンサの名前を取得するように変更しました．また，オイラー角から回転行列を求めるところなどを自分で計算していた部分を，ライブラリを使うように変更しました．

diffは以下になります．よろしくお願いします．

Index: KalmanFilter.h

--- KalmanFilter.h (リビジョン 889) +++ KalmanFilter.h (作業コピー) @@ -209,6 +209,7 @@ double m_dt; KFilter r_filter, p_filter, y_filter; hrp::BodyPtr m_robot;

hrp::Matrix33 m_sensorR; unsigned int m_debugLevel; int dummy; };

Index: KalmanFilter.cpp

--- KalmanFilter.cpp (リビジョン 889) +++ KalmanFilter.cpp (作業コピー) @@ -12,6 +12,8 @@

include <rtm/CorbaNaming.h>

include <hrpModel/ModelLoaderUtil.h>

include

+#include <hrpModel/Link.h> +#include <hrpModel/Sensor.h>

// Module specification // @@ -135,6 +137,10 @@ m_rpy.data.r = 0; m_rpy.data.p = 0; m_rpy.data.y = 0; +
hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
m_sensorR = sensor->link->R * sensor->localR; + return RTC::RTC_OK; }

@@ -204,6 +210,10 @@ // g = [ 0 0 -g]t // s = G g = [g sinb, -g cosb sina, -g cosb cosa]t double sx = m_acc.data.ax, sy = m_acc.data.ay, sz = m_acc.data.az; +

// hrp::RateGyroSensor* sensor = m_robot->sensorhrp::RateGyroSensor("gyrometer");
hrp::Vector3 s = m_sensorR * hrp::Vector3(sx, sy, sz);
sx = s(0); sy = s(1); sz = s(2); // transform to imaginary acc data double g = sqrt(sx * sx + sy * sy + sz * sz); // atan2(y, x) = atan(y/x) double a, b; @@ -240,12 +250,27 @@ // kalman filter // x[0] = m_rpyRaw.data.r; // angle (from m/sec Acceleration3D, unstable, no drift ) // x[1] = m_rate.data.avx; // rate ( rad/sec, AngularVelocity, gyro, stable/drift )
hrp::Vector3 av = m_sensorR * hrp::Vector3(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz);
m_rate.data.avx = av(0); m_rate.data.avy = av(1); m_rate.data.avz = av(2); // transform to imaginary rate data
// use kalman filter with imaginary data r_filter.update(m_rate.data.avx, m_rpyRaw.data.r); p_filter.update(m_rate.data.avy, m_rpyRaw.data.p); y_filter.update(m_rate.data.avz, m_rpyRaw.data.y);
m_rpy.data.r = r_filter.getx()[0];
m_rpy.data.p = p_filter.getx()[0];
m_rpy.data.y = y_filter.getx()[0]; +
// m_rpy.data.r = r_filter.getx()[0];
// m_rpy.data.p = p_filter.getx()[0];
// m_rpy.data.y = y_filter.getx()[0]; +
Eigen::AngleAxis aaZ(y_filter.getx()[0], Eigen::Vector3d::UnitZ());
Eigen::AngleAxis aaY(p_filter.getx()[0], Eigen::Vector3d::UnitY());
Eigen::AngleAxis aaX(r_filter.getx()[0], Eigen::Vector3d::UnitX());
Eigen::Quaternion q = aaZ * aaY * aaX;
hrp::Matrix33 imaginaryRotationMatrix = q.toRotationMatrix();
hrp::Matrix33 realRotationMatrix = imaginaryRotationMatrix * m_sensorR; // inverse transform to real data
hrp::Vector3 euler = realRotationMatrix.eulerAngles(2,1,0);
m_rpy.data.y = euler(0);
m_rpy.data.p = euler(1);
m_rpy.data.r = euler(2);
if 0

std::cout << m_acc.tm.sec + m_acc.tm.nsec/1000000000.0 << " " << m_rpyRaw.data.r << " "

Attachment: KalmanFilter.cpp KalmanFilter.h

fkanehiro commented 10 years ago

From kei.ok...@gmail.com on November 14, 2013 19:49:07

r897 で対応しました．

Status: Fixed

fkanehiro / hrpsys-base

KalmanFilterがうまく動いてない #142

Index: KalmanFilter.cpp

Index: KalmanFilter.h

Index: KalmanFilter.cpp

include <rtm/CorbaNaming.h>

include <hrpModel/ModelLoaderUtil.h>

include

if 0