基于加速度计的倾角检测算法-C语言程序

一、前言

在物联网时代，传感器是数据收集的最底端，如何运用传感器数据是一门技术活。

加速度计是一种常见的传感器，在某些场合下，我们可以利用加速度计求出物体实时姿态与参考姿态的夹角，例如做老人宝项目倾倒检测、数字倾角计等。

本文将会分析加速度计来做倾角检测的原理，以及给出原代码。

二、倾角检测原理

加速度计能够采集物体的加速度数据，市面上有很多加速度传感器，例如LIS3DH等，其输出的加速度数据与我们理解的加速度不太一样，例如：当正立放置时(+Z轴指向重力的反方向)，此时，加速度计输出的加速度值（理想情况下，不考虑噪声）为:X:0mg,Y:0mg,Z:1000mg,将加速度计倒立，加速度为:X:0mg,Y:0mg,Z:-1000mg,将+Y轴正立放置，加速度为X:0mg,Y:1000mg,Z:0mg，-Y轴正立：X:0mg,Y:-1000mg,Z:0mg，以此类推。

我们可以看出，加速度计内部模型可以理解为:加速度计是一个立方体，中间有一个小球，立方体的六个内表面有压力传感器，六个内表面记为±X，±Y，±Z，当+Z朝上时，小球受到重力，压在+Z面上，+Z面上受到1000mg重力，其他表面不受力，因此加速度输出为:X:0mg,Y:0mg,z1000mg。以此类推。
根据以上分析，X，Y，Z轴三个轴的加速度合值，必然指向重力的反方向，即使是加速度计向任意方向倾斜的情况下，亦是如此。

那么如果我们想得出一个物体与参考方向的倾角，我们可以将参考方向的三轴加速度值作为参考向量a(x1,y1,z1)，物体实时加速度作为实时向量b(x2,y2,z2),只需要计算向量a与向量b的夹角即可得出物体与参考方向的夹角。

三、算法简介

根据以上分析，算法本质是求参考向量与实时向量的夹角a(x1,y1,z1),b(x2,y2,z2)，根据向量内积公式可得出:
cos(∠aob) = (a · b)/(norm(a)norm(b));//向量内积
第一步：先归一化X,Y,Z轴加速度值
为什么要归一化呢？
因为每次加速度数据并不都是稳定的，加速度的矢量和大小会改变，因此需要做归一化处理。
第二步：根据内积公式求参考向量与实时向量夹角

四、原代码

/本例给出了倾角超过40°报警的示例，可以修改/

typedef struct {
rt_int32_t AcceX;				/*acceleration x*/
rt_int32_t AcceY;				/*acceleration y*/
rt_int32_t AcceZ;				/*acceleration z*/
} MENS_XYZ_STATUS_T;
 bool lean_check(MENS_XYZ_STATUS_T *acc_xyz)
{
bool Ret = FALSE;
float norm,ax,ay,az,dot,dot_norm; 
MENS_XYZ_STATUS_T tmp;
float angle;
/*去除加速度计抖动,30mg变化视为抖动*/	
if(abs(acc_xyz->AcceX-acc.AcceX)>=30){
	tmp.AcceX = acc_xyz->AcceX;
	acc.AcceX = acc_xyz->AcceX;
}else{
	tmp.AcceX = acc.AcceX;
}
if(abs(acc_xyz->AcceY-acc.AcceY)>=30){
	tmp.AcceY = acc_xyz->AcceY;
	acc.AcceY = acc_xyz->AcceY;
}else{
	tmp.AcceY = acc.AcceY;
}

if(abs(acc_xyz->AcceZ-acc.AcceZ)>=30){
	tmp.AcceZ = acc_xyz->AcceZ;
	acc.AcceZ = acc_xyz->AcceZ;
}else{
	tmp.AcceZ = acc.AcceZ;
}

/*归一化处理*/
norm = (float)sqrt((double)(tmp.AcceX*tmp.AcceX) + (double)(tmp.AcceY*tmp.AcceY) + (double)(tmp.AcceZ*tmp.AcceZ));
if (norm == 0.0) norm = 0.000001;  //avoid Nan happen
ax = tmp.AcceX / norm;
ay = tmp.AcceY / norm;
az = tmp.AcceZ / norm;

/*检测倾角是否超限40°*/

/*ax_ref,ay_ref,az_ref
为归一化后的参考向量的分量，可以自己设置，如定义为ax_ref=0,ay_ref=0,az_ref=1，表示加速度计正立放置
也可以让终端程序稳定运行一段时间后，取一个加速度值作为参考向量，这样就可以不论物体怎么摆放，当他稳定后就可以确定初始向量*/
	dot = ax*ax_ref+ay*ay_ref+az*az_ref;
	dot_norm = (float)sqrt((double)(ax*ax) + (double)(ay*ay) + (double)(az*az));
	if (dot_norm == 0.0) dot_norm = 0.000001;
	**//针对不少人的疑问:norm_ref和dot_norm 一样是归一化后的向量的模，norm_ref是参考的向量而已,可以在开机时记录，或者在某个特定时刻指定,计算公式参考dot_norm ,需要注意的是参考向量也需要归一化**
	angle = acos(dot/(dot_norm*norm_ref))*180/PI;//弧度转化为角度，
	
	if(angle>=40){
		return TRUE;
	}else{
		return FALSE;
	}
return Ret;

}

