+V (K) (2) X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为 K时刻的测量值，H为测量系数;W(K)和V(K)分别表示系统和测量过程中的噪声，噪声满足 高斯白噪声模型，设W (K)和V (K)的协方差分别为Q和R ; 基于卡尔曼滤波的5个经典核心公式： X(k|k-l)=A*X(k-l |k-l)+B*U(k) (3) 其中X(k|k-1)是利用上一状态预测的结果，X(k-l|k-l)为上一时刻的最优估计值， U (k)为现在状态的控制量，在模型中，控制量U (k)为O ; 经过公式（3)之后，系统结果已经更新了，接下来计算X (k I k-Ι)的协方差，用P来表示 协方差，则为： P (k Ik-1) = A*P(k-l|k-l)*ir + Q (4) 其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-I|k-1)为X(k-I|k-1)对应的协方差， 遍"为A的转置矩阵，Q为系统噪声。
[0015] X(k|k) = X(k|k-1) + _ (k)*(Z(k) - H*X(k|k-l)) (5) _ (k) = P(k|k-l)*_f AH*P(k|k-l)*__ + R) (6) P(k|k) = (I -、（k)*H)*P(k|k-l) (7)； 步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象 出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的 方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点，涯）为质心，直线段 则标识为（?, _，L，_)，L为线段的长度，％为线段与X轴的夹角；对于拟合出的圆弧， 我们取其圆心（玛:.，和:）为质心，圆弧则标识为（％，fc.，R)，R为圆弧半径； 步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧， 加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标（X，y)，其中每个 特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集 P ((\，_ )，（々，殺:）......（蟬，^ )); 巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到Τ+l时刻沿行 进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标 (_，瑪）计算得到Τ+l时刻的全局坐标(1_:，％_); 巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二 乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其（L，I )，其中L为 线段的长度，％为线段与X轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径； 巡检步骤B中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位 置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局 特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个 特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到（兩:，备，％ )和（X!:，_:，i ?); 巡检步骤B中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信 息A，4)和（裕，_，：?:)，运用平面三角定位的方法，计算当时时刻巡检机器 人的坐标（％，&)，在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相 同的卡尔曼滤波方法计算得到巡检机器人的位置最佳估计值; 巡检步骤B中所述机器人位姿调整模块：根据上一模块得到的位置最佳估计值（ι|， i )，在位置信息集P中查找对应的坐标（:?:，％)，调整机器人PID控制部分，实现机器人 从（域:，減:）一? (5%:.，為::）的移动； 实施例2 : 一种基于激光扫描仪的自主导航方法，包括以下步骤： A、 建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次 巡视，依次巡视形成以下模块：位置测量模块一位置估计模块一几何特征抽象模块一全局 特征地图构建模块； B、 巡检：巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块一几何特 征抽象模块一特征匹配模块一位置估计模块一机器人位置调整模块； 步骤A中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻 沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的 全局坐标(Χγ，轉）计算得到T+1时刻的全局坐标(Χ?·η， 步骤A中的所述位置估计模块的建立：运用卡尔曼滤波建立巡检机器人的运动模型和 观测模型； X (K) =A*X (K-1) +B*U (K) +W (K) (I) Z (K) =H*X (K) +V (K) (2) X(K)为K时刻的系统状态，U(K)为K时刻对系统的控制量，A和B为系统参数；Z(K)为 K时刻的测量值，H为测量系数;W(K)和V(K)分别表示系统和测量过程中的噪声，噪声满足 高斯白噪声模型，设W (K)和V (K)的协方差分别为Q和R ; 基于卡尔曼滤波的5个经典核心公式： X(k|k-l)=A*X(k-l |k-l)+B*U(k) (3) 其中X(k|k-1)是利用上一状态预测的结果，X(k-l|k-l)为上一时刻的最优估计值， U (k)为现在状态的控制量，在模型中，控制量U (k)为O ; 经过公式（3)之后，系统结果已经更新了，接下来计算X (k I k-Ι)的协方差，用P来表示 协方差，则为： P(k|k-1) = A*P(k-l|k-l)* 教f + Q (4) 其中p(k|k-i)为x(k|k-i)对应的协方差，p(k-i|k-i)为x(k-i|k-i)对应的协方差， 義?为A的转置矩阵，Q为系统噪声。
[0016] X(k|k) = X(k|k-1) + _ (k)*(Z(k) - H*X(k|k-l)) (5) 1? (k) = P(k|k-l)*|f 7(H*P(k|k-l)*_: + R) (6) P(k|k) = (I - (k)*H)*P(k|k-l) (7)。
[0017] 根据实际的观测模型，简化上述公式。令公式（3)中的A=l，U(k)=0,则公式简化 为： X(k|k-1) = X(k-l|k-l) (8) 公式⑷简化为： P (k |k-l) = P(k-1 |k-l) + Q (9) 再令H=I，公式（5) (6) (7)可简化为： X(k|k) = X(k|k-1) + _ (k)*(Z(k) - X(k|k-1)) (10) (k) = P (k |k-l) /(P(k|k-1) + R) (11) P(k|k) = (I - Ks (k))*P(k|k-l) (12) 通过公式（8) (9) (10) (11) (12)得到每一时刻的最优估计值X(k|k); 步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象 出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的 方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点（垮，&)为质心，直线段 则标识为（垮，_，L，| :)，L为线段的长度，I为线段与X轴的夹角；对于拟合出的圆弧， 我们取其圆心（释，_ )为质心，圆弧则标识为（瑪R)，R为圆弧半径； 步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧， 加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标（X，y)，其中每个 特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集 P ((4，我），（&，顆）......（，)); 巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行 进方向运动的距离；根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标 (_，)计算得到T+1时刻的全局坐标c%_，％释）； 巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二 乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其（L，$ )，其中L为 线段的长度，为线段与X轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径； 巡检步骤B中的特征匹配模块：对于上一模块中抽象出来的直线段或者圆弧，参考位 置测量模块中里程计给出的位置信息以及匹配的先后顺序，将抽象出来的几何特征在全局 特征地图中进行匹配，以确认该几何特征对应的全局地图中的特征；选取匹配成功的两个 特征，获取其全局位置坐标信息，并且计算质心与巡检机器人的距离d，得到（鮮，綠，^ )和（5?:，，：?.); 巡检步骤B中的位置估计模块：根据上一模块特征匹配得到的两个匹配成功特征的信 息Ui:，fe:，A)和（释，_，：?)，运用平面三角定位的方法，计算当时时刻巡检机器 人的坐标，編），在取得巡检机器人的坐标之后，运用特征地图和巡检线路的建立中相 同的卡尔曼滤波方法计算得到巡