毕设笔记

SLAM（同步定位与建图）

SLAM研究体系分类

SLAM通常包括如下几个部分，特征提取，数据关联，状态估计，状态更新以及特征更新等。

SLAM的一般过程

SLAM的核心是EKF。EKF用于结合上述信息估计机器人准确位置。上述选取的特征一般称作地标。EKF将持续不断的对上述机器人位置和周围环境中地标位置进行估计。

ekf，扩展卡尔曼滤波简称,太偏向物理方面了

特征提取

• slam的第一步需要通过测距单元获取机器人周围环境的信息。
  • 具体的方式可以通过激光测距，或者在仿真的时候直接给出
• 机器人自身运动模型：机器人自身位置数据通过对机器人轮胎运行圈数的估计可以得到机器人自身位置的一个估计，其可以被看作EKF的初始估计数据
  • 需要注意需要保证机器人自身位置数据与测距单元数据的同步性。为了保证其同步性，一般采用插值的方法对数据进行前处理
• 特征（即是地标）
  • 地标是环境中易于观测和区分的特征
    • 地标应该可以从不同的位置和角度观察得到
    • 地标应该是独一无二的，从而可以很容易的将底边从其他物体中分辨出来

数据关联

是将不同时刻位置传感器提取到的地标信息进行关联的过程，也成为重观察过程。

• 此处在仿真的时候，可以选择依赖或者不依赖先验知识，直接从图像序列中检测到运动目标，然后进行目标识别

路径规划

所谓机器人的最优路径规划问题就是根据某个或某些优化准则，如工作代价最小，行走路径最短，行走时间最短等准则，在其工作空间中找到一条从起始点到目标点的能避开障碍物的最优路径。

需要解决的问题分为下面三点：

• 始于初始点止于目标点
• 避开障碍
• 尽可能优化路径

常见的路径规划

基于采样的方法、基于节点的方法、基于数学模型的算法、基于生物启发式的算法、和多融合算法。此处重点关注的是基于节点的方法。————可以采用学过的Dijkstra法、A*法

A*法的介绍：也称为栅格法。

1. 通过下面这个函数来计算每个节点的优先级。
   $$
   f(n)=g(n)+h(n)
   $$
   其中：

   • f(n)是节点n的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
   • g(n) 是节点n距离起点的代价。
   • h(n)是节点n距离终点的预计代价，这也就是A*算法的启发函数。关于启发函数我们在下面详细讲解。

2. A*算法在运算过程中，每次从优先队列中选取f(n)值最小（优先级最高）的节点作为下一个待遍历的节点。

3. A*算法使用两个集合来表示待遍历的节点，与已经遍历过的节点，这通常称之为open_set和close_set。

4. ```

   • 初始化open_set和close_set；
   • 将起点加入open_set中，并设置优先级为0（优先级最高）；
   • 如果open_set不为空，则从open_set中选取优先级最高的节点n：
     • 如果节点n为终点，则：
       • 从终点开始逐步追踪parent节点，一直达到起点；
       • 返回找到的结果路径，算法结束；
     • 如果节点n不是终点，则：
       • 将节点n从open_set中删除，并加入close_set中；
       • 遍历节点n所有的邻近节点：
         • 如果邻近节点m在close_set中，则：
           • 跳过，选取下一个邻近节点
         • 如果邻近节点m也不在open_set中，则：
           • 设置节点m的parent为节点n
           • 计算节点m的优先级
           • 将节点m加入open_set中

             5. 启发函数
             
                - Dijkstra算法就是启发函数h(n)=0时的A*算法
                - 如果h(n)始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
                - 如果h(n)完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。
                - 如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。
             
                在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可
             
                这里我们的启发函数采用欧几里得距离：两个节点之间的直线距离

             function heuristic(node) =
             dx = abs(node.x - goal.x)
             dy = abs(node.y - goal.y)
             return D * sqrt(dx * dx + dy * dy)

                
             
             A\*算法的变种：ARA*、D*、Field D*
             
             ## A*算法的优化
             
             ### 估值函数的改进h(n)
             
             ![](https://i.loli.net/2021/01/21/bne26cT41qAd7fZ.png)
             
             ### 估值函数中引入惩罚函数
             
             ## 基于DWA的局部路径规划算法
             
             当移动机器人遇到动态的障碍物时，需要针对突然出现的物体有自主避障的能力--因此需要局部路径规划
             
             ### DWA介绍
             
             [参考文章](https://blog.csdn.net/weixin_37835423/article/details/89683302)
             
             ————动态窗口法
             
             其算法过程主要分为仿真获取机器人的运动轨迹、对轨迹进行评价选择最优轨迹两个主要过程，动态窗口表达的是仿真的运动轨迹数量有限，主要是因为机器人在较短的控制周期内只能达到一定的速度。
             
             #### 获取移动机器人速度样本
             
             对移动机器人的速度空间（v,w）[v表示的是线速度，w表示的是角速度]进行采用，通过建模对采样的速度进行轨迹预测，通过评价函数对预估的轨迹进行评价
             
             #### 生成运动轨迹
             
             根据速度样本确定运动轨迹，是简单运行学知识，主要注意的是用的仿真时间产生的样本还是仿真周期产生的样本。
             
             #### 针对轨迹进行评价并选取最优轨迹

             oscillation_costs_ //振荡代价函数，一旦速度发生振荡，直接丢弃速度样本
             obstacle_costs_ //障碍物代价函数，当轨迹进入障碍物区，直接丢弃当前轨迹样本
             goal_costs_ //目标代价函数，优先选择距离局部目标点近的轨迹
             path_costs_ //路径代价函数，优先选择贴近全局路径的轨迹

```

• 通过仿真轨迹将机器人轮廓映射到全局坐标系下，并对轮廓边上的代价值进行分析，选择最大的代价值作为障碍物代价，从而确定机器人是否会撞到障碍物
• 首先根据全局路径与局部代价地图的边界确定局部目标点，然后依据局部目标点生成栅格地图，每个栅格处的值代表当前栅格到目标点的距离，对于障碍物栅格，直接置为到目标点的距离无穷远。最后再根据轨迹末端点处栅格的位置，直接通过索引在地图中获取该位置距目标点的距离，作为距目标点的代价。因此，目标代价函数的主要任务是生成栅格地图。
• 该代价函数的实现原理与目标代价函数类似，只是该函数是以局部路径上的所有点作为起点向外膨胀来构建栅格地图，并且是以仿真轨迹上的所有点的距离值的和的平均值作为轨迹的代价值。

RRT算法