本发明涉及一种机器人位置与姿态估计方法，具体涉及使用编码器与惯性导航单元数据融合的位置姿态估计方法。
背景技术：
对于双主动轮机器人，对位置和姿态的估计，最常见的做法是在主动轮上加装编码器读取转速信息，再根据测程法计算历程信息得到机器人的位置与姿态。但是由于编码器的精度问题，以及轮子与地面的细小打滑，此法得到的历程信息会逐渐累积误差越偏越大，尤其是在对姿态的估计不准确。其次，使用惯性导航单元也是较为常见的方法。但是与编码器相同，时间长之后也会累积误差，尤其是对位置的估计不准确。而且由于惯性导航单元测量的是角加速度与加速度，要得到位置与姿态需要积分两次，这会大大放大误差，所以一般工程上可实用的都是价格昂贵的高精度惯性导航单元。
技术实现要素：
本发明解决了编码器与惯性导航单元的数据融合问题，使得最终位置姿态估计的效果有大幅提升，具体内容如下：
考虑到编码器对方向的估计的误差会累积到对路程的估计上，对路程的短期估计较准确；相应的惯性导航单元对方向估计并没有累积误差，而对位移的估计有累积误差。故考虑使用编码器得到的方向信息与惯性导航单元得到的方向信息做互补滤波得到方向输出，以此方向为准再加上编码器的路程估计得到位姿估计。具体如下，一种基于编码器与惯性导航单元的机器人位姿估计方法：
1) 对惯性导航单元的加速度计做低通滤波，截止频率取机器人运行时的震动频率，得到重力向量；
2) 直接对惯性导航单元的陀螺仪输出的数据做积分得到机器人的姿态；
3) 将上述的重力向量和上述的姿态做互补滤波，使得角度在短期内的变化情况较为准确且长期误差也不会发散，此时可消除角度roll、yaw、pitch三个分量中roll与pitch的误差发散；
4) 对于yaw角，使用陀螺仪积分出的yaw角与惯性导航单元的磁力计输出的数据做互补滤波，使得yaw角误差也不发散，如此得到惯性导航单元的姿态输出；
5) 对两个编码器的速度差进行积分得到角度估计，由于此估计也会存在误差累积，故与惯性导航单元输出的姿态做互补滤波，得到最终角度的输出；
6) 在上述的最终角度下，对两编码器的速度的平均值做位移积分，得到最终的位置输出。
进一步地，传感器在机器人上的安装位置为：将编码器安装在两主动轮侧面读取转速信息；将惯性导航单元安装在两主动轮中心点，即机器人的自转圆心的正上方。
进一步地，传感器的数据读取方法为：编码器连接stm32单片机，用外部中断触发中断服务函数计数，通过串口发送给上位机；惯性导航单元直接使用SPI接口连接上位机，直接将数据传入上位机。
上述方法结合了编码器与惯性导航单元各自的优势，提升了整体位姿估计的精度。而且可以应用在低陈本惯性导航单元上，降低了系统的硬件价格。
具体实施方式
1. 传感器安装
1) 将编码器安装在两主动轮侧面读取转速信息；
2) 将惯性导航单元安装在两主动轮中心点，即机器人的自转圆心的正上方。
2. 数据读取
1) 编码器连接stm32单片机，用外部中断触发中断服务函数计数，通过串口发送给上位机。
2) 惯性导航单元直接使用SPI接口连接上位机，直接将数据传入上位机。
3.计算流程
1)对惯性导航单元的加速度计做低通滤波，截止频率取机器人运行时的震动频率，得到重力向量；
2)直接对惯性导航单元的陀螺仪输出的数据做积分得到机器人的姿态；
3) 将上述的重力向量和上述的姿态做互补滤波，使得角度在短期内的变化情况较为准确且长期误差也不会发散，此时可消除角度roll、yaw、pitch三个分量中roll与pitch的误差发散；
4)对于yaw角，使用陀螺仪积分出的yaw角与惯性导航单元的磁力计输出的数据做互补滤波，使得yaw角误差也不发散，如此得到惯性导航单元的姿态输出；
5)对两个编码器的速度差进行积分得到角度估计，由于此估计也会存在误差累积，故与惯性导航单元输出的姿态做互补滤波，得到最终角度的输出；
6)在上述的最终角度下，对两编码器的速度的平均值做位移积分，得到最终的位置输出。