【SLAM入门】概率机器人中的一些重要概念（1）

特龙. 概率机器人. 机械工业出版社, 2017. Print. 国际电气工程先进技术译丛.

第一章：绪论

1. 机器人学的不确定性

   • 机器人环境 robot environment
   • 传感器的量程和分辨率：测量噪声
   • 机器人的执行机构 robot actuation：控制噪声
   • 机器人软件：对世界进行抽象表达的内部模型 internal model

2. 概率机器人学 probabilistic robotics：用概率理论的运算去明确地表示不确定性，用概率算法来表示在整个推测空间的概率分布信息。

   • 移动机器人定位 mobile robot localization：相对外部参考新来估计机器人坐标的问题。给定环境地图，机器人参照传感器数据，定位自己在地图上的相对位置。全局定位 global localization：机器人被放置在已知的环境中的某处，然后从头开始确定自己的位置。概率范式通过在整个位置空间上的一个概率密度函数爱表示机器人的瞬时置信度。

   • 机器人规划和控制：概率算法能计算瞬间不确定性，也能预知未来的不确定性，并在决定正确的控制选择时，对未来不确定性进行考虑。

     

3. 概率机器人学的优势：

   • 将模型和传感器数据进行了无缝集成，同时克服了两者的局限性。

第二章：递归状态估计

1. 状态估计：目的在于从传感器数据中找回状态变量，概率状态估计算法在可能的状态空间上计算置信度分布。

2. 机器人环境交互：环境 environemnt 或世界 world 是拥有内部状态的动态系统。

   • 状态 state
     • 机器人位姿 pose：机器人相对于全局坐标系的位置和方向。刚性移动机器人有六个状态变量，平面上刚性机器人位姿由三个变量确定。
     • 机器人执行机构配置 configuration of the robot’s actuators：运动学状态 kinematic state
     • 动态状态 dynamic state：机器人速度和角速度
     • 环境中周围物体的位置和特征：物体假设为地标 landmark形式，它们是可以被可靠识别的明显的固定的环境特征。
   • 连续状态空间：机器人位姿，即相对于外部坐标系的位置和方向。
   • 环境交互：机器人通过执行机构影响环境的状态，同时它通过传感器收集有关状态的信息。
     • 环境传感器测量 measurement：又称观察 oberseration 或认知 percept，传感器测量一般有延迟，因此他们提供的是之前的状态信息。
     • 控制动作改变世界的状态：控制动作 control action 的实例包括机器人运动和物体的操纵。
     • 控制数据携带环境中关于状态改变 change of state 的信息
     • 控制数据的另一个来源是里程表 odometer：里程计是测量机器人轮子运行的传感器，虽然是传感器，但仍将里程计视为控制数据，因为它们测量了控制动作的影响。
   • 测量与控制的不同：
     • 环境感知提供了环境状态的信息，因此往往会增加机器人的信息
     • 由于机器人执行机构的固有噪声和机器人环境的随机性，运动往往会引起信息的丢失。

第五章：机器人运动模型（预测方程 $x_t=f(x_{t-1})+Q$)

1. 运动学构型 kinematics configuration：运动学 kinematics 是描述控制行为对机器人构型产生影响的微积分。

   运动模型 motion model ：由状态转换概率$p(x_t|u_t,x_{t-1})$构成，控制结果将用这个后验概率来描述，这个模型描述了对$x_{t-1}$执行运动控制$u_t$后，机器人取得的运动学状态的后验分布，其中$u_t$有时由机器人的里程计提供。

2. 两种特定的概率运动概率模型$p(x_t|u_t,x_{t-1})$：

   • 速度运动模型：假定运动数据$u_t$指定了机器人电动机的速度指令，许多商业移动机器人（差分驱动、同步驱动）是由独立的平移和旋转速度驱动。规划算法（如防撞）必须预测运动的影响，因此速度运动模型主要用于概率运动规划。
   • 里程计模型：假设机器人具有测距信息，大多数商业平台具有里程计使用运动信息（走过的距离、旋转的角度）提供测距。里程计信息仅仅在执行完运动控制后才能获得，因此里程模型主要用于估计。

3. 速度运动模型 velocity motion medel：认为可以通过两个速度（一个旋转速度和一个平移速度）来控制机器人。驱动系统通常是通过这种方式控制的，包括差分驱动、阿克曼驱动和同步驱动。

   用$v_t$表示t时刻的平移速度 translational velocity，用${\omega }_t$表示旋转速度 ratational velocity，并规定逆时针旋转（左转）为正；向前运动为正。

   $u_t=(v_t,{\omega }_t{)}^T$

   无误差基于速度信息计算预测$p(x_t|u_t,x_{t-1})$

