(11-2)基于SLAM的自主路径导航系统:公用文件

11.3  公用文件

在本项目中,公用文件负责实现一系列方法和类的声明、原型和定义,这些方法和类用于实现随机数生成、向量运算等功能,这些功能在自主探索和路径规划算法中被用到。具体来说,这些文件定义了用于生成随机浮点数、计算向量之间的距离、查找最近点、检查障碍物等功能的方法和类。

11.3.1  数学运算

(1)文件functions.h定义了一些方法原型和一个类,用于在自主探索过程中进行路径规划和环境感知。这些方法提供了处理地图、计算距离、检查障碍物等功能,用于机器人在未知环境中的自主探索和路径规划过程中的一些基本操作。

// rdm类,用于生成随机浮点数
class rdm {
    int i;
public:
    // 构造方法
    rdm();
    // 生成随机数的方法
    float randomize();
};

// Norm方法原型
// 计算向量之间的欧几里得距离
float Norm(std::vector<float>, std::vector<float>);

// sign方法原型
// 返回给定浮点数的符号
float sign(float);

// Nearest方法原型
// 返回给定点集中距离目标点最近的点
std::vector<float> Nearest(std::vector<std::vector<float>>, std::vector<float>);

// Steer方法原型
// 在给定两个点之间的方向上给定距离的点
std::vector<float> Steer(std::vector<float>, std::vector<float>, float);

// gridValue方法原型
// 获取给定点在给定地图中的栅格值
int gridValue(nav_msgs::OccupancyGrid &, std::vector<float>);

// ObstacleFree方法原型
// 检查给定点是否在给定地图上的障碍物自由空间中
char ObstacleFree(std::vector<float>, std::vector<float> &, nav_msgs::OccupancyGrid);

(2)文件functions.cpp实现了头文件functions.h中声明的各个方法的具体功能,这些方法为自主探索和路径规划算法提供了基本的数学计算和功能支持。各个方法的具体功能如下所示。

  1. 类rdm的构造方法和randomize方法:用于生成随机浮点数。
  2. Norm方法:计算两个向量之间的欧几里得距离。
  3. sign方法:返回给定浮点数的符号。
  4. Nearest方法:找到给定点集中距离目标点最近的点。
  5. Steer方法:在指定两个点之间的方向上给定距离内生成一个新点。
// rdm类,用于生成随机浮点数
class rdm {
    int i;
public:
    // 构造方法
    rdm() { i = time(0); }
    // 生成随机数的方法
    float randomize() {
        i = i + 1;
        srand(i);
        return float(rand()) / float(RAND_MAX);
    }
};

// 计算两个向量之间的欧几里得距离
float Norm(std::vector<float> x1, std::vector<float> x2) {
    return pow((pow((x2[0] - x1[0]), 2) + pow((x2[1] - x1[1]), 2)), 0.5);
}

// 返回给定浮点数的符号
float sign(float n) {
    if (n < 0.0) {
        return -1.0;
    } else {
        return 1.0;
    }
}

// 找到给定点集中距离目标点最近的点
std::vector<float> Nearest(std::vector<std::vector<float>> V, std::vector<float> x) {
    float min = Norm(V[0], x);
    int min_index;
    float temp;

    for (int j = 0; j < V.size(); j++) {
        temp = Norm(V[j], x);
        if (temp <= min) {
            min = temp;
            min_index = j;
        }
    }

    return V[min_index];
}

// 在给定两个点之间的方向上给定距离内生成一个新点
std::vector<float> Steer(std::vector<float> x_nearest, std::vector<float> x_rand, float eta) {
    std::vector<float> x_new;

    if (Norm(x_nearest, x_rand) <= eta) {
        x_new = x_rand;
    } else {
        float m = (x_rand[1] - x_nearest[1]) / (x_rand[0] - x_nearest[0]);

        x_new.push_back((sign(x_rand[0] - x_nearest[0])) * (sqrt((pow(eta, 2)) / ((pow(m, 2)) + 1))) + x_nearest[0]);
        x_new.push_back(m * (x_new[0] - x_nearest[0]) + x_nearest[1]);

        if (x_rand[0] == x_nearest[0]) {
            x_new[0] = x_nearest[0];
            x_new[1] = x_nearest[1] + eta;
        }
    }

    return x_new;
}

