Robotics: Estimation and Learning.WEEK 3

${\Large WEEK \qquad 3}$WEEK3

3.1 关于建图

$\qquad$机器人的地图就是其所处的环境。建立一个地图的过程叫做mapping(地图构建)。根据需要解决的问题建立相应的地图:
$\qquad$ 1.使用何种坐标系表示，需要包含何种信息
$\qquad$ 2.可使用的传感器
$\qquad$ 3.机器人的需求
地图的类型:
$\qquad$ 1.尺度地图，位置表示为坐标值，作为移动机器人在自我定位时使用的坐标系。
$\qquad$ 2.拓扑地图，在图中的位置表示为节点，它们之间的连接表示为弧。弧被用来表述节点间的连接代价或者是限制条件，这种图在轨迹规划测验中很有用。
$\qquad$ 3.语义地图，与之前最重要的不同点是标签以及标注对象间的相对位置，可以描述对象的位置，而不使用具体的坐标。
$\qquad$之后将要讨论的地图都是尺度地图（下文中的栅格地图），建图所需解决的技术问题：
$\qquad$ 1.在有噪声的测量中估计目标状态
$\qquad$ 2.测量往往在相对坐标系下进行，需要转化为绝对坐标系
$\qquad$ 3.其他机器人问题如轨迹规划与导航等，地图需要实时更新

3.2 构建栅格地图

3.2.1 定义

$\qquad$“占据”被定义为一个二进制随机变量，随机变量是对真实事物采样空间的函数。“占据”被定义在有两种可能状态的概率空间中包括空闲和被占据。
$m_{x,y}:\{free,occupied\}\rightarrow\{0,1\}\qquad(1)$mx,y​:{free,occupied}→{0,1}(1)即随机变量的取值包括0和1。
$\qquad$占据栅格地图就是占据变量组成的数组，其中每格都是随机占据变量。
$图1\ 占据栅格地图$图1 占据栅格地图占据栅格地图使用贝叶斯滤波器生成。贝叶斯滤波器意味着地图递归更新。
$\qquad$某个事件(X)发生的概率(p(X))的比例可以记为赢率:
$Odd:=\frac{(X\ happens)}{(X\ not \ happens)}=\frac{p(X)}{p(X^{c})}\qquad(2)$Odd:=(X not happens)(X happens)​=p(Xc)p(X)​(2)$\qquad$使用赢率可以带来许多便利。

3.2.2 测量

