埋め込みアルゴリズム空間ベクトル角度式とその応用

[[350122]]

一部のデバイスは、正しく動作するために適切な方向に設置する必要があります。たとえば、GPS アンテナは、最良の信号を確保するために空を向いている必要があり、温度と湿度のセンサー監視ポートは、時間内に応答するために測定対象に向いている必要があります。ソフトウェアの要件は、取り付け角度が異常な場合にユーザーに位置を変更するように通知することです。では、デバイスはどのようにして現在の位置を感知するのでしょうか? これには、アルゴリズムによって補完された加速度計のハードウェアサポートが必要です。

1. 重力加速度

物理学の常識によれば、地上で静止している物体は 1g の重力加速度を受け、方向は垂直下向きです。

傾斜角度が異なるため、1g 加速度はベクトルによって xyz 軸に分解されます。

acc_x=1g.sinθ.cosϕ
acc_y=-1g.sinθ.sinϕ
acc_z=1g.cosϕ

記号 . は乗算を表します。加速度センサーの xyz 3 軸データの読み取りの詳細については、この記事の範囲外です。

センサーのビット精度や範囲が異なるため、同じ1gでもレジスタ内の値は異なります。以下の説明を統一するため、値255が1gの加速度に対応すると仮定します。物体が水平に静止しているときは理想的な加速度値は（0, 0, 255）、水平に置かれているときは（0, 0, -255）です。この値の比例的な縮小または拡大は角度の判定に影響しません。

2. 空間ベクトル角度

正しい設置方法で期待される 3 軸データが (x0, y0, z0) であり、実際の 3 軸データが (x1, y1, z1) であると仮定します。現在の偏差角度はどうやって求めるのでしょうか? 2 つの空間ベクトルの座標 a=(x1, y1, z1) と b=(x2, y2, z2) が与えられている場合、2 つのベクトル間の角度の余弦 cosθ の式は次のようになります。

実際のアプリケーションでは、フリップ角度が 181 度の場合、179 度として処理されます。この記事では、0 度から 180 度までのアプリケーションのみを検討します。180 度を超えるアプリケーションでは、反転の追加計算が必要になります。

空間ベクトル角度の余弦と逆余弦に基づいて 0 度から 180 度までの角度を取得し、機器が正しい角度で設置されているかどうかを判断できます。

3. コードの実装

#include "math.h"  
 #include "stdio.h"   
 
 #PI 3.1415926 を定義します
 
 typedef構造体
{
    符号なしショートx;
    符号なしショート y;
    符号なしショートz;
 }センサーデータ構造体; 
 
静的センサーデータ構造体参照={0,0,255};
静的センサーデータ構造体テスト={0,180,180}; 
 
 //現在のベクトルと参照ベクトルの間の角度を計算します
float get_angle(sensor_data_structデータ)
 {
浮動小数点コサイン;
フロート 温度、角度;
    コサイン=(データ.x*ref.x+データ.y*ref.y+データ.z*ref.z)/ \
            ((sqrt(データ.x*データ.x+データ.y*データ.y+データ.z*データ.z))*\
             (sqrt(ref.x*ref.x+ref.y*ref.y+ref.z*ref.z))); 
 
 temp = acos(コサイン);
     角度=(温度*180)/円周率;
戻り角度;
 } 
 
 int main( int argc, char *argv[])
 {
フロート角度;
    printf( "参照ベクトル (%d,%d,%d)\r\n" ,ref.x,ref.y,ref.z);
    printf( "テストベクトル (%d,%d,%d)\r\n" ,test.x,test.y,test.z);
    角度=get_angle(テスト);
    printf( "角度 = %f'\r\n" ,角度);
 0を返します。
 }

参照ベクトル (0,0,255)
テストベクトル (0,180,180)
角度 = 45.000004'

4. 最適化と改善

角度を解くために使用される三角関数は、一部のマイクロコントローラではサポートされていない可能性があります。角度の精度のために、整数を使用できます。この条件に基づいて、1 度、2 度、3 度、180 度のステップで cosθ の配列テーブルを作成できます。配列テーブルは、次のコードに従って生成できます。

 void create_table(void)
 {
フロートi;
 for (i=0;i<180;i++)//iのステップ値によって精度が決まります
    {
        ((符号なしchar )i%9==0)の場合
        {
            printf( "\r\n" );
        }
        printf( "%f," ,cos(i*PI/180)); //角度をラジアンに変換して渡す
    }
 }

