摘要：
实时定位与地图构建（SLAM）在自主移动机器人研究中是一个重要的课题，SLAM问题最通用的解决方案是EKF-SLAM和FAST-SLAM，前者需要一个精确的过程和观测模型同时该方法存在线性化方面的问题，而后者不适用于实时实现。因此，本文提出了一种新的基于平滑可变结构滤波器（SVSF-SLAM）算法的解决方案用于解决地面无人车辆（UGV）的SLAM问题。SVSF滤波器是一种预测-校正格式的滤波器，具有在参数不确定和模型存在误差情况下的鲁棒性而且不需要对噪声特性进行任何假设。本文通过利用里程计和激光数据构建环境地图并将UGV定位于这个地图中来实现SVSF-SLAM算法。本文将该算法与EKF-SLAM算法进行了比较和验证，相比于EKF-SLAM算法，SVSF-SLAM取得了良好的效果，尤其当噪声是有色的或受到可变偏差（variablebias）的影响时，这些结论证实了SVSF-SLAM算法的有效性。
1.概述
实时定位与地图构建（SLAM）是一个使移动机器人能够在未知环境中定位和建立环境地图的过程，这个过程只使用传感器检测到的最相关的特征信息。SLAM是UGV在未知环境中运行时必备的能力，在这种未知环境下全局准确的位置数据（例如GPS）是无法提供的。当前，移动机器人的发展已经展现巨大的研究前景，特别是在远程行驶、危险环境或者有人驾驶太过昂贵的情况。如果机器人要在外部环境自主操作，如海面以下、地下以及在其他星球的表面，他们必须能够建立地图并且根据这些地图实现可靠地导航。SLAM问题的解决方案被很多人认为是实现机器人完全自主行驶的一个关键前提，这个问题在过去十年中一直是重点研究的对象，SLAM问题也在机器人学术研究中也引起了极大的关注。SLAM解决了移动机器人先前在环境中移动时没有地图可用的问题。机器人对其自身运动和环境特征进行相对的观察，但是观察到的信息都受到了噪声的破坏，SLAM的目标是重建全局地图同时建立机器人的行驶路径。
1986年Smith和Cheeseman在一篇开创性的论文中介绍了SLAM问题的主要方法，并首先被Moutarlier 和Chatila发展成为一个可实施的系统。这种方法使用扩展卡尔曼滤波（EKF）估计机器人的后验姿态和地图。EKF-SLAM在不同环境下已经有几种不同的实现方法，如室内、水下和户外。EKF将SLAM后验估计近似为一个基于所有地图和机器人姿态特征得到的高维高斯，由于机器人状态和地标位置的高维高斯近似而导致的单假设数据关联与二次复杂性，使得协方差矩阵的非对角元素数量非常大。这导致了高复杂性和计算成本的增加，在大多数情况下会导致滤波器分散。此外，EKF协方差矩阵是地图尺寸的二次方，并且更新它们需要的时间是地标数量的二次方。而且，当大量的地标存在于环境中时，计算几乎是不可能短时间实现的。另一种方法使用无迹卡尔曼滤波（UKF）的原理进行估计，它是在N维中使用2N+1个样本的高斯随机变量表示，这些样本称为Sigma点。这种表示利用矩阵平方根和协方差定义的属性来选择这些点，这样就使它们具有与高斯近似相同的协方差。UKF产生的结果相当于一个状态模型的三阶泰勒级数展开式，然而扩展卡尔曼滤波器（EKF）只具有一阶线性准确性。无迹变换方法的另一个优点是可以以非线性的方式处理噪声以解决非高斯或非加性噪声问题。虽然无法避免但UKF受到的线性化问题较少，跟EKF一样，UKF也无法在质量较差的地标环境中使用。
还有一种采用多重假设数据关联和对数复杂性代替平方的算法，这种方法被称为FAST-SLAM，它利用Rao-Blackwellised粒子滤波器有效地解决了SLAM问题。使用FAST-SLAM算法，后验估计是基于机器人的姿势和地标位置得到的。FAST-SLAM算法成功实现了几千个地标的处理而对比EKF-SLAM只能处理几百个地标，因而FAST-SLAM显示出相当大的优势。除此之外，还有另一种新颖的算法叫做平滑可变结构滤波器（SVSF）。SVSF是一种相对较新的基于滑模理论的预测-校正估计方法。2007年，基于可变结构理论和滑动模式概念的平滑可变结构滤波器（SVSF）开始被提出。它实现了用一个可切换增益将预估值收敛在真实状态范围内，这个范围称为存在子空间。正如本文提到的，SVSF为模型的不确定性和误差提供了一个鲁棒的、稳定的估计值，这在当系统模型未被良好定义或用户已知时是很有利的。
平滑可变结构滤波器（SVSF）是一种新的基于滑模理论的预测-校正滤波器，常用于状态和参数估计，具有模型不确定环境下的鲁棒性和稳定性。这些特性使它适用于无人地面车辆（UGV）的定位和地图匹配问题。此外，SVSF是一个鲁棒的递归预测-校正方法，可以有效地处理与初始条件和里程计/激光系统模型误差相关的不确定性。在参数估计中，EKF-SLAM策略对模型不确定性是非常敏感的，容易受到模型不稳定性的影响。SVSF-SLAM算法在应用于不确定系统中时保留了SVSF的近似最佳性能，除此之外，它还能够显著改进估计过程的鲁棒性。
本文的结构如下：
第2节描述了UGV的过程模型；
第3节提出了观测模型；
第4节描述的EKF-SLAM算法；
第5节详细介绍了SVSF滤波器；
第6节提出了SVSF-SLAM算法；
最后，仿真结果，实验结论在第7和第8节中分别介绍。
图1.UGV（Pioneer 3-AT）模型，装备有一个SICK2D 激光雷达测距仪（LMS-200）
图2. UGV模型在全局坐标系中的表示
2.UGV的过程模型
本文中使用的先锋3-AT（Pioneer3-AT）机器人如图（1）所示，这是一个四轮非完整机器人。让包含(Xr Yr)的向量[Xr Yr θr]作为机器人在全局坐标系中的坐标，θr作为机器人在全局坐标系中的方向。非完整约束公式如下：
由图（2）可得，机器人的状态转移方程是:
移动机器人的运动模型和测量输出模型用于描述机器人和地标在下一个时间步的运动和状态，其中ω代表角速度。当vy = 0，v = vx时，通过离散表达式（2），该模型将被写成如下形式：
机器人进化模型反映了机器人的前一状态的XR（k）及其当前状态XR（k + 1）之间的关系。当Uk作用于机器人上时，让Uk = [v , ω]T作为控制向量。机器人在一个平面上移动，在同一平面上接收地标距离和方向信息。在SLAM中，系统的状态向量有一个UGV的位置（XR）信息，它通过一个三元素向量（XR = [Xr, Yr, θr]T ∈ R3）表示，地图是由（mi = [x1, y1,..., xM, yM]T ∈ R2M）向量表示，其中M是地标的总数量 ，∆T表示时间步长，εxr yrθr是由编码器和车轮滑动等产生的噪声。我们可以将方程（3）写成如下形式：
图3.UGVSLAM：（a）直接观测模型 （b）逆观测模型
3.观测模型
3.1直接观测点模型
感知提供了相对于机器人测量得到的已知地标的距离和方向角信息，见图3(a)。
我们假设在环境中有mi个地标的位置点是已知的。我们还假设，激光测距传感器垂直位于斧轮（axe wheels）中心的上方，因为这样假设可以简化方程和研究算法。如果mi是已知的，那么UGV将作为观察点，所观察到的量是状态的非线性方程。当在时刻k时，地标mi的坐标Xmig与Ymig被检测到，一个基于X轴测量的角度βi是连接地标中心与UGV中心的连线与X轴的夹角，ρi是 UGV坐标系 [Xr Yr θr]T的原点到地标的距离。
本实验采用的UGV配备激光测距仪（见图1），在单次的扫描返回361个测距数据，范围分辨率0.05米，0.5°角分辨率，最大距离范围15米时有180°扫描角。在用点表示地标的情况下，扫描的每个点被认为是一个地标，由两个参数[ρi，βi]表示。
根据其在本地坐标系中的坐标，全局坐标系中的坐标表示由下式给出：
整体转化为局部逆变换如下：
说明：
(Xmig；ymig)：在全局坐标系中地标点坐标；
(Xrl ; Yrl)：在局部坐标系中地标点坐标；
(Xr , Yr) :机器人在全局坐标系中的坐标。
θr：机器人的方向角。
上标i是地标样本集中的第i个样本。观测值通过式子Z(k) = [ρi(k) , βi(k)]T给出，这是机器人上的传感器相对于地标的距离和角度。
直接观测模型表达式如下：
上式中，ερi是测量距离的噪声，εβi是角度误差。
3.2逆观测点模型
考虑图（3（b））中所示的例子，机器人的状态是(Xr ; Yr)，观测到一个地标mnew， 由激光测距仪测得其坐标为(Xnew ; Ynew)，使Zi = [ρil; βil] 表示机器人观测到的地标mnew。
地标匹配模型是一个逆观测模型，知道机器人的状态值和观测值，它可以写成如下形式：
4.EKF滤波器
卡尔曼滤波器（KF）为存在高斯白噪声的线性动态系统提供了一种优异的，统计最优的解决方案。它是一种利用状态线性相关的测量和误差协方差矩阵来生成一个被称为卡尔曼增益的方法。该增益被应用于先验状态估计，从而产生后验估计。以下两个方程描述了一般用于线性状态估计的系统动态模型和测量模型。
接下来的五个来自KF算法的方程，使用迭代的方式，结合方程（10）和（11），方程（12）推断出先验状态估计，方程（13）是相应的状态误差协方差。卡尔曼增益可以由方程（14）计算得到，并分别用于更新由方程（15）和（16）所描述的状态估计和误差协方差。
EKF可用于非线性系统，它在概念上类似于迭代KF的过程。根据其相应的雅可比矩阵对非线性系统（F）和测量（H）矩阵进行线性化，这个雅克比矩阵是对应的一阶偏导矩阵。这种线性化有时会导致实施EKF时引起不稳定。EKF算法的主要步骤可以定义如下（见图4）：
图4. EKF滤波算法
本文提出了一种采用经典的EKF-SLAM “点对点”算法的地标关联方法，如图5所示。在这种情况下的系统模型和测量模型是非线性的，测量是在笛卡尔坐标系下进行的。在下面的部分本文将研究SVSF算法，本文将SVSF作为一种替代解决方法用来解决跟EKF相关的线性化问题，在转移到解决无人地面车辆SLAM问题所必须的非线性SVSF研究之前，本文首先考虑SVSF在线性系统中应用的情况。
图5. EKF-SLAM算法步骤