【粒子滤波定位】（一）移动机器人系统建模

1 运动模型

机器人运动模型描述机器人从前一时刻的状态在控制输入和噪声干扰左右下运动到当前时刻的位姿变化。
机器人的两个驱动轮电机上装有光电编码器，里程计在一定时间内根据光电编码器检测车轮转过的弧度，推算出机器人相对位置和转角的变化。
车轮运动的距离或者弧度：$$\Delta d=2\times (N/p)\times \pi r$$
其中，$r$为车轮半径，光码盘为$p$线/转，$\Delta T$时间内光码盘输出的脉冲数为$N$。

两轮差速驱动机器人轮间距为$a$，$k$时刻机器人位姿$[x_t(k),y_r(k),{\theta }_r(k)]$.
从$k-1$时刻到·$k$时刻机器人移动距离为：$$\Delta D=(\Delta d_L+\Delta d_R)/2$$机器人相对角度为：$$\Delta \theta =(\Delta d_L+\Delta d_R)/a$$
将$u(k)=[\Delta D(k),\Delta \theta (k)]$作为里程计模型的输入，机器人的运动半径可以定义为：$$R(k)=\frac{\Delta D(k)}{\Delta \theta (k)}$$
移动机器人的运动模型可表述为一个离散差分方程：
$$X_r(k)=f[X_r(k-1),u(k)]+w(k)$$
非线性的函数$f$是系统的状态转换函数，$k$时刻的状态为$X_r(k)$；输入控制指令$u(k)$；零均值的高斯白噪声$w(k)$，表示在机器人运动过程中轮子的滑动、系统建模的误差和传感器的误差漂移。

实际应用中，用圆弧模型近似里程计运动：
X r ( k ) = f [ X r ( k − 1 ) , u ( k ) ] + w ( k ) = [ x r ( k − 1 ) + R ( k ) { c o s [ θ r ( k − 1 ) + Δ θ ( k ) ] − c o s [ θ r ( k − 1 ) ] } y r ( k − 1 ) + R ( k ) { s i n [ θ r ( k − 1 ) + Δ θ ( k ) ] − s i n [ θ r ( k − 1 ) ] } θ r ( k − 1 ) + Δ θ ( k ) ] + [ w x w y w θ ] \begin{aligned}X_r(k)&=f[X_r(k-1),u(k)]+w(k)\\&=\begin{bmatrix}x_r(k-1)+R(k)\{cos[\theta_r(k-1)+\Delta{\theta(k)}]-cos[\theta_r(k-1)]\}\\y_r(k-1)+R(k)\{sin[\theta_r(k-1)+\Delta{\theta(k)}]-sin[\theta_r(k-1)]\}\\\theta_r(k-1)+\Delta{\theta(k)}\end{bmatrix}+\begin{bmatrix}w_x\\w_y\\w_{\theta}\end{bmatrix}\\\end{aligned} Xr​(k)​=f[Xr​(k−1),u(k)]+w(k)= ​xr​(k−1)+R(k){cos[θr​(k−1)+Δθ(k)]−cos[θr​(k−1)]}yr​(k−1)+R(k){sin[θr​(k−1)+Δθ(k)]−sin[θr​(k−1)]}θr​(k−1)+Δθ(k)​ ​+ ​wx​wy​wθ​​ ​​

2 传感器分类与感知模型

2.1 传感器的分类

内部传感器：里程计、陀螺仪、加速度计、GPS和方位传感器；用于相对位姿估计。

外部传感器：超声传感器、红外传感器、PSD传感器、接触传感器、视觉传感器和激光测距传感器；用于环境感知、目标识别与跟踪、安全作业。

1. 里程计

机器人车轮上的光电编码器检测出两个轮子各自转过的圈数，然后应用两个轮子圈数的差值通过航迹推算估计出机器人不同时刻的相对位姿。

2. 视觉传感器

主要使用具有较高精确性和稳定性的电荷耦合器件（Charge Coupled Device，CCD），分为单目视觉（Monocular Vision）和立体视觉两种。
双目视觉（Binocilar Vision）和三目视觉（Trinocular Vision）可以计算物体的景深，属于立体视觉。
立体视觉测距根据同一景物在两个或多个摄像头上成像的视差计算出景深，从而获得景物距离信息。

3. 激光测距仪

由激光二极管对准目标发射激光脉冲，通过测量激光脉冲反射时间长短来确定目标距离。
激光经目标反射后向各个方向散射，光学系统接收到一部分散射光后成像到雪崩光电二极管上。
雪崩光电二极管将检测到的极其微弱的光信号进行放大，根据记录从光脉冲发出到接收返回的时间测定目标的距离。

2.2 视觉传感器感知模型

2.3 激光测距仪感知模型

感知量$Z$包含环境特征相对于传感器的距离和方向信息，扫描数据的极坐标表示形式$$Z=[\rho ,\varphi {]}^T$$
扫描数据的直角坐标表示形式
$$Z=[x,y{]}^T$$
感知模型描述了传感器感知量与机器人位姿之间的相互关系
$$Z(k)=h[X_r(k)]+\nu (k)$$
其中，$k$时刻感知量为$Z(k)$;测量函数为$h$;$\upsilon (k)$为描述测量噪声和模型误差的噪声模型。
则激光测距仪的感知模型为
$$Z(k)=\left[\begin{array}{c}\rho (k)\\ \varphi (k)\end{array}\right]=\left[\begin{array}{c}\sqrt{[x_i-x_s(k){]}^2+[y_i-y_s(k){]}^2}\\ arctan[\frac{y_i-y_s(k)}{x_i-x_s(k)}]-{\theta }_s(k)\end{array}\right]+\nu (k)$$

2.4 传感器噪声模型

为了提高传感器感知信息的可靠性，有必要建立一个噪声模型来描述感知信息的不确定性、环境模型的不确定性、决策的不确定性。

高斯噪声模型是最常用的噪声模型。