コードに従って配列テーブルを生成した後、コサインテーブルを検索すると、配列の添え字が角度になります。

静的定数フロートcos_table[180]={
    1.000000,0.999848,0.999391,0.998630,0.997564,0.996195,0.994522,0.992546,0.990268,
    0.987688,0.984808,0.981627,0.978148,0.974370,0.970296,0.965926,0.961262,0.956305,
    0.951057,0.945519,0.939693,0.933580,0.927184,0.920505,0.913545,0.906308,0.898794,
    0.891007,0.882948,0.874620,0.866025,0.857167,0.848048,0.838671,0.829038,0.819152,
    0.809017,0.798636,0.788011,0.777146,0.766044,0.754710,0.743145,0.731354,0.719340,
    0.707107,0.694658,0.681998,0.669131,0.656059,0.642788,0.629320,0.615661,0.601815,
    0.587785,0.573576,0.559193,0.544639,0.529919,0.515038,0.500000,0.484810,0.469472,
    0.453991,0.438371,0.422618,0.406737,0.390731,0.374607,0.358368,0.342020,0.325568,
    0.309017,0.292372,0.275637,0.258819,0.241922,0.224951,0.207912,0.190809,0.173648,
    0.156434,0.139173,0.121869,0.104528,0.087156,0.069756,0.052336,0.034900,0.017452,
    0.000000、-0.017452、-0.034899、-0.052336、-0.069756、-0.087156、-0.104528、-0.121869、-0.139173、
    -0.156434、-0.173648、-0.190809、-0.207912、-0.224951、-0.241922、-0.258819、-0.275637、-0.292372、
    -0.309017、-0.325568、-0.342020、-0.358368、-0.374607、-0.390731、-0.406737、-0.422618、-0.438371、
    -0.453990、-0.469472、-0.484810、-0.500000、-0.515038、-0.529919、-0.544639、-0.559193、-0.573576、
    -0.587785、-0.601815、-0.615661、-0.629320、-0.642788、-0.656059、-0.669131、-0.681998、-0.694658、
    -0.707107、-0.719340、-0.731354、-0.743145、-0.754710、-0.766044、-0.777146、-0.788011、-0.798635、
    -0.809017、-0.819152、-0.829038、-0.838671、-0.848048、-0.857167、-0.866025、-0.874620、-0.882948、
    -0.891007、-0.898794、-0.906308、-0.913545、-0.920505、-0.927184、-0.933580、-0.939693、-0.945519、
    -0.951057、-0.956305、-0.961262、-0.965926、-0.970296、-0.974370、-0.978148、-0.981627、-0.984808、
    -0.987688、-0.990268、-0.992546、-0.994522、-0.996195、-0.997564、-0.998630、-0.999391、-0.999848、
 }; 
 
符号なしショート get_angle(sensor_data_struct データ)
 {
浮動小数点コサイン;
    符号なしショート i; 
 
    コサイン=(データ.x*ref.x+データ.y*ref.y+データ.z*ref.z)/ \
            ((sqrt(データ.x*データ.x+データ.y*データ.y+データ.z*データ.z))*\
             (sqrt(ref.x*ref.x+ref.y*ref.y+ref.z*ref.z))); 
 
 (i=0;i<180;i++)の場合
    {
        if(cos_table[i]<cosine) //テーブルを参照
        {
 iを返します。
        }
    }
 180 を返します;//エラー
} 
 
 int main( int argc, char *argv[])
 {
    unsigned short angle; //プラスチックに変更
    printf( "参照ベクトル (%d,%d,%d)\r\n" ,ref.x,ref.y,ref.z);
    printf( "テストベクトル (%d,%d,%d)\r\n" ,test.x,test.y,test.z); 
 
    角度=get_angle(テスト); 
 
    printf( "角度 = %d'\r\n" ,角度);
 0を返します。
 }