11.3.2  伪随机数生成器

(1)文件mtrand.h定义了一个名为MTRand_int32的类,实现了Mersenne Twister伪随机数生成器,以及派生类MTRand、MTRand_closed、MTRand_open和MTRand53,分别用于生成不同范围的双精度浮点数。总之,类MTRand_int32提供了种子初始化、生成随机整数和浮点数等功能。

#ifndef MTRAND_H
#define MTRAND_H

class MTRand_int32 { // 梅森旋转随机数生成器
public:
    // 默认构造方法:仅在这是第一个实例时使用默认种子
    MTRand_int32() { if (!init) seed(5489UL); init = true; }
    // 以32位整数作为种子的构造方法
    MTRand_int32(unsigned long s) { seed(s); init = true; }
    // 以大小为32位整数的数组作为种子的构造方法
    MTRand_int32(const unsigned long* array, int size) { seed(array, size); init = true; }
    // 两个种子方法
    void seed(unsigned long); // 使用32位整数种子
    void seed(const unsigned long*, int size); // 使用数组种子
    // 重载操作符()以使其成为生成器(方法对象)
    unsigned long operator()() { return rand_int32(); }
    // 2007-02-11:使析构方法为虚方法;感谢"double more"指出这一点
    virtual ~MTRand_int32() {} // 析构方法
protected: // 由派生类使用,否则不可访问;使用()操作符
    unsigned long rand_int32(); // 生成32位随机整数
private:
    static const int n = 624, m = 397; // 编译时常量
    // 下面的变量是静态的(不存在重复)  
    static unsigned long state[n]; // 状态向量数组
    static int p; // 状态数组中的位置
    static bool init; // 如果调用了init方法则为true
    // 用于生成伪随机数的私有方法
    unsigned long twiddle(unsigned long, unsigned long); // 由gen_state()使用
    void gen_state(); // 生成新状态
    // 使复制构造方法和赋值运算符不可用,它们没有意义
    MTRand_int32(const MTRand_int32&); // 未定义复制构造方法
    void operator=(const MTRand_int32&); // 未定义赋值运算符
};

// 内联以提高速度,因此必须放在头文件中
inline unsigned long MTRand_int32::twiddle(unsigned long u, unsigned long v) {
    return (((u & 0x80000000UL) | (v & 0x7FFFFFFFUL)) >> 1)
        ^ ((v & 1UL) * 0x9908B0DFUL);
    // 2013-07-22: 根据http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/Ierymenko.html对上面的行进行了修改以提高性能
    // 感谢Vitaliy FEOKTISTOV指出这一点
}

inline unsigned long MTRand_int32::rand_int32() { // 生成32位随机整数
    if (p == n) gen_state(); // 需要新的状态向量
    unsigned long x = state[p++];
    x ^= (x >> 11);
    x ^= (x << 7) & 0x9D2C5680UL;
    x ^= (x << 15) & 0xEFC60000UL;
    return x ^ (x >> 18);
}

