2-里程计运动学模型法用于校准轮式里程计和激光雷达实现代码）

轮式里程计定义
轮式里程计与激光SLAM的关系
轮式里程计校准前言
轮式里程计和激光雷达标定数学建模
Code
Result

轮式里程计定义

轮式里程计是什么：用轮子转速测量机器人行程的装置原理如下图所示，简单直接在这里插入图片描述实物就是这样光电编码器通常用于测量机器人领域的轮子转速。当轮子转动时，光电编码器接收脉冲信号，脉冲数乘以系数知道轮子转了多少圈。

两轮机器人，通过连续的轮速积分操作，可以知道机器人向前移动了多少，同时可以利用两轮速差计算机器人转动多少度,从而实现机器人航迹的计算和定位。

轮式里程计和激光SLAM的关系

激光SLAM帧间匹配的步骤之一是通过两个时刻的点云匹配获得这两个时刻的相对位置。

上面里程计也可以通过统计电机的速度来计算机器人的位置.

既然能得到位置，就涉及到多传感器融合的问题。说到融合，每个传感器都有自己的优缺点。

激光SLAM不适用于环境几何特征点较少的情况，如长走廊，显然轮式里程计与这种情况无关当路面打滑时，轮式里程计会出现累积误差。显然，激光SLAM没有这种情况.既然两者各有优势，就融合吧.

综合思想如下：激光可以定位轮式里程计SLAM提供两个时刻之间匹配的初始值，激光可以在初始值准确的情况下提高SLAM的精度。另外，轮式里程计可以单独实现短距离航迹计算定位，激光SLAM定位也可以实现，两者的融合可以加强定位的准确性性和鲁棒性.

轮式里程计标定前言

轮式里程计的基本原理是利用两轮速差计算机器人转动的程度。前提是已知车轮半径和两轮间距。

因此，在这种情况下，需要标定轮式里程计

硬件变形的轮速计
车轮半径和两轮间距发生变化
脉冲信号转换系数不允许

标定方法可分为两类

白已知的传感器物理模型中，需要采用基于模型的方法
黑盒校准：当您不知道传感器的物理模型时，可以尝试直接线性方法

轮式里程计和激光雷达标定数学建模

这里主要说的是两轮差速底盘运动学模型

一般室内机器人使用这种，优点是

结构简单
成本低
模型简单

在设计校准数学模型时，首先要确定已知的数量通过激光传感器的匹配结果可以知道 x位移 y位移角度两个轮子的旋转角度可以通过轮速里程计获得. 也就是说，里程计算需要通过轮子的角速来表示 x位移 y位移角度

建立数学模型的公式目标方向是用轮间距和轮半径来表示车身的角速和线速

如下图所示：

与车身旋转中心相比，底盘中心的转角速度等于两轮相对于车身旋转中心的转角速度。也就是说引入线速与角速关系d 整理得到r

运动解算求解w 将r带回，可以得到w为：

运动解算求解v 通过w*r简化即可得到v为：

车中心的线速和角速用轮子的角速来表示到目前为止，通过运动学模型建立的数学模型的角速度和车身的线速度

车身角速里程计算的车身角速和线速后，剩余的需要与激光雷达数据建立关系。激光雷达通过匹配，可以知道相对帧之间的平移和旋转。那么通过对车体的角速度及线速度积分即为旋转和平移需要注意的是，如果轮速里程计更新频率高，则需要积分激光雷达帧间所有里程计数据里程计在一次更新中以匀速假设。

角速积分关系如下线速积分关系如下实际上需要离散化

上面写成矩阵，方便使用最小二乘

以上是由第一帧激光及其帧间的轮速里程积累的数学方程。由m帧激光雷达数据和相应的轮速里程计数据建立的线性方程组如下：

这里有线性最小二乘的形式至此可求出J21和J22 通过平移关系，求解D后，即可完成校准。

通过积分求解轮速里程计的平移公式如下：同样，实际上需要离散化 D在上述离散方程中需要标定参数，J21和J22是上面已经找到的，可以计算角速和角度，所以D后括号中的一大块是已知量

与激光雷达的平移建立联系，以下方程可以租用

线性最小二乘方程组在这里建立D 目前已知D，J21、J22

Code

首先收集和处理数据得到

轮速里程计的信息 t_r(持续时间) w_L(左轮角度) w_R(右轮角度)
激光雷达的时间和匹配值 t_s(持续时间) s_x (x位移) s_y(y位移) s_th(角度)

