为障碍或特征发现 装置，由此在检测到的障碍或特征在水池清洁器12正前方一定距离时控制水池清洁器12 的转弯运动。在一些实施例中，控制系统10可以被配置成绘制放置水池清洁器12的环境 (即，游泳池，温泉等），并利用同步定位和绘制（SLAM)技术，基于来自激光测距仪16和第 二传感器组件24的输出了解水池清洁器的周围（即，无需先前的与环境相关的校准或教 导）。通过这种方式，控制系统10能够确定和优化清洁路线并能够操作水池清洁器12以遵 循（follow)这些优化的清洁路线（例如，在特定时间段内遍历整个游泳池地板）。此外，控 制系统10能够跟踪清洁器的运动，以便跟踪被清洁碎肩的路线，并确保已经在特定时间段 之内遍历了整个游泳池地板。在一些实施例中，如下所述，可以由控制系统10使用基于特 征的扩展卡尔曼滤波器（EKF) (Extended Kalman Filter)SLAM技术。在其他实施例中，可 以使用其他SLAM技术。 通常，为了使机器人运载工具能够自主在任何环境中执行任务，它们必须能够确定其 位置以及定位并且记住该环境中的障碍以及目标对象的位置，或者换言之，它们必须能够 SLAM。可以使用扩展的卡尔曼滤波器（EKF)来估计SLAM之后的情况。以下段落提供了根 据本发明一些实施例的EKF SLAM方法的概要。 从概率的意义上，SLAM的目标是连同周围环境的地图估计水池清洁器12当前姿态的 后续发展，其被表示为 p (xt, m| Zj：t, u !：t), Eq. 10 其中xt是水池清洁器12在时间t的姿态，m是地图，z 1:t是测量结果，u 1:t为控制输入。 EKF能够假设状态转换和测量模型定义为 Xt= g(u t，Xt Ο + Πχ,?，t = 1，2· · ·， Eq. 11 Zt= h(x t) + qz，t， Eq. 12 其中8(.)和11(.)是非线性噪声和加性噪声，11)^和110是协方差分别为1^和^的 零平均值高斯过程。SLAM的EKF解落在称为基于特征的方法的一类解中。在基于特征的 SLAM中，假设围绕水池清洁器12的环境可以由称为特征的一组不同的点表达。结果，完整 的SLAM状态由清洁器12的状态和地图的状态构成。
其中X和y是清洁器12在二维（2D)平面中的位置，Θ是行进方向。地图由N个特征 来代表，并且每个特征在2D平面中的位置保持在状态和中。 SLAM的EKF解能够使用经典的预测校正模型。更具体而言，EKF的预测步骤基于由以 上方程11给出的系统的状态转换模型，且可以定义为
其中xt i是根据前一时间步骤的状态估计，X t :是当前时间步骤的完整SLAM状态的预 测，Stl是前一时间步骤的协方差估计，g是当前时间步骤的协方差预测，且匕是8(.)相 对于在4和X t i评估的X t 4勺雅可比行列式。校正步骤来自由以上方程12给出的测量模 型，且可以定义为
其中氏是h(.)相对于在X t JP z t评估的X t i的雅可比行列式，z t是当前时间的测量 结果。 一些实施例的当前EKF SLAM技术能够包括标准EKF中没有出现的附加步骤，其与向 SLAM状态增加新特征相关。例如，在遇到新特征时，必须将其集成到完整SLAM状态（xt)以 及SLAM协方差（X t)两者中。可以由下式定义SLAM状态的增强（augmentation):
其中4是增加新特征之后的SLAM状态，且f(.)基于当前清洁器状态和对特征的观测 估计在全局系中新特征的位置。 相对于SLAM协方差的增强，对SLAM协方差的检查表明其采取如下形式
[0070] 其中Σρ是清洁器估计的协方差，Σ ^是清洁器估计和地图估计之间的协方 差，且Xt,m是地图估计的协方差。根据Bailey等人（"Simultaneous localization and mapping (slam) ：Part ii'，。RoboticsMutomation Magazine，IEEE，13 (3)，pp. 108-117)， 可以将SLAM协方差的增强形式计算为
其中是增强的SLAM协方差，FtiX是f(.)相对于在x ,和z t评估的x t的雅可比行列 式，Ft,^ f (·)相对于在X JP z t计算的z t的雅可比行列式。 参考本发明实施例的控制系统10,传感器组件16、24能够提供表示上述状态转换模型 输入Ut和特征测量结果z t的数据。传统上，对于地面运载工具应用，状态转换模型的输入由 来自车轮编码器的测距读数构成，而特征的位置是利用光探测和测距（LiDAR)来计算的。 不过，这些类型的传感器不能在水下环境中工作。在典型的水下环境中，很多现有的传感器 技术基于声学，其中从多普勒速度记录仪（DVL)向运载工具提供测距数据，且利用SONAR传 感器定位特征。不过，如上所述，由于在相对小的封闭环境（例如游泳池和温泉）中可能产 生大量多次回波，所以基于声学的传感器是存在问题的。此外，当前可用的传感器存在传感 器特有的问题。例如，如上所述，水池清洁器12能够直接在水池地板上操作或非常接近水 池地板工作。在这样的操作环境中，DVL传感器的性能差，而且它们还尺寸大且价格高，使得 它们无法用于小型廉价水下载体中。此外，SONAR传感器的问题是它们难以在实施基于特征 的SLAM方法时用于特征提取。更具体而言，SONAR只能报告位于SONAR传感器扫描锥前方 一定距离处的对象的存在，使其难以识别可用于在基于特征的SLAM中生成地图的独特特 征。结果，在可以发生适当数据关联之前必须从多个位置观测特征。基于计算机视觉算法和 上述传感器组件16、24,本发明的控制系统10能够克服以上问题并能够确定状态转换模型 的控制输入，以及封闭水下环境中的有效界标测量结果（valid landmark measurement)， 如下文进一步所述。 对于第二传感器组件24而言，可以通过跟踪相机获取的连续图像之间的一组点，从面 向下的相机计算视觉测距数据。根据帧之间点的平移和旋转，可以确定清洁器取向的变化 (因此提供状态转换模型输入）。例如，参考图13,比较两幅图像以确定清洁器12的取向变 化（即，相对于全局参照系167所示的当前图像I。和之前图像I P)。选择要在帧间跟踪的 点的一种方法是从图像随机选择点（例如图13中的点162)。不过，选择的所得点可能难 以唯一地识别，从而跟踪点变得相当困难。为了缓解这个问题，根据一些实施例，可以使用 具有其内置功能GoodFeaturesToTrack的开源图像处理库OpenCV。GoodFeaturesToTrack 功能选择图像中的角作为能够容易识别和跟踪的特征。在一个实施例中，可以基于Shi和 Tomasi 提出的方法计算角（"Good features to track'，，Computer Vision and Pattern Recognition，1994。 Proceedings CVPR'94,1994IEEE Computer Society Conference, pp. 593-600)，这种方法首先利用Sobel算子计算点周围的Hessian矩阵，以计算二阶导数。 然后比较Hessian矩阵的两个本征值的最小值，如果其高于预设的最小阈值，选择该点作 为有效角。利用选择的一组可跟踪的点，可以通过从I p到IJS踪点，计算如图13所示的帧 间位置的变化。 为了在帧之间跟踪点，可以使用多步算法。首先，例如，可以使用内核大小（kernel size)为7的拉普拉斯算符滤波器过滤IP。可以使用过滤的图像而不是原始图像进行跟踪， 以便考虑两帧之间光照条件的变化（例如，以防止由于阴影或光亮的变化导致的跟踪性能 的劣化）。 在过滤Ip之后，可以对图像执行GoodFeaturesToTrack功能，以计算在帧之间进行跟 踪的点集。然后可以使用对Ip使用的相同方法过滤Ic。然后可以利用交叉相关（cross correlation)技术，例如由 Nourani-vatani 等人描述的技术("Correlation Based Visual Odometry for Ground Vehicles"。Journal of Field Robotics，28(5)，ρρ· 742-768)，在 Ic中找到从IP选择的每个点。例如，可以从IP选择包含点的窗口，并可以在点窗口和I c之 间进行交叉相关。交叉相关最大值的位置对应于Ic中的点的位置。可以利用小角度近似 和2D齐次变换方程的线性化版本来确定I#P I ε中的点之间的关系：
其中\丨，^和3^分别是该点在。和1。中的叉和7位置，3；!，5,50是清洁器在 相机参照系中的变化和取向的分量。整理方程22获得 yp δ θ + δ χ = xc-xp, Eq. 23 -Xp δ θ + δ y = yc-yp, Eq. 24 可以针对所有被跟踪的点将其组合为
其中i = l，2，……，M，M是被跟踪点的数目。可以通过利用SVD算法计算伪逆 (pseudoinverse)来发现所得的取向变化。然后可以利用事先从运行校准算法确定的校准 常数将取向变化δ χ，δ jP δ e从像素单位变换到世界单位。 在建立状态转换模型中可以考虑两个参照系：运载工具参考系169,在其中测距数据 被收集；以及全局参考系167,清洁器12在其中工作，两者都示于图14中（其中全局参考 系167由yjP X g表示，而运载工具参考系169由y'和X'表示）。视觉测距数据从相机参 考系到全局参考系的转化（rotation)，从几何的角度上来说，可以定义为：
其中Δχ和Ay是清洁器在全局系中的平移，Δχ'和Δ/是在运载工具系中的平移。 所得的状态转换矩阵被定义为
其中