输入：

• 初始位姿 $x_{t-1}=(xy\theta {)}^T$
• 控制 $u_t=(vw{)}^T$
• 假想的后继位姿 $x_t=(x^{\prime }{y}^{\prime }{\theta }^{\prime }{)}^T$

输出：后验概率 $p(x_t|u_t,x_{t-1})$

固定时间常数$\Delta t$，参数${\alpha }_1\sim {\alpha }_6$是机器人特有的运动误差参数。

函数$prob(x,b^2)$建立了运动误差的模型，它计算参数x以零为中心，方差为$b^2$的随机变量下的概率。

粒子滤波采样算法计算速度运动模型后验概率：对于固定的控制$u_t$和位姿$x_{t-1}$由$p(x_t|u_t,x_{t-1})$ 产生随机样本。

输入：

• 初始位姿 $x_{t-1}=(xy\theta {)}^T$

• 控制 $u_t=(vw{)}^T$

输出：最终姿态$x_t$

采样提议分布$sample(b^2)$产生来自具有方差$b^2$以零为中心的分布的随机的样本

采样$x_t$比计算给定$x_t$的密度要容易，这是因为采样仅需一个物理运动模型的前向仿真。对于第一个程序，计算一个假想位姿的概率意味着误差参数的重新推测，这需要计算物理运动模型的逆。

4. 里程计运动模型 odometry motion model：整合轮子的编码信息，用距离测量代替控制。里程计法只有在机器人移动后才是可用的。里程计模型使用相对运动信息 relative motion information，该信息由机器人内部里程计测量。在时间间隔$(t-1,t]$内，机器人从位姿$x_{t-1}$前进到$x_t$，里程计反馈了从${\overline{x}}_{t-1}=(\overline{x}\overline{y}\overline{\theta })$到 ${\overline{x}}_t=({\overline{x}}^{\prime }{\overline{y}}^{\prime }{\overline{\theta }}^{\prime })$的相对前进，帽子代表是嵌在机器人内部坐标的。在状态估计中，${\overline{x}}_{t-1}$和${\overline{x}}_t$的相对差是真实位姿$x_{t-1}$和$x_t$之间差异的一个很好的估计器。

   运动信息$u_t=({\overline{x}}_{t-1},{\overline{x}}_t{)}^T$，为了提取相对距离，$u_t$被转变成三个步骤的序列：初始旋转${\delta }_{rot1}$、直线运动（平移）${\delta }_{trans}$和另一个旋转${\delta }_{rot2}$，每对位置$(\overline{s},{\overline{s}}^{\prime })$都有唯一的参数向量$({\delta }_{rot1},{\delta }_{trans},{\delta }_{rot2}{)}^T$确定其之间的相对运动，因此这三个量一起组成里程计编码的相对运动的统计量。

里程计模型计算预测$p(x_t|u_t,x_{t-1})$

输入：

• 初始位姿$x_{t-1}$
• 从里程计获得的一对位姿$u_t=({\overline{x}}_{t-1},{\overline{x}}_t{)}^T$
• 假定的最终姿态$x_t$

输出：后验概率 $p(x_t|u_t,x_{t-1})$

粒子滤波采样算法计算里程计模型后验概率：将初始姿态$x_{t-1}$和里程计读数$u_t$作为输入，输出一个随机$x_t$服从 $p(x_t|u_t,x_{t-1})$ 的分布，与前面的算法不同，它随机地推测一个位姿$x_t$，而不是计算给定$x_t$的概率。

输入：

• 从里程计获得的一对位姿$u_t=({\overline{x}}_{t-1},{\overline{x}}_t{)}^T$
• 初始位姿$x_{t-1}$

输出：最终姿态$x_t$

5. 运动和地图：前面提到的模型是在缺乏环境特性信息的情况下描述机器人运动的。总是给定一个地图m，它包括关于机器人能或不能通过的空间的信息。例如，占用地图 occupancy maps。机器人的位姿必须一直在闲置区域。这种考虑需要运动模型考虑地图m，即 $p(x_t|u_t,x_{t-1},m)$

   $p(x_t|u_t,x_{t-1})\ne p(x_t|u_t,x_{t-1},m)$

   $p(x_t|u_t,x_{t-1},m)$ 作为基于地图的运动模型 map-based motion model，指被放置在地图m所表示的环境里的机器人在位姿$x_{t-1}$处执行动作$u_t$到达位姿$x_t$的可能性。

   地图运动模型的近似：
   $$p(x_t|u_t,x_{t-1},m)=\eta \frac{p(x_t|u_t,x_{t-1})p(x_t|m)}{p(x_t)}$$
   其中，无地图估计$p(x_t|u_t,x_{t-1}) $，$p(x_t|m)$表达了位姿$x_t$和地图m的一致性。对于占用地图，当且仅当机器人被放置在地图的占用栅格单元的时候才有$p(x_t|m)=0$，否则假定$p(x_t|m)$是一个常数值。

   