int main(int argc, char** argv) { 
             // 放置激光雷达的时间和匹配值 t_s s_x s_y s_th     vector<vector<double>> s_data;     // 放置轮速计的时间和左右轮角速度 t_r w_L w_R     vector<vector<double>> r_data;      //通过 ifstream 读取txt文件     ifstream fin_s(scan_match_file);     ifstream fin_r(odom_file);      ////判断文件路径是否正确     if (!fin_s || !fin_r)
    { 
        
        cerr << "文件路径设置有问题" << endl;
        return 1;
    }

    // 读取激光雷达的匹配值
    //eof()判断是否读完
    while (!fin_s.eof()) { 
        
        double s_t, s_x, s_y, s_th;
        fin_s >> s_t >> s_x >> s_y >> s_th;
        s_data.push_back(vector<double>({ 
        s_t, s_x, s_y, s_th}));
    }
    fin_s.close();

    // 读取两个轮子的角速度
    while (!fin_r.eof()) { 
        
        double t_r, w_L, w_R;
        fin_r >> t_r >> w_L >> w_R;
        r_data.push_back(vector<double>({ 
        t_r, w_L, w_R}));
    }
    fin_r.close();

通过 ifstream 读取txt中的数据,并赋值给最前面声明的两个数据

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //计算中间变量J_21和J_22
    Eigen::MatrixXd A;//声明线性最小二乘的矩阵A
    Eigen::VectorXd b;//声明线性最小二乘的矩阵B
    // 设置数据长度
    A.conservativeResize(5000, 2);//设置A的大小 5000*2
    b.conservativeResize(5000);//设置B的大小 5000*1
    A.setZero();//A矩阵清零
    b.setZero();//B矩阵清零

    size_t id_r = 0;//轮速里程计数据取值id
    size_t id_s = 0;//激光数据取值id
    double last_rt = r_data[0][0];//取出轮速里程计第一个数据的时间
    double w_Lt = 0;//里程计角度积分累加值
    double w_Rt = 0;
    while (id_s < 5000)
    { 
        
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];//该组激光雷达的时间
        const double &s_th = s_data[id_s][3];//该组激光雷达的角度
        // 里程计信息
        const double &r_t = r_data[id_r][0];//该组里程计的时间
        const double &w_L = r_data[id_r][1];//该组里程计的左轮角速度
        const double &w_R = r_data[id_r][2];//该组里程计的右轮角速度

        //由于里程计更新的频率高,则选择里程计的时间刚大于激光雷达的时间的时候为匹配对
        ++id_r;//里程计的取值id加1

        // 在2帧激光匹配时间内进行里程计角度积分
        if (r_t < s_t)//如果轮速里程计的时间小于激光雷达的时间 则时间未匹配好
        { 
        
            double dt = r_t - last_rt;//轮速里程计 该段数据的 dt
            w_Lt += w_L * dt;//累加该段的角速度
            w_Rt += w_R * dt;//累加该段的角速度
            last_rt = r_t;//更新时间
        }
        else//时间匹配上了
        { 
        
            double dt = s_t - last_rt;//轮速里程计 该段数据的 dt
            w_Lt += w_L * dt;//累加该段的角速度
            w_Rt += w_R * dt;//累加该段的角速度
            last_rt = s_t;//更新时间
            // 填充A, b矩阵
            A(id_s, 0) = w_Lt;//填充A
            A(id_s, 1) = w_Rt;//填充A
            b[id_s] = s_th;//填充B
            w_Lt = 0;//清零一次激光雷达对应的 里程计角度值
            w_Rt = 0;//清零一次激光雷达对应的 里程计角度值
            ++id_s;//处理下一个激光雷达数据
        }
    }

首先用读入的数据填充A矩阵和B矩阵上面每一行代码均已注释注意: 和推导不同的是,轮速里程计的更新频率快,所以需要找到时间最为接近的数据作为填入值. 那么在一次的激光雷达更新,则里程计已更新多次,那么需要累加这几次的角度,即出现了 w_Lt += w_L * dt;//累加该段的角速度 w_Rt += w_R * dt;//累加该段的角速度

++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 进行最小二乘求解
    Eigen::Vector2d J21J22;
    J21J22 = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    const double &J21 = J21J22(0);
    const double &J22 = J21J22(1);
    cout << "J21: " << J21 << endl;
    cout << "J22: " << J22 << endl;