$\qquad$占据栅格算法往往利用一个距离传感器提供距离信息，在地图中每个栅格都只有两种可能的测量结果开放(0)或者被占据(1)。
$图2\ 距离传感器$图2 距离传感器$\qquad$测量结果中浅蓝色的栅格表示光线可以通过，即为开放栅格。黄色的栅格表示光线不能通过，即为被占据栅格。
$z\rightarrow\{0,1\}\qquad(3)$z→{0,1}(3)测量模型
$p(z|m_{x,y})\qquad(4)$p(z∣mx,y​)(4)测量中仅有的四种条件概率
$p(z=1|m_{x,y}=1)：正确占据测量\\ p(z=0|m_{x,y}=1)：错误开放测量\\ p(z=1|m_{x,y}=0)：错误占据测量\\ p(z=0|m_{x,y}=0)：正确开放测量$p(z=1∣mx,y​=1)：正确占据测量p(z=0∣mx,y​=1)：错误开放测量p(z=1∣mx,y​=0)：错误占据测量p(z=0∣mx,y​=0)：正确开放测量$\qquad$依据贝叶斯法则，通过先验、测量模型(似然估计)和归一化参数($\eta$η)推算后验。$\mathop{p(m_{x,y}|z)}\limits^{后验概率}=\frac{ \mathop{p(z|m_{x,y})}\limits^{似然估计} \mathop{p(m_{x,y})}\limits^{先验概率}}{p(z)}\qquad(5)$p(mx,y​∣z)后验概率​=p(z)p(z∣mx,y​)似然估计​p(mx,y​)先验概率​​(5)$\qquad$设当实际的状态为被占据状态时，其赢率形式为：$Odd((m_{x,y}=1)given\ z)=\frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}\qquad(6)$Odd((mx,y​=1)given z)=p(mx,y​=0∣z)p(mx,y​=1∣z)​(6)$\qquad$应用条件概率，可以包含进传感器概率模型。
$p(m_{x,y}=1|z)=\frac{p(z|m_{x,y}=1)p(m_{x,y}=1)}{p(z)}\qquad(7)$p(mx,y​=1∣z)=p(z)p(z∣mx,y​=1)p(mx,y​=1)​(7)写成赢率形式:$Odd：\frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=\frac{p(z|m_{x,y}=1)p(m_{x,y}=1) /p(z)}{p(m_{x,y}=0|z)}=\frac{p(z|m_{x,y}=1)p(m_{x,y}=1) }{p(z|m_{x,y}=0)p(m_{x,y}=0) }\qquad(8)$Odd：p(mx,y​=0∣z)p(mx,y​=1∣z)​=p(mx,y​=0∣z)p(z∣mx,y​=1)p(mx,y​=1)/p(z)​=p(z∣mx,y​=0)p(mx,y​=0)p(z∣mx,y​=1)p(mx,y​=1)​(8)取对数形式$Log-Odd:log\frac{p(m_{x,y}=1|z)}{p(m_{x,y}=0|z)}=log\frac{p(z|m_{x,y}=1)p(m_{x,y}=1) }{p(z|m_{x,y}=0)p(m_{x,y}=0) }=log\frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)}+log\frac{p(m_{x,y}=1)}{p(m_{x,y}=0) }\qquad(9)$Log−Odd:logp(mx,y​=0∣z)p(mx,y​=1∣z)​=logp(z∣mx,y​=0)p(mx,y​=0)p(z∣mx,y​=1)p(mx,y​=1)​=logp(z∣mx,y​=0)p(z∣mx,y​=1)​+logp(mx,y​=0)p(mx,y​=1)​(9)得
$log\ odd^{+}=log\ odd\ meas+log\ odd^{-}\qquad(10)$log odd+=log odd meas+log odd−(10)由对数特性右边相乘的两项变成相加。$图3\ 栅格地图对数赢率变化$图3 栅格地图对数赢率变化$\qquad$栅格地图存储了每一个栅格的对数赢率，测量模型也变成对数赢率。地图的更新计算变成了求和对数赢率。使用更新法则是需注意两点，第一，只更新观察到的栅格。第二，更新后的值变成了先验信息，整个更新法则是递归的。
$\qquad$测量模型为：
$log\frac{p(z|m_{x,y}=1)}{p(z|m_{x,y}=0)}\qquad(11)$logp(z∣mx,y​=0)p(z∣mx,y​=1)​(11)$\qquad$考虑两种情况：
$cells\ with\ z=1\quad log\ odd\downarrow occ:=log\frac{p(z=1|m_{x,y}=1)}{p(z=1|m_{x,y}=0)}\qquad(12)\\ cells\ with\ z=0\quad log\ odd\downarrow free:=log\frac{p(z=0|m_{x,y}=1)}{p(z=0|m_{x,y}=0)}=-log\frac{p(z=0|m_{x,y}=0)}{p(z=0|m_{x,y}=1)}\qquad(13)\\$cells with z=1log odd↓occ:=logp(z=1∣mx,y​=0)p(z=1∣mx,y​=1)​(12)cells with z=0log odd↓free:=logp(z=0∣mx,y​=0)p(z=0∣mx,y​=1)​=−logp(z=0∣mx,y​=1)p(z=0∣mx,y​=0)​(13)设测量模型为：
$log\ odd\downarrow occ:=0.9$log odd↓occ:=0.9
$log\ odd\downarrow free:=-0.7$log odd↓free:=−0.7
$图4\ 地图栅格对数赢率初始化为0$图4 地图栅格对数赢率初始化为0$\qquad$即$t_{0}$t0​时刻$log\ odd=0\ for\ all(x,y)$log odd=0 for all(x,y)。这种初始化等价于栅格被占据和开放的概率是一样的。
$图5\ 测量之后地图栅格对数赢率由t_{0}到t_{1}变化$图5 测量之后地图栅格对数赢率由t0​到t1​变化$\qquad$ 栅格对数赢率更新为：
$cells\ with\ z=1\quad log\ odd=0+0.9\\ cells\ with\ z=0\quad log\ odd=0-0.7$cells with z=1log odd=0+0.9cells with z=0log odd=0−0.7$图6\ 测量之后地图栅格对数赢率由t_{1}到t_{2}变化$图6 测量之后地图栅格对数赢率由t1​到t2​变化$\qquad$栅格对数赢率更新为：$cells\ with\ z=1\quad log\ odd=0+0.9\\ cells\ with\ z=0\quad log\ odd=-0.7-0.7 (两次都测量到)\\ cells\ with\ z=0\quad log\ odd=0-0.7 (只有一次测量到)\\$cells with z=1log odd=0+0.9cells with z=0log odd=−0.7−0.7(两次都测量到)cells with z=0log odd=0−0.7(只有一次测量到)$\qquad$假设机器人姿态已知，并在地图中将它定位，然后就要讨论如何将机器人坐标系下的传感器数据变换到全局地图坐标下。
$图7\ 建立坐标系$图7 建立坐标系$\qquad$已知机器人在全局坐标系中的位置为$(x_{1},x_{2})$(x1​,x2​)，传感器方向与$x_{1}$x1​之间的角度为$\theta$θ，传感器探测距离为的d。
$图8\ 将机器人置于坐标系$图8 将机器人置于坐标系$\qquad$求$(x_{1,occ},x_{2,occ})$(x1,occ​,x2,occ​):
$\begin{bmatrix} x_{1,occ} \\ x_{2,occ} \end{bmatrix}=\begin{bmatrix} cos\theta & sin\theta\\ -sin\theta & cos\theta \end{bmatrix}\begin{bmatrix} d\\ 0 \end{bmatrix}+\begin{bmatrix} x_{1}\\ x_{2} \end{bmatrix}\quad(14)$[x1,occ​x2,occ​​]=[cosθ−sinθ​sinθcosθ​][d0​]+[x1​x2​​](14)求出目标点的在全局坐标系的位置。
$图9\ 连续地图转化为栅格地图$图9 连续地图转化为栅格地图$\qquad$连续坐标除以单位栅格长度后取整。
$i=ceil(x/r)\qquad(15)$i=ceil(x/r)(15)
$图10\ 坐标转化为栅格坐标$图10 坐标转化为栅格坐标$\begin{bmatrix} i_{1,occ} \\ i_{2,occ} \end{bmatrix}=ceil(\frac{1}{r}\begin{bmatrix} x_{1,occ} \\ x_{2,occ} \end{bmatrix})\qquad(16)$[i1,occ​i2,occ​​]=ceil(r1​[x1,occ​x2,occ​​])(16)$\qquad$得出目标点栅格坐标。