基于ROS与C++，只利用IMU实现里程计和计步器，精确计算行走距离

老规矩，开门见山，先说说本篇文章实现了什么：

1、只使用IMU模块，实现了对于人行走距离的检测（精度在0.5m左右）

2、只使用IMU模块，通过峰值检测\机器学习两个方法，实现了一个精确的计步器

背景：

最近在开发一个可穿戴设备，需要实现对于人行走距离的精确判断，并获得这个行走距离。这么做的目的是因为项目要求可穿戴设备能够实现SLAM中的建图，定位，导航。我们知道，无论是常见的建图算法（Gmapping、cartographer）还是后期涉及的自助导航，都需要订阅里程信息（/odom），在一般的移动机器人中，里程信息比较容易获取，可以直接由安装在底盘轮子上的轮速计或者编码器来获得。

但是在这个可穿戴项目中，并没有轮子这种东西，所以只能依靠各种传感器来实现里程计，常见的能够装载在可穿戴设备上的传感器有IMU、激光雷达、超声波测距探头（HC-SR04）、GPS。单一的传感器由于系统误差和各种限制，获取的距离信息都有一定程度上的不准确，实际上如果要获得准确的距离信息，一定是需要实现多传感数据融合的，这里涉及到一些卡尔曼滤波的知识，还有很多更深入的关于如何融合传感数据的考虑，随着项目的开发可能后续还会记录。

这里先记录一下只利用简单的IMU模块实现一个比较精确地里程计（经过多轮测试，误差可以控制在0.5m以内）

涉及技术：

简单的C++和ROS编程知识，如果想要玩的花一点，可以再加一个python+pytorch，引入机器学习方案。

具体思路及实现：

首先思考IMU模块能够带来什么？IMU模块能够为我们提供携带IMU传感模块的设备在运动时的三轴加速度（ax、ay、az）。利用这个信息，我们很快就能够产生第一条思路：是否能够利用中学都学过的牛顿第二定律，结合几个速度公式直接简单粗暴的计算出来移动的距离？

V = V0 + a*t

D = V0*t + 0.5*a*t*t

说干就干，首先在ROS中订阅IMU的数据，获取三轴的加速度，出于测试，我们只考虑人走直线的情况，就是只使用x轴向加速度，假设我们从静止开始移动，设定初速度V0为0，编写程序，把速度公式用上，然后在rosrun节点后开始走路，走5m就停止，看一下算出来的距离和5m有多少出入。

void IMUcallback(const sensor_msgs::Imu::ConstPtr& msg)
 {
		double a_now, volocity, pre_volocity, scantime;
    //加速度
     double ax = msg->linear_acceleration.x;
		//v = v0 + at
     velocity =pre_velocity + a_now*scantime;
		//d = v0t + 0.5*a*t*t
     distance_a += velocity*scantime+0.5*a_now*scantime*scantime;

     ROS_INFO("distance_a is %2f",distance_a);
     pre_velocity = velocity;
 }

测试结果发现，计算获得的距离远远大于5M，短短几秒钟已经飙到了100多M，分析了一下原因，找到了以下几点：

1、采样频率，imu的采样频率是0.1秒一次，远远大于人的走路速度（查文献得知，人的走路速度大概是0.5秒一步），如果代入这个时间进行计算，会导致数据增长过快（这也是距离飚到100M的原因之一）

2、没有正确认识人的走路模式，在上述程序中，我们把人的行走看成了一个简单粗暴的质点运动，但是查阅文献后得知，实际上人的行走是一个动态的过程，如果把人走路过程中的速度和加速度的变化抽象成数学模型，那么都是类正弦函数（SINX），是一个先变大后变小的动态过程，具体原因是由于人在抬腿迈步和收腿的过程中重心会产生变化，导致加速度变化。

（绘制走路时的IMU加速度数据，验证走路时加速度变化类似于正弦函数）

结合以上两点，我们可以修改一下代码，解决以上两个问题。

针对第一点，我们可以由ROS：：Timer中创建一个timer对象，通过设定Duration（）中的时间，来控制计算频率。我们也可以创建vector容器，往里面插入数据（由于回调函数是0.1秒跑一次）所以当vector中插入了5个数据时，代表过了0.5秒，由于还要解决第二个问题，所以这里采用第二个创建vector容器的方案。

针对第二点，由于创建了vector容器，我们可以获得0.5秒内每0.1秒的加速度，对这些加速度取平均值来作为对动态过程的考虑，同时速度也利用数据的均值来进行计算。

//方案一示例
int main(){
ros::Nodehandle nh;
double scantime = 0.5;
ros::Timer timer = nh.createTimer(ros::Duration(scantime), timerCallback);
ros::spin();
}

void timerCallback(const ros::TimerEvent& event){
//进行计算....
}