欢迎使用Markdown编辑器

你好！ 这是你第一次使用 Markdown编辑器 所展示的欢迎页。如果你想学习如何使用Markdown编辑器, 可以仔细阅读这篇文章，了解一下Markdown的基本语法知识。

新的改变

我们对Markdown编辑器进行了一些功能拓展与语法支持，除了标准的Markdown编辑器功能，我们增加了如下几点新功能，帮助你用它写博客：

1. 全新的界面设计 ，将会带来全新的写作体验；
2. 在创作中心设置你喜爱的代码高亮样式，Markdown 将代码片显示选择的高亮样式 进行展示；
3. 增加了 图片拖拽 功能，你可以将本地的图片直接拖拽到编辑区域直接展示；
4. 全新的 KaTeX数学公式 语法；
5. 增加了支持甘特图的mermaid语法¹ 功能；
6. 增加了 多屏幕编辑 Markdown文章功能；
7. 增加了 焦点写作模式、预览模式、简洁写作模式、左右区域同步滚轮设置 等功能，功能按钮位于编辑区域与预览区域中间；
8. 增加了 检查列表 功能。

功能快捷键

撤销：Ctrl/Command + Z
重做：Ctrl/Command + Y
加粗：Ctrl/Command + B
斜体：Ctrl/Command + I
标题：Ctrl/Command + Shift + H
无序列表：Ctrl/Command + Shift + U
有序列表：Ctrl/Command + Shift + O
检查列表：Ctrl/Command + Shift + C
插入代码：Ctrl/Command + Shift + K
插入链接：Ctrl/Command + Shift + L
插入图片：Ctrl/Command + Shift + G

合理的创建标题，有助于目录的生成

直接输入1次#，并按下space后，将生成1级标题。
输入2次#，并按下space后，将生成2级标题。
以此类推，我们支持6级标题。有助于使用TOC语法后生成一个完美的目录。

如何改变文本的样式

强调文本 强调文本

加粗文本 加粗文本

标记文本

删除文本

引用文本

H₂O is是液体。

2¹⁰ 运算结果是 1024.

插入链接与图片

链接: link.

图片:

带尺寸的图片:

居中的图片:

居中并且带尺寸的图片:

当然，我们为了让用户更加便捷，我们增加了图片拖拽功能。

如何插入一段漂亮的代码片

去博客设置页面，选择一款你喜欢的代码片高亮样式，下面展示同样高亮的 代码片.

// An highlighted block
var foo = 'bar';

生成一个适合你的列表

• 项目
  • 项目
    • 项目

1. 项目1
2. 项目2
3. 项目3

• 计划任务
• 完成任务

创建一个表格

一个简单的表格是这么创建的：

设定内容居中、居左、居右

使用:---------:居中
使用:----------居左
使用----------:居右