第六章：机器人感知 更新步 $z_t=h(x_t)+R$

1. 环境测量模型 environment measurement models：描述在客观世界生成传感器测量的过程。概率机器人为传感器测量噪声建立模型，考虑传感器固有的不确定性。形式上，测量模型定义为一个条件概率分布$p(z_t|x_t,m)$，即似然。

   移动机器人感知：

   • 声呐测距扫描 sonar range scan，对于声呐，不能可靠地测量到附近物体的距离经常被理解成是传感器噪声。

• 激光测距扫描 laser range scan：基于飞行时间ToF，主动发出激光光束信号，并记录其回波。与声呐不同，激光提供更集中的激光束。

概率机器人要包容传感器模型的随机误差：**将测量过程建模为一个条件概率密度$p(z_t|x_t)$，而不是一个确定的函数$z_t=f(x_t)$，传感器模型的不确定性可以被包含在模型的不确定性部分。**概率机器人的优势就是在于可以用相对粗糙的模型。

传感器可能产生不止一个测量值，即$z_t=\{z_t^1,...,z_t^k\}$，其中$z_t^k$表示某一个独立的测量（一个测距值）。则概率
$$p(z_t|x_t,m)=\prod _{k=1}^{k}p(z_t^k|x_t,m)$$
即相当于每个独立测量束噪声之间存在独立性假设 independence assumption。

2. 地图 map：环境的地图就是环境中物体的列表及其属性（如位置）。
   $$m=\{m_1,m_2,...,m_N\}$$
   其中N为环境中物体的总数，每一个$m_n$指定一个属性。

   • 基于特征的地图 feature-based：n是一个特征的索引，$m_n$包括特征的属性和特征的笛卡尔位置。
     • 仅指明在指定位置（地图中包含的对象的位置）的环境的形状。
   • 基于位置的地图 location-based：索引号n与特定的位置相对应，平面地图中用$m_{x,y}$代替$m_n$来表示一个地图元素。
     • 有体积的 volumetric：这种地图为环境中的许多位置都提供标签。体积地图不仅包括物体的信息，也包括了对象没有物体的信息（如闲置空间）。
     • 占用栅格地图 occupancy grid map：为每一个x-y坐标分配一个二值占用值，指明一个位置是否被一个对象占用。

