三轴加速度计值检测碰撞和摔倒

一.思路

1.发生碰撞:依次计算加速度计值之间的差值,当差值累计超过一定值时则发生碰撞。这个方法比较简单,缺点是误差比较大,需要通过不断调整阈值以及次数使结果接近准确

2.发生摔倒:摔倒后为静止态时,才会去检测发生摔倒,运动时不进行检测。分别计算x,y,z三轴与重力加速度之间的夹角,当夹角大于一定度数多次后,判定为摔倒。

二.摔倒计算夹角分析

 发生碰撞的计算方式比较简单这里不再赘述。

(1) 当芯片处于静止状态时,x,y重力加速度计值为0,z轴加速度计值为重力加速度g,如下图所示

        

  (2) 当物体倾斜一定的角度后。需要计算x,y,z与水平线之间的夹角也就是α1,β1, γ1;  α, β, γ分别是α1,β1, γ1的余角,也是和重力加速度g之间的夹角,根据这个关系可以推导出α1,β1, γ1

    

 这里为了好理解只计算X轴的角度α1;

角的关系:sinα1 = Ax / g   ->  Ax = gsinα1                                                                    (1)

边的关系:g*g = Ax*Ax + gconα1*gconα1  ->  gconα1 = sqrt(g*g - Ax*Ax)                     (2)

 (1)比(2) ->  tanα1 = Ax / sqrt(g*g - Ax*Ax) 

因为在静止时重力加速度的平方等于三轴平方和,即:g*g = Ax*Ax + Ay*Ay + Az*Az

综上tanα1 = Ax / sqrt(Az*Az + Ay*Ay) 

其中 α1 、β1 、γ1分别是X、Y、Z轴和水平线的弧度值(反三角函数计算的值是弧度),Ax 、Ay、Az是三个轴上的加速度值。那么弧度值分别为:

α1 = arctan(Ax / sqrt(Ay*Ay + Az*Az))

β1= arctan(Ay / sqrt(Ax*Ax+Az*Az))

γ1= arctan( Az / sqrt(Ax*Ax +Ay*Ay))

又因为弧度= θπR/180,所以

θx  = [arctan(Ax / squr(Ay*Ay + Az*Az))] *180/π

θy  = [arctan(Ay / squr(Ax*Ax+Az*Az))]*180/π

θz  = [arctan( Az / squr(Ax*Ax +Ay*Ay))]*180/π

二.代码实现

碰撞

sensor_event report = {0};
void collision_detect()
{
    uint16_t acc_x = 0;
    uint16_t acc_y = 0;
    uint16_t acc_z = 0;
    static uint16_t last_x = 0;
    static uint16_t last_y = 0;
    static uint16_t last_z = 0;
//发生碰撞的阈值,根据需要修改
    static uint16_t detect_threshold = 6000;
//碰撞的次数
    static uint8_t detect_count;
    uint8_t ret;


    sensor_acc_get_data_original(&(report.x_acc), &(report.y_acc), &(report.z_acc));//获取原始值


    acc_x = port_abs(report.x_acc - last_x);
    acc_y = port_abs(report.y_acc - last_y);
    acc_z = port_abs(report.z_acc - last_z);

           
    ret = (acc_x > detect_threshold) + (acc_y > detect_threshold) + (acc_z > detect_threshold);
    if (ret > 0) {
          detect_count += ret;
    }

    last_x = report.x_acc;
    last_y = report.y_acc;
    last_z = report.z_acc;



    if ((detect_count >= 3)) {
        detect_count = 0;
        last_x = 0;
        last_y = 0;
        last_z = 0;
        port_trace("happend collision");
        return;
    }
}

摔倒的代码 

#define g 1024
#define PI 3.14
sensor_event report = {0};
uint8_t tumble_count = 0;
void tumble_handle()
{
    uint16_t result = 0;
    uint16_t angle_x;
    uint16_t angle_y;    
    uint16_t angle_z;

    result = sqrt(report.x_acc * report.x_acc + report.y_acc * report.y_acc + report.z_acc * report.z_acc);

//判断为静止
    if (result < g ) {
        port_trace("no tumble");
    }

    angle_x = (atan(report.x_acc / sqrt(report.y_acc * report.y_acc + report.z_acc * report.z_acc))) * 180 / PI;
    angle_y = (atan(report.y_acc / sqrt(report.x_acc * report.x_acc + report.z_acc * report.z_acc))) * 180 / PI;
    angle_z = (atan(report.z_acc / sqrt(report.x_acc * report.x_acc + report.y_acc * report.y_acc))) * 180 / PI;


    if ((port_abs(angle_x) > 45 ) || (port_abs(angle_y) > 45) || (port_abs(angle_z) > 45 )) {
        tumble_count ++;
        if (tumble_count >= 3) {
            port_trace("tumble");
            tumble_count = 0;
        }
    } else {
        tumble_count = 0;
    }
}

  • 3
    点赞
  • 31
    收藏
  • 打赏
    打赏
  • 5
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页
评论 5

打赏作者

QX_a11

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值