//方案二示例
void imuCallback(const sensor_msgs::Imu::Constptr& imu_msg){
vector v1;
double ax = imu_msg->linear_acceleration.x;
v1.push_back(ax);
if(v1.size()==5)
{
//开始计算
v1.clear();//计算完了以后清理容器
}

考虑完了以上问题，再次进行测试，又发现了几个问题：

第一是人在停止时，距离数据依旧在累计。这是由于IMU在静止时由于人的轻微晃动，也采取到了x轴加速度的数据，虽然很小，但是依旧能够计算出距离，由于我们的算法是不含类似回环检测这种消除全局累积误差的方法的，所以我们尽可能在前期就避免引入过多的微小误差，以免随着运行时间的增加误差越累计越大。

第二是依旧是数据爆炸的问题，走了5M的路，但是实际上算出来的数据也虽然比上一个没优化过的好，但是依旧远大于5M。

要解决这两个问题，就要实现对于人的行走状态检测，只有检测到人在行走时，才开始计算人的移动距离，人在静止状态下，不累计距离。实现方案有两种，先说简单的，由于人在行走的时候三轴加速度都有一定程度的变化，通过收集数据，发现三轴加速度都符合前文提到的正弦函数模型，并且发现峰值的数量刚好能和走的步数对应上，通过查阅资料发现，这其实就是很多计步器的实现原理，即通过设备中的IMU采集人的三轴加速度，进行一定的数据耦合后检测耦合数据的峰值，每出现一个峰值，就代表着人走了一步。

（峰值检测，红点为峰值，可以看到去除了中间的小峰，也避开了开头的静止状态）

 明白了这个问题以后，我设计了一个算式耦合了三轴加速度的数据，同时编写了一个峰值检测算法，注意峰值检测算法要注意以下问题

• 检测峰值之前要对数据进行平滑化处理，只保留峰值，去除噪点数据
• 注意峰值检测的频率要符合人走路的频率
• 放大峰值能够发现，在大峰值之间存在很多小峰值，如果不进行去除，会影响判断，所以需要先把这些小峰值给去掉，只保留大的峰值，去除掉峰值之间坑坑洼洼的小峰值
• 要给峰值检测设定一个检测范围，只检测某个阈值之上的峰值，因为人在静止的时候三轴加速度也会有微小的变化，也会产生峰，所以要把这些给去掉，只保留运动时较大的峰值

（放大观察，可以看到有不少小峰值，如果不避开，会干扰检测）                  

结合以上几点注意事项，编写峰值检测算法，直接写进IMU的回调函数中进行实时检测，这样就做到了对于人的行走状态判断，同时还可以衍生出一个副产品---计步器（经过测试发现数据还蛮准确的，和我的Apple Watch测出来的一样），这个计步器可以为我们的里程计提供第二个数据来源，稍后再说。

这里留个坑，利用机器学习方法实现峰值检测。由于数据变化的很有规律，完全可以在这里上机器学习，搭建一个浅层的神经网络来学习数据变化的特征，由于人行走只有两个状态，0表示静止，1表示行走，可以很轻松的进行数据的标注。利用python+pytorch实现这个小任务，实现的后的效果和峰值检测类似，相对来说更准确一点，适用范围也更广一点，后续有时间补上。

中段总结：

回忆一下到目前为止我们做了什么，首先我们实现了对于人的行走状态判断，同时我们考虑了人的走路模型，考虑了加速度和速度的动态变化过程，同时我们考虑了人走路的频率和IMU采样数据频率之间的差异，基于以上几点考虑，此时实现的IMU里程计代码思路大致如下：

vodi IMUcallback(const sensor_msgs::Imu imu_msg)
{
double ax;
ax = imu_msg.acceleration.x;
vector v1;
v1.push_back(ax);

double imu_xyz;//自己设计的耦合数据

//峰值检测算法
bool is_walk = false;
.....
//如果持续检测到有效峰值，则把is_walk设为true，认为人在行走

//检测到人行走，才执行下面的距离计算代码，考虑人的启停
if(is_walk)
{
    if(v1.size()==5)//这里体现的是考虑走路频率
    {
    double a_mean;//用均值计算，考虑动态过程
    a_means = v1[0]+v1[1]...../5;
    double distance, velocity;
    distance += .....;
    v1.clear();
    }
}

接下来用以上代码结合实验室的IMU进行实际测试，走10M路，走25个来回（微信步数就是这么刷出来的），得到25组数据，最终的平均误差在1m以下还算不错。由于后面还有雷达和GPS，可以利用这两个好兄弟的数据进行融合，这个1m的误差很容易干掉，所以到此.......还不算完！

加入计步器来优化数据

突发奇想，上面已经实现了一个副产品，计步器，并且测试得到计步器的数据十分准确，基本上和走的步数相差无几，那么用一个很简单的思路：距离=步数*每一步的距离，那也能够得到一个里程数据，每一步的距离看着玄乎，但是步数很准确呀，还是有可行之处的。

这里我测量了一下我走路的平均距离，大概是0.6m/步，同时chatgpt告诉我亚洲人的平均步伐距离是0.7m/步，看来我拖后腿了，带入这个数据（0.6m/步），然后生成第二组里程数据distance_steps，上面用距离公式得到的数据记为distance_a，用这两组数据进行一个优化，如果峰值检测很顺利，异常数据很少（我在峰值检测算法中收集了异常数据），x轴的加速度变化的也比较规律，异常值较少，这就意味着走路走的很丝滑，各项数据都比较准确，那么就更愿意相信distance_a，给到它75%的权重。否则就更相信计步器的数据，毕竟人的行走状态判断容错率大，依据的是三轴数据的藕合，而距离公式只依赖于x轴，x轴数据一旦波动，就会导致距离累计误差很大，容错率小，此时给到75%的权重给到distance_steps。（有点多传感融合的味道了）

最终结果

完成这一切以后，再次走了25个来回，惊喜的发现，此时的平均误差控制到了0.5m，算一个不错的进步了，最后整理一下代码，规范一下写作，就利用IMU实现了一个简单的里程计，可以提供比较精确地里程数据，为后面的建图导航定位打下了一个不错的基础~