3. 测距仪波束模型：测距仪测量到附近物体的距离，沿着一个个波束测量距离。

   测量算法：测量噪声包括测量噪声、意外对象引起的误差、未检测到对象引起的误差、随机意外噪声。因此期望模型$p(z_t|x_t,m)$是四个密度的混合，每一种密度都与一个特定类型的误差有关。

   • 具有局部测量噪声 measurement noise的正确范围：由测距传感器有限分辨率、大气对测量信号的影响等引起。通常由一个窄的均值为$z_t^{k\ast }$、标准偏差为${\sigma }_{hit}$的高斯建模。用$p_{hit}$代表高斯。

     建立模型：测距传感器的测量值位于区间$[0,z_{max}]$，其中$z_{max}$表示最大传感器距离。因此，测量概率
     $$p_{hit}(z_t^k|x_t,m)=\{\begin{array}{rl}& \eta N(z_t^k;z_t^{k\ast },{\sigma }_{hit}^2)0\le z_t^k\le z_{max}\\ & 0其他\end{array}$$
     其中$z_t^{k\ast }$从$x_t$和m经过射线投射计算，$N(z_t^k;z_t^{k\ast },{\sigma }_{hit}^2)$为具有均值$z_t^{k\ast }$标准偏差为${\sigma }_{hit}$的单变量正态分布。

• 意外对象：地图中不包含的对象可能引起测距仪产生短距离数据，典型例子是与机器人共同移动的人。

  处理办法：

  • 作为状态向量的一部分来估计它们的位置
  • 作为传感器噪声处理

  建立模型：指数分布描述，用测量模型固有参数${\lambda }_{short}$。
  $$p_{short}(z_t^k|x_t,m)=\{\begin{array}{rl}& \eta {\lambda }_{short}{e}^{-{\lambda }_{short}{z}_k^t}0\le z_t^k\le z_t^{k\ast }\\ & 0其他\end{array}$$

• 传感器检测失败 sensor failure：环境中的障碍被完全忽略，如声呐传感器遇到了镜面反射，如激光测距仪检测到黑色吸光的对象时或者某些激光系统在明媚的阳光下测量物体时。

  典型结果：出现最大测量距离问题 max-range measurement，即传感器返回他的最大允许值$z_{max}$。

  建立最大测量范围的模型：以$z_{max}$为中心的离散点群分布
  $$p_{max}(z_t^k|x_t,m)=I(z=z_{max})=\{\begin{array}{rl}& 1z=z_{max}\\ & 0其他\end{array}$$
  其中I是指示函数，当其参数为真时取值为1，否则取0。

• 无法解释的测量 unexplainable measurements：如当超声波被几面墙反弹或者它们受到不同传感器之间的串扰时，声呐经常产生幻读

  建立模型：在区间$[0,z_{max}]$的均匀分布
  $$p_{rand}(z_t^k|x_t,m)=\{\begin{array}{rl}& \frac{1}{z_{max}}0\le z_t^k\le z_{max}\\ & 0其他\end{array}$$

• 对上述四种分布通过四个参数进行加权平均混合$z_{hit}+z_{short}+zmax+z_{rand}=1$,

得到典型密度分布图

计算扫描$z_t$的似然的算法

固有模型参数调节：选择传感器模型参数，包括混合参数$z_{hit},z_{short},z_{max},z_{rand}$，以及参数${\sigma }_{hit}.{\lambda }_{short}$，所有参数记为$\Phi$。利用极大似然法 Maximum Likelihood, ML，具体方法为期望值极大化 Expectation Maximization, EM。

测距仪波束模型实际考虑：计算所有传感器的读书的密度非常复杂，如激光测距仪经常每扫描一次返回几百个值，扫描速率每秒几次，因此不能总是实时地将整个扫描综合到当前的置信度中。解决此问题的典型方法是：设置节流阀 throttle，仅综合所有测量的小子集，如每次激光扫描测距只取8个等间隔测量，而不是360个。这样做还有额外好处，因为一次测距扫描中的相邻波束往往不独立，这样做也会使其不易收到相邻测量的相关噪声的影响。

波束模型的主要计算时间的消耗是射线投射操作。

4. 测距仪的似然域模型：将障碍物检测的似然描述成全局x-y坐标的函数，叫做似然域。首先将传感器扫描终点$z_t$映射到地图的全局坐标空间。设机器人t时刻位姿$x_t=(xy\theta {)}^T$，与机器人固连的传感器局部坐标系位置$(x_{k,sens}{y}_{k,sens})$，传感器波束相对于机器人航向的角度${\theta }_{k,sens}$。测量的终点经过三角变换映射到全局坐标系，其中当$z_t^k=z_{max}$时此坐标没有意义，被抛弃。

与波束模型类似，假定三种噪声和不确定性来源

• 测量噪声：测量过程引起的噪声使用高斯进行建模。令dist为测量坐标$(x_{z_t^k}{y}_{z_t^k}{)}^T$与地图m上最近物体之间的欧氏距离，则传感器测量概率可以由一个以0为中心的高斯函数给出
  $$p_{hit}(z_t^k|x,m)={ϵ}_{{\sigma }_{hit}}(dist)$$
  如具有三个障碍物的环境实例，位置越亮，用测距仪测量到一个物体的可能性越大。

  
  

• 测量失败：点群分布$p_{max}$进行建模

• 无法解释的随机测量：均匀分布$p_{rand}$建模

• 基于波束传感器模型，期望概率$p(z_t^k|x_t,m)$集成三种分布
  $$z_{hit}{p}_{hit}+z_{hit}{p}_{rand}+z_{max}{p}_{max}$$

似然域计算测量概率算法

5. 基于相关性的测量模型：地图匹配 map matching将扫描转换为占用地图 occupancy map，地图匹配方法通常是将少量的连续扫描编制到局部地图 local map上，用$m_{local}$表示。

6. 基于特征的测量模型：从高维传感器测量中提取较少数量的特征，大大减少计算复杂性。

   处理地标的通用模型利用距离和方位传感器 range and bearing sensors，假设任何来自测距扫描的局部特征都伴随着有距离和方位信息。假定签名可以使一个数量值，也可以是一个表征观测到地标类型的整数，或者用一个多维向量表征一个地标（高度和颜色）。用r表示距离，$\Phi$表示方位，s表示签名，则特征向量由一个三维的集合给定。