加速度計のデータに対して重力補正を行おうとしています。6 DOF(加速度計、ジャイロスコープ)を備えた加速度計が与えられた場合、加速度計の読み取りにおける重力の影響を除去/補正したいと思います(加速度計は自由に回転できます)。
以下は、生のセンサー値をstruct
呼び出された場所に保存する方法ですsample
。
uint8_t *p=data; // p is a pointer to the sensor data
int i;
for(i=0; i<4; i++) // quaternion
{
sample.quaternion[i]=((float)get_int32(p))/(1<<29);
len+=snprintf(s+len, sizeof(line)-len, "\t%9.6f", sample.quaternion[i]);
p+=4;
}
for(i=0; i<3; i++) // euler213_degrees
{
sample.euler213_degrees[i]=get_int16(p);
len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler213_degrees[i]);
p+=2;
}
for(i=0; i<3; i++) // euler123_degrees
{
sample.euler123_degrees[i]=get_int16(p);
len+=snprintf(s+len, sizeof(line)-len, "\t%d", sample.euler123_degrees[i]);
p+=2;
}
for(i=0; i<3; i++) // acceleration_g
{
sample.acceleration_g[i]=(2.0*get_int16(p))/(1<<15);
len+=snprintf(s+len, sizeof(line)-len, "\t%6.3f", sample.acceleration_g[i]);
p+=2;
}
for(i=0; i<3; i++) // gyroscope_dps
{
sample.gyroscope_dps[i]=(2000.0*get_int16(p))/(1<<15);
len+=snprintf(s+len, sizeof(line)-len, "\t%6.1f", sample.gyroscope_dps[i]);
p+=2;
}
重力補償された加速度計データを取得する方法を教えていただけますか?
IMU(6 DOFデバイス)によって計算されるクォータニオンは、基本的に、ローカルの地球参照フレームに対するデバイスの姿勢(3D方向)です。そのため、加速度測定値をローカルボディリファレンスフレーム(IMUによって直接測定された値)からローカルアースリファレンスフレーム(地面に接するxy平面を持つ座標系)に回転させるために使用できます。これを行うには、クォータニオン乗算を使用できます
v '= q。v。q *
ここで、qはクォータニオン、vは加速度ベクトルです(詳細については、クォータニオンベクトルの乗算を調べてください*)。重力はローカル地球参照フレームのベクトルu =(0,0、g)であることがわかっているため(IMU測定値もm / s ^ 2であると仮定すると、g≈9.81m/ s ^ 2)、次に、このベクトルをv 'から差し引くことができます
v '' = v'-u
v ''は、重力で補正/削除されたベクトルです。ただし、これはIMUのローカルボディ参照フレームにはありません(ローカル地球参照フレームに回転したことを思い出してください)。したがって、元に戻すには、逆四元数を掛けます。
v '' '= q-1。v。q-1 *
v '' 'は、元の加速度測定値から重力を引いたものになります。結果が正しいことを確認するために、デバイスが静止している間に測定を行うことができます。デバイスがどのように回転しても、加速度の測定値は(0,0,0)に近いはずです。
IMUがクォータニオン測定を提供しなかった場合は、加速度計とジャイロスコープの読み取り値の融合によって計算された可能性があります(これはIMUがとにかく行っていることです)。これに興味がある場合は、姿勢推定のためにMadgwick、Mahony、またはExtended KalmanFilterアルゴリズムを調べることをお勧めします。
*役立つリソース
クォータニオン乗算:https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
クォータニオンを使用してベクトルを回転させる(特に、コードを見たい場合は3番目の回答):https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by- a-unit-quaternion
この記事はインターネットから収集されたものであり、転載の際にはソースを示してください。
侵害の場合は、連絡してください[email protected]
コメントを追加