里程计

里程计的协方差在StatusPublisher.cpp中，是个固定值：

car_status.encoder_ppr = 4 * 12 * 64;
var_angle = (0.01f / 180.0f * PI) * (0.01f / 180.0f * PI);
// speed[0] = car_status.omga_r / car_status.encoder_ppr * 2.0 * PI * wheel_radius;
// speed[1] = car_status.omga_l / car_status.encoder_ppr * 2.0 * PI * wheel_radius;
var_len = (50.0f / car_status.encoder_ppr * 2.0f * PI * wheel_radius) * (50.0f / car_status.encoder_ppr * 2.0f * PI * wheel_radius);

CarOdom.pose.covariance = boost::assign::list_of(var_len)(0)(0)(0)(0)(0)(0)(var_len)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(var_angle);

观察左右轮的编码器增量，大部分情况下都相同，不同时只差1，极少情况差2，但可以忽略。

IMU

话题xqserial_server/IMU, IMU型号为MPU9250。 /xqserial_server/IMU的协方差矩阵没有处理，所以元素全是0

里程计持续偏差以及标定IMU

过了一段时间，发现机器人启动后，什么操作没有，base_link朝向会慢慢偏离odom坐标系的x轴，echo IMU话题发现朝向一直在累计，于是按照小强的官网进行标定，等到IMU话题的朝向都是0为止，问题解决

IMU长期使用后可能不准，车静止时，发现imu/linear_acceleration如下

# x, y 比0多了太多
x: 0.124591998756
y: 0.00958400033414
z: 9.84276771545

一样进行标定

建图时旋转后，里程计漂移严重

建图时如果只走直线，不旋转，看不出问题，稍微旋转，map和odom坐标系的相对位姿变化很明显，不能再使用。

走正方形测里程计的累计误差

机器人直接走正方形的程序

rosrun nav_test nav_square.py

但是我们需要先修改这个文件，因为默认的有问题。打开文件，头部添加：

import PyKDL
import math

紧接着添加下面内容，也就是在class NavSquare():之前：

def quat_to_angle(quat):
  rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
  return rot.GetRPY()[2]

def normalize_angle(angle):
    res = angle
    while res > math.pi:
        res -= 2.0*math.pi
    while res < -math.pi:
        res += 2.0*math.pi
    return res

将线速度改为0.15，角速度0.15，原来的实在太大了

从网上下载rbx1工程，这是一本教程里用到的，只把其中的rbx1_nav放到当前工作空间里，编译通过。

现在就可以运行了，走正方形的效果如下：


第一张图的左侧经过了一段不平的地面，明显效果不如第二张

用move_base走正方形

教程４里的fake_move_base_blank_map.launch其实是在一个空地图上运行了move_base的一套节点,指定odom和map坐标系的转换全是0,也就是重合. 对代价地图和算法的配置不必关心. square.py是让机器人以全局路径算法，从当前位置开始向前、向左走一个正方形，每次走一米。所以走得不是严格正方形

我写了一个程序，当机器人以odom为固定坐标系，base_link相对其运动时，显示运动的轨迹。再配合square.py，可以显示出小强走正方形的轨迹，连续走四次的轨迹如下：