通过最小二乘,求解 J21和J22 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 求解轮间距b
    Eigen::VectorXd C;//声明线性最小二乘的矩阵A
    Eigen::VectorXd S;//声明线性最小二乘的矩阵b
    // 设置数据长度
    C.conservativeResize(10000);//5000个数据,需要5000*2的长度
    S.conservativeResize(10000);//5000个数据,需要5000*2的长度
    C.setZero();//A矩阵清零
    S.setZero();//b矩阵清零

    id_r = 0;//轮速里程计数据取值id
    id_s = 0;//激光雷达数据取值id
    last_rt = r_data[0][0];//取出轮速里程计第一个数据的时间
    double th = 0;//累加的轮速里程计 角度
    double cx = 0;//累加的轮速里程计 x
    double cy = 0;//累加的轮速里程计 y
    while (id_s < 5000)
    { 
        
        // 激光的匹配信息
        const double &s_t = s_data[id_s][0];//该组激光雷达的时间
        const double &s_x = s_data[id_s][1];//该组激光雷达的x
        const double &s_y = s_data[id_s][2];//该组激光雷达的y
        // 里程计信息
        const double &r_t = r_data[id_r][0];//该组里程计的时间
        const double &w_L = r_data[id_r][1];//该组里程计的左轮角速度
        const double &w_R = r_data[id_r][2];//该组里程计的右轮角速度
        ++id_r;//里程计的取值id加1
        // 在2帧激光匹配时间内进行里程计位置积分
        if (r_t < s_t)//如果轮速里程计的时间小于激光雷达的时间 则时间未匹配好
        { 
        
            double dt = r_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);//累加里程计该段时间的x
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);//累加里程计该段时间的y
            th += (J21 * w_L + J22 * w_R) * dt;//累加里程计该段时间的角度
            last_rt = r_t;//更新时间
        }
        else//时间匹配了
        { 
        
            double dt = s_t - last_rt;
            cx += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * cos(th);//累加里程计该段时间的x
            cy += 0.5 * (-J21 * w_L * dt + J22 * w_R * dt) * sin(th);//累加里程计该段时间的y
            th += (J21 * w_L + J22 * w_R) * dt;//累加里程计该段时间的角度
            last_rt = s_t;//更新时间
            // 填充C, S矩阵
            //TODO: (4~5 lines)
            C[id_s * 2] = cx;//填充A
            C[id_s * 2 + 1] = cy;//填充A
            S[id_s * 2] = s_x;//填充b
            S[id_s * 2 + 1] = s_y;//填充b
            //end of TODO
            cx = 0;//清零一次激光雷达对应的 里程计x
            cy = 0;//清零一次激光雷达对应的 里程计y
            th = 0;//清零一次激光雷达对应的 里程计角度
            ++id_s;//处理下一次激光雷达数据
        }
    }

和上面求J21和J22的时填充A和b类似. 每行已做注释值得注意的就是需要积分激光雷达两帧时间段内的里程计的多次数据 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 进行最小二乘求解，计算b, r_L, r_R
    double b_wheel;//轮间距
    double r_L;//左轮半径
    double r_R;//右轮半径
    
    b_wheel = C.colPivHouseholderQr().solve(S)[0];
    r_L = -J21 * b_wheel;
    r_R = J22 * b_wheel;
    
    cout << "b: " << b_wheel << endl;
    cout << "r_L: " << r_L << endl;
    cout << "r_R: " << r_R << endl;

    return 0;

进行最小二乘求解，计算b, r_L, r_R 完毕 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

Result

一个数据标定结果如下:

资讯详情

轮式里程计与激光雷达进行标定2--里程计运动学模型方法（理论+实现代码）

2-里程计运动学模型法用于校准轮式里程计和激光雷达实现代码）

轮式里程计定义

轮式里程计和激光SLAM的关系

轮式里程计标定前言

轮式里程计和激光雷达标定数学建模

Code

Result

74hc595芯片数码管显示

轮式里程计与激光雷达进行标定2--里程计运动学模型方法（理论+实现代码）

2-里程计运动学模型法用于校准轮式里程计和激光雷达 实现代码）

轮式里程计定义

轮式里程计和激光SLAM的关系

轮式里程计标定前言

轮式里程计和激光雷达标定数学建模

Code

Result

74hc595芯片数码管显示

最近热搜

历史搜索 清除历史记录

2-里程计运动学模型法用于校准轮式里程计和激光雷达实现代码）

历史搜索清除历史记录