基于加速度计的倾角检测算法-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语法1 功能;
  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语法后生成一个完美的目录。

如何改变文本的样式

强调文本 强调文本

加粗文本 加粗文本

标记文本

删除文本

引用文本

H2O is是液体。

210 运算结果是 1024.

插入链接与图片

链接: link.

图片: Alt

带尺寸的图片: Alt

居中的图片: Alt

居中并且带尺寸的图片: Alt

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

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

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

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

生成一个适合你的列表

  • 项目
    • 项目
      • 项目
  1. 项目1
  2. 项目2
  3. 项目3
  • 计划任务
  • 完成任务

创建一个表格

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

项目 Value
电脑 $1600
手机 $12
导管 $1

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

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

第一列 第二列 第三列
第一列文本居中 第二列文本居右 第三列文本居左

SmartyPants

SmartyPants将ASCII标点字符转换为“智能”印刷标点HTML实体。例如:

TYPE ASCII HTML
Single backticks 'Isn't this fun?' ‘Isn’t this fun?’
Quotes "Isn't this fun?" “Isn’t this fun?”
Dashes -- is en-dash, --- is em-dash – is en-dash, — is em-dash

创建一个自定义列表

Markdown
Text-to- HTML conversion tool
Authors
John
Luke

如何创建一个注脚

一个具有注脚的文本。2

注释也是必不可少的

Markdown将文本转换为 HTML

KaTeX数学公式

您可以使用渲染LaTeX数学表达式 KaTeX:

Gamma公式展示 Γ ( n ) = ( n − 1 ) ! ∀ n ∈ N \Gamma(n) = (n-1)!\quad\forall n\in\mathbb N Γ(n)=(n1)!nN 是通过欧拉积分

Γ ( z ) = ∫ 0 ∞ t z − 1 e − t d t   . \Gamma(z) = \int_0^\infty t^{z-1}e^{-t}dt\,. Γ(z)=0tz1etdt.

你可以找到更多关于的信息 LaTeX 数学表达式here.

新的甘特图功能,丰富你的文章

Mon 06 Mon 13 Mon 20 已完成 进行中 计划一 计划二 现有任务 Adding GANTT diagram functionality to mermaid
  • 关于 甘特图 语法,参考 这儿,

UML 图表

可以使用UML图表进行渲染。 Mermaid. 例如下面产生的一个序列图::

张三 李四 王五 你好!李四, 最近怎么样? 你最近怎么样,王五? 我很好,谢谢! 我很好,谢谢! 李四想了很长时间, 文字太长了 不适合放在一行. 打量着王五... 很好... 王五, 你怎么样? 张三 李四 王五

这将产生一个流程图。:

链接
长方形
圆角长方形
菱形
  • 关于 Mermaid 语法,参考 这儿,

FLowchart流程图

我们依旧会支持flowchart的流程图:

Created with Raphaël 2.2.0 开始 我的操作 确认? 结束 yes no
  • 关于 Flowchart流程图 语法,参考 这儿.

导出与导入

导出

如果你想尝试使用此编辑器, 你可以在此篇文章任意编辑。当你完成了一篇文章的写作, 在上方工具栏找到 文章导出 ,生成一个.md文件或者.html文件进行本地保存。

导入

如果你想加载一篇你写过的.md文件或者.html文件,在上方工具栏可以选择导入功能进行对应扩展名的文件导入,
继续你的创作。


  1. mermaid语法说明 ↩︎

  2. 注脚的解释 ↩︎

你可能感兴趣的:(传感器)