[[350122]]

有些設備正常工作時需按合適的方位安裝，比如GPS天線必須朝向天空才能保證信號最佳，溫濕度傳感器監測口必須朝向被測目標才能及時響應。軟件需求是在安裝角度異常時提醒用戶改變位置。那設備如何感知當前方位呢?需要一顆加速度傳感器硬件支持，輔以算法實現。

1、重力加速度

根據物理常識，地面上任何物體靜止時都受到1g的重力加速度，且方向是豎直向下。

因為傾斜角的不同，1g的加速度按向量分解到xyz三軸：

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

符號.代表相乘，讀取加速度傳感器的xyz三軸數據的細節，不是本文考慮的范疇。

因為傳感器的位數精度和量程不同，同樣的1g，讀寄存器的數值不同，為統一后文描述，假設數值255對應1g的加速度。物體水平靜止時加速度值理想情況是(0，0，255)，反向水平放置是(0，0，-255)。這個數值等比例的縮小或放大，不影響角度的判斷。

2、空間向量夾角

假設期望的正確安裝方式下三軸是(x0,y0,z0)，實際三軸數據是(x1,y1,z1)。那如何得出當前偏差的角度呢?已知兩空間向量的坐標為a=(x1,y1,z1)，b=(x2,y2,z2)，則兩向量的夾角余弦cosθ公式為：

在實際應用中，翻轉角度為181度時，按179度處理。本文只考慮0-180度的應用，180度以上的需要額外再計算翻轉。

根據空間向量夾角余弦，再反余弦得出在0-180度的角度，即可判斷設備安裝角度是否正確。

3、代碼實現

#include "math.h" 
#include "stdio.h" 
 
#define PI  3.1415926 
 
typedef struct 
{ 
    unsigned short x; 
    unsigned short y; 
    unsigned short z; 
}sensor_data_struct; 
 
static sensor_data_struct ref={0,0,255}; 
static sensor_data_struct test={0,180,180}; 
 
//計算當前向量與參考向量的夾角 
float get_angle(sensor_data_struct data) 
{ 
    float cosine; 
    float temp,angle; 
    cosine=(data.x*ref.x+data.y*ref.y+data.z*ref.z)/ \ 
            ((sqrt(data.x*data.x+data.y*data.y+data.z*data.z))*\ 
             (sqrt(ref.x*ref.x+ref.y*ref.y+ref.z*ref.z))); 
 
     temp=acos(cosine); 
     angle=(temp*180)/PI; 
     return angle; 
} 
 
int main(int argc, char *argv[]) 
{ 
    float angle; 
    printf("reference vector (%d,%d,%d)\r\n",ref.x,ref.y,ref.z); 
    printf("test vector (%d,%d,%d)\r\n",test.x,test.y,test.z); 
    angle=get_angle(test); 
    printf("angle = %f'\r\n",angle); 
    return 0; 
} 

• reference vector (0,0,255)
• test vector (0,180,180)
• angle = 45.000004'

4、優化改進

求解角度使用的三角函數，部分單片機可能不支持;對角度的精度，使用整形即可。基于這個條件，可以建立cosθ的數組表，以1度--2度--3度---180度步進，按如下代碼生成數組表：

void creat_table(void) 
{ 
    float i; 
    for(i=0;i<180;i++)//i的步進值決定精度 
    { 
        if((unsigned char )i%9==0) 
        { 
            printf("\r\n"); 
        } 
        printf("%f,",cos(i*PI/180));//角度轉弧度再傳入 
    } 
} 

根據代碼生成數組表后，查找余弦表，數組的下標即為角度。

static const float 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, 
}; 
 
unsigned short get_angle(sensor_data_struct data) 
{ 
    float cosine; 
    unsigned short i; 
 
    cosine=(data.x*ref.x+data.y*ref.y+data.z*ref.z)/ \ 
            ((sqrt(data.x*data.x+data.y*data.y+data.z*data.z))*\ 
             (sqrt(ref.x*ref.x+ref.y*ref.y+ref.z*ref.z))); 
 
    for(i=0;i<180;i++) 
    { 
        if(cos_table[i]<cosine)//查表 
        { 
            return i; 
        } 
    } 
    return 180;//error 
} 
 
int main(int argc, char *argv[]) 
{ 
    unsigned short angle;//改成整形 
    printf("reference vector (%d,%d,%d)\r\n",ref.x,ref.y,ref.z); 
    printf("test vector (%d,%d,%d)\r\n",test.x,test.y,test.z); 
 
    angle=get_angle(test); 
 
    printf("angle = %d'\r\n",angle); 
    return 0; 
} 

• reference vector (0,0,255)
• test vector (0,180,180)
• angle = 46'

查表得出46度，因為查表以及浮點的精度，所以角度誤差+-1度，但這個不影響業務邏輯。

5、小節

1、空間向量夾角公式可在基于xyz三軸的傳感器中應用。

2、針對范例中的應用，兩個向量的參數必須是在靜止情況下采樣，根據向量模進行過濾，否則角度計算錯誤。

3、未考慮大于180度的翻轉。

本文轉載自微信公眾號「嵌入式系統」，可以通過以下二維碼關注。轉載本文請聯系嵌入式系統公眾號。