// 在半开区间[0, 1)中生成双精度浮点数
class MTRand : public MTRand_int32 {
public:
    MTRand() : MTRand_int32() {}
    MTRand(unsigned long seed) : MTRand_int32(seed) {}
    MTRand(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
    ~MTRand() {}
    double operator()() {
        return static_cast<double>(rand_int32()) * (1. / 4294967296.); } // 除以2^32
private:
    MTRand(const MTRand&); // 未定义复制构造方法
    void operator=(const MTRand&); // 未定义赋值运算符
};

// 在闭区间[0, 1]中生成双精度浮点数
class MTRand_closed : public MTRand_int32 {
public:
    MTRand_closed() : MTRand_int32() {}
    MTRand_closed(unsigned long seed) : MTRand_int32(seed) {}
    MTRand_closed(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
    ~MTRand_closed() {}
    double operator()() {
        return static_cast<double>(rand_int32()) * (1. / 4294967295.); } // 除以2^32 - 1
private:
    MTRand_closed(const MTRand_closed&); // 未定义复制构造方法
    void operator=(const MTRand_closed&); // 未定义赋值运算符
};

// 在开区间(0, 1)中生成双精度浮点数
class MTRand_open : public MTRand_int32 {
public:
    MTRand_open() : MTRand_int32() {}
    MTRand_open(unsigned long seed) : MTRand_int32(seed) {}
    MTRand_open(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
    ~MTRand_open() {}
    double operator()() {
        return (static_cast<double>(rand_int32()) + .5) * (1. / 4294967296.); } // 除以2^32
private:
    MTRand_open(const MTRand_open&); // 未定义复制构造方法
    void operator=(const MTRand_open&); // 未定义赋值运算符
};

// 在半开区间[0, 1)中生成53位分辨率的双精度浮点数
class MTRand53 : public MTRand_int32 {
public:
    MTRand53() : MTRand_int32() {}
    MTRand53(unsigned long seed) : MTRand_int32(seed) {}
    MTRand53(const unsigned long* seed, int size) : MTRand_int32(seed, size) {}
    ~MTRand53() {}
    double operator()() {
        return (static_cast<double>(rand_int32() >> 5) * 67108864. +
            static_cast<double>(rand_int32() >> 6)) * (1. / 9007199254740992.); }
private:
    MTRand53(const MTRand53&); // 未定义复制构造方法
    void operator=(const MTRand53&); // 未定义赋值运算符
};

#endif // MTRAND_H

(2)文件mtrand.cpp mtrand.cpp 实现了随机数生成器类Mersenne Twister的成员方法,包括生成新的状态向量、使用 32 位种子初始化、使用数组初始化等功能。

#include "mtrand.h"
// 静态私有成员的初始化
unsigned long MTRand_int32::state[n] = {0x0UL};
int MTRand_int32::p = 0;
bool MTRand_int32::init = false;

// 生成新的状态向量
void MTRand_int32::gen_state() {
    for (int i = 0; i < (n - m); ++i)
        state[i] = state[i + m] ^ twiddle(state[i], state[i + 1]);
    for (int i = n - m; i < (n - 1); ++i)
        state[i] = state[i + m - n] ^ twiddle(state[i], state[i + 1]);
    state[n - 1] = state[m - 1] ^ twiddle(state[n - 1], state[0]);
    p = 0; // 重置位置
}

// 使用32位种子进行初始化
void MTRand_int32::seed(unsigned long s) {
    state[0] = s & 0xFFFFFFFFUL; // 用于 > 32 位机器
    for (int i = 1; i < n; ++i) {
        state[i] = 1812433253UL * (state[i - 1] ^ (state[i - 1] >> 30)) + i;
        state[i] &= 0xFFFFFFFFUL; // 用于 > 32 位机器
    }
    p = n; // 强制下一次生成随机数调用 gen_state()
}

// 使用数组进行初始化
void MTRand_int32::seed(const unsigned long* array, int size) {
    seed(19650218UL);
    int i = 1, j = 0;
    for (int k = ((n > size) ? n : size); k; --k) {
        state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1664525UL)) + array[j] + j; // 非线性
        state[i] &= 0xFFFFFFFFUL; // 用于 > 32 位机器
        ++j;
        j %= size;
        if ((++i) == n) {
            state[0] = state[n - 1];
            i = 1;
        }
    }
    for (int k = n - 1; k; --k) {
        state[i] = (state[i] ^ ((state[i - 1] ^ (state[i - 1] >> 30)) * 1566083941UL)) - i;
        state[i] &= 0xFFFFFFFFUL; // 用于 > 32 位机器
        if ((++i) == n) {
            state[0] = state[n - 1];
            i = 1;
        }
    }
    state[0] = 0x80000000UL; // MSB 为 1,确保初始数组非零
    p = n; // 强制下一次生成随机数调用 gen_state()
}

  • 3
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

码农三叔

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值