有些设备正常工作时需按合适的方位安装,比如GPS天线必须朝向天空才能保证信号最佳,温湿度传感器监测口必须朝向被测目标才能及时响应。软件需求是在安装角度异常时提醒用户改变位置。
有些设备正常作业时需按适宜的方位装置,比方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"
- #definePI3.1415926
- typedefstruct
- {
- unsignedshortx;
- unsignedshorty;
- unsignedshortz;
- }sensor_data_struct;
- staticsensor_data_structref={0,0,255};
- staticsensor_data_structtest={0,180,180};
- //核算当时向量与参阅向量的夹角
- floatget_angle(sensor_data_structdata)
- {
- floatcosine;
- floattemp,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;
- returnangle;
- }
- intmain(intargc,char*argv[])
- {
- floatangle;
- printf("referencevector(%d,%d,%d)\r\n",ref.x,ref.y,ref.z);
- printf("testvector(%d,%d,%d)\r\n",test.x,test.y,test.z);
- angle=get_angle(test);
- printf("angle=%f'\r\n",angle);
- return0;
- }
- reference vector (0,0,255)
- test vector (0,180,180)
- angle = 45.000004'
4、优化改善
求解视点运用的三角函数,部分单片机或许不支撑;对视点的精度,运用整形即可。依据这个条件,能够树立cosθ的数组表,以1度--2度--3度---180度步进,按如下代码生成数组表:
- voidcreat_table(void)
- {
- floati;
- for(i=0;i<180;i++)//i的步进值决议精度
- {
- if((unsignedchar)i%9==0)
- {
- printf("\r\n");
- }
- printf("%f,",cos(i*PI/180));//视点转弧度再传入
- }
- }
依据代码生成数组表后,查找余弦表,数组的下标即为视点。
- staticconstfloatcos_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,
- };
- unsignedshortget_angle(sensor_data_structdata)
- {
- floatcosine;
- unsignedshorti;
- 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)//查表
- {
- returni;
- }
- }
- return180;//error
- }
- intmain(intargc,char*argv[])
- {
- unsignedshortangle;//改成整形
- printf("referencevector(%d,%d,%d)\r\n",ref.x,ref.y,ref.z);
- printf("testvector(%d,%d,%d)\r\n",test.x,test.y,test.z);
- angle=get_angle(test);
- printf("angle=%d'\r\n",angle);
- return0;
- }
- reference vector (0,0,255)
- test vector (0,180,180)
- angle = 46'
查表得出46度,由于查表以及浮点的精度,所以视点差错+-1度,但这个不影响事务逻辑。
5、末节
1、空间向量夹角公式可在依据xyz三轴的传感器中运用。
2、针对典范中的运用,两个向量的参数有必要是在停止状况下采样,依据向量模进行过滤,不然视点核算错误。
3、未考虑大于180度的翻转。
本文转载自微信大众号「嵌入式体系」,能够经过以下二维码重视。转载本文请联络嵌入式体系大众号。
知优网 » 嵌入式算法之空间向量夹角公式及其使用(计算空间向量夹角)