【嵌入式算法】空间向量夹角公式及其应用

有些设备正常工作时需按合适的方位安装，比如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.1415926typedef struct
{signed short x;signed short y;signed 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;
}

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;
}

查表得出46度，因为查表以及浮点的精度，所以角度误差±1度，但这个不影响业务逻辑。

5、小节
1、空间向量夹角公式可在基于xyz三轴的传感器中应用。
2、针对范例中的应用，两个向量的参数必须是在静止情况下采样，根据向量模进行过滤，否则角度计算错误。
3、未考虑大于180度的翻转。

【嵌入式算法】空间向量夹角公式及其应用相关推荐

python之计算空间向量夹角
原理: 代码 import numpy as npdef cal_angle_of_vector(v0, v1, is_use_deg=True):dot_product = np.dot(v0, v ...
c++计算空间向量的夹角
代码 void UtilsClass::normalization(double unnorm_array[3]) {// 归一化double max_val = std::max(abs(unnor ...
Kinect体感机器人（三）—— 空间向量法计算关节角度
Kinect体感机器人(三)-- 空间向量法计算关节角度 By 马冬亮(凝霜 Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 终于写到体感机器人的核心代码了, ...
（转）Kinect体感机器人—— 空间向量法计算关节角度
Kinect体感机器人(三)-- 空间向量法计算关节角度 By 马冬亮(凝霜 Loki) 一个人的战争(http://blog.csdn.net/MDL13412) 终于写到体感机器人的核心代码了, ...
matlab求两向量夹角_高等数学之向量代数与空间解析几何知识点与题型总结
向量代数与空间解析几何知识点: (1)向量代数知识点 (2)两平面夹角与两直线夹角公式两平面夹角和两直线夹角公式 (3)点到直线的距离公式点到直线的距离 (4)常见二次曲线常见二次曲线题型一: ...
matlab求两向量夹角_高中数学《平面向量的数量积》说课稿
高中数学<平面向量的数量积>说课稿作为一位无私奉献的人民教师,有必要进行细致的说课稿准备工作,借助说课稿可以让教学工作更科学化.那么写说课稿需要注意哪些问题呢?以下是小编为大家收集的高中 ...
c++ 空间向量类(Vector3D)的实现
该类可用于描述3D向量和点在空间中的位置. 可以使用该类保存与计算 3D 位置和方向. 此外,它还包含用于执行常见向量操作的函数. 头文件: #pragma once #include <cma ...
matlab求两向量夹角_高考数学解题技巧：专题21 平面向量中最值、范围问题高中数学黄金解题模板...
↑ 点击上方"红色笔记"关注我们高中各科解题技巧下载回复数字:800009 高考数学解题技巧:专题24 数列求和方法高中数学黄金解题模板高考数学解题技巧:专题26 含参不等 ...
嵌入式算法移植优化学习笔记5——CPU,GPU,TPU,NPU都是什么
嵌入式算法移植优化学习笔记5--CPU,GPU,TPU,NPU都是什么一.什么是CPU? 二.什么是GPU? 三.什么是NPU? 四.什么是TPU? 附: 随着AI的广泛应用,深度学习已成为当前AI ...

【嵌入式算法】空间向量夹角公式及其应用

【嵌入式算法】空间向量夹角公式及其应用相关推荐

最新文章

热门文章