基于IMU的人体姿态检测

最近用IMU做了一个人体姿态检测的demo，可实现两个肢节（大腿&小腿）的姿态检测。具体做法是下位机用Arduino Mega 2560读取姿态传感器的角度数据，再通过串口向上位机发送姿态数据。上位机使用Matlab读取串口数据，并实时显示出来，效果如下。

1.姿态解算

从IMU中可以获取陀螺仪的三轴角速度和加速度计的三轴加速度，由角速度积分可以得到三轴角度，由加速度分量也可以得到重力方向上的两轴角度。但角速度积分存在累计误差，加速度解算噪声很大，所以再用Kalman滤波将这两个数据融合一下（具体算法下次再写），就可以得到较为准确的两轴角度值。若没有磁力计，则偏航角只能由陀螺仪积分得到，不太准确且有漂移问题。

但为了方便，本实验没做姿态结算，而是直接购买了自带姿态结算的模块，然后只要用Arduino读取数据就可以啦。向上位机发送数据时，以逗号为间隔符，以换行为结束符。Arduino代码如下：

#include <Wire.h>
#include <JY901.h>

CJY901 IMU1;
CJY901 IMU2;

void setup() 
{
  Serial.begin(2000000); 
  Serial1.begin(115200);
  Serial2.begin(115200);
  delay(1000);
}

void loop() 
{
  Serial.print((float)IMU1.stcAngle.Angle[0]/32768*180);
  Serial.print(',');
  Serial.println((float)IMU2.stcAngle.Angle[0]/32768*180);
  delay(50);

  while (Serial1.available()) 
  {
    IMU1.CopeSerialData(Serial1.read()); //Call JY901 data cope function
  }
  while (Serial2.available()) 
  {
    IMU2.CopeSerialData(Serial2.read()); //Call JY901 data cope function
  }
}

Matlab显示

为了做到串口数据读取和实时绘图，这里的处理相对麻烦一点。代码如下

Serial.m

clc;

%% 定义全局变量
global t;
global x;
global y1;
global y2;
global index;
global point1;
global point2;

%% 变量初始化
t = [0];
y1 = [0];
y2 = [0];
index = 0;
x = -100;
point1  = [0 -50];
point2  = [0 -100];

%% 绘图设置
subplot(2,1,2);
p1 = plot(t,y1,'EraseMode','background','MarkerSize',5);
hold on
p2 = plot(t,y2,'EraseMode','background','MarkerSize',5);
axis([x-200 x+200 -90 90]);
grid on;
subplot(2,1,1);
pp = plot([0,point1(1),point2(1)],[0,point1(2),point2(2)],'-bo');
axis([-110 110 -110 0]);

%% 串口设置
try
    s=serial('com10');
catch
    error('cant serial');
end
set(s,'BaudRate', 2000000,'DataBits',8,'StopBits',1,'Parity','none','FlowControl','none');
s.BytesAvailableFcnMode = 'terminator';
s.BytesAvailableFcn = {@callback,p1,p2,pp};
fopen(s);

%% 关闭
pause;
fclose(s);
delete(s);
clear s;
close all;
clear all;

回调函数

function callback(s,BytesAvailable,p1,p2,pp)
    
    global t;
    global x;
    global y1;
    global y2;
    global index;
    global point1;
    global point2;
    
    %% 计算角度
    STR = fscanf(s);
    str = strsplit(STR,',');
    angle1 = 90-str2num(char(str(1)));
    angle2 = 90-str2num(char(str(2)));
    
    %% 更新绘图参数
    t = [t index];
    y1 = [y1 angle1];
    y2 = [y2 angle2];
    point1 = [50*sin(angle1/180*pi) -50*cos(angle1/180*pi)];
    point2 = [(point1(1)+50*sin(angle2/180*pi)) (point1(2)-50*cos(angle2/180*pi))];
    
    %% 更新绘图
    set(p1,'XData',t,'YData',y1(1,:));
    set(p2,'XData',t,'YData',y2(1,:));
    set(pp,'XData',[0,point1(1),point2(1)],'YData',[0,point1(2),point2(2)]);
    subplot(2,1,2);
    axis([x-200 x+200 -90 90]);
    subplot(2,1,1);
    axis([-110 110 -110 0]);
    drawnow;

    x = x + 1;
    index=index+1;
   
end