[发明专利]一种基于激光扫描仪的自主导航方法

权利要求书

1.一种基于激光扫描仪的自主导航方法，其特征在于：包括以下步骤：

建立特征地图和巡检线路：通过人工的方式牵引巡检机器人完成对巡检路线的一次巡视，依次巡视形成以下模块：位置测量模块→位置估计模块→几何特征抽象模块→全局特征地图构建模块；

巡检：

巡检机器人自动按照以下步骤进行巡检，直至结束：位置测量模块→几何特征抽象模块→特征匹配模块→位置估计模块→机器人位置调整模块。

2.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：步骤A中所述位置测量模块的建立：首先根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离；然后根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标（,）计算得到T+1时刻的全局坐标（,）。

3.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：步骤A中的所述位置估计模块的建立：运用卡尔曼滤波建立巡检机器人的运动模型和观测模型；

X(K)=A*X(K-1)+B*U(K)+W(K)(1)

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-1)=A*X(k-1|k-1)+B*U(k)

（3）

其中X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)为上一时刻的最优估计值，U(k)为现在状态的控制量，在模型中，控制量U(k)为0；

经过公式（3）之后，系统结果已经更新了，接下来计算X(k|k-1)的协方差，用P来表示协方差，则为：

P(k|k-1)=A*P(k-1|k-1)*+Q

（4）

其中P(k|k-1)为X(k|k-1)对应的协方差，P(k-1|k-1)为X(k-1|k-1)对应的协方差，为A的转置矩阵，Q为系统噪声；

X(k|k)=X(k|k-1)+(k)*(Z(k)–H*X(k|k-1))

（5）

(k)=P(k|k-1)*/(H*P(k|k-1)*+R)

（6）

P(k|k)=(1-(k)*H)*P(k|k-1)

（7）。

4.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：步骤A中所述的几何特征抽象模块的建立：对于激光扫描仪采集的数据点，将其抽象出几何特征，以便在巡检过程中进行位置匹配；抽象出的几何特征为直线段和圆弧，使用的方法为最小二乘法拟合；对于拟合出的直线段，取线段的中心点(,)为质心，直线段则标识为(,,L,)，L为线段的长度，为线段与x轴的夹角；对于拟合出的圆弧，我们取其圆心(,)为质心，圆弧则标识为(,,R)，R为圆弧半径。

5.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：步骤A中所述的全局特征地图构建模块的建立：对应每一个拟合出的直线段和圆弧，加上位置测量模块中里程计给出的信息，确定其全局坐标系中的坐标(x,y)，其中每个特征都有全局坐标系中的坐标，和其先后的顺序，以及每一个时刻机器人的位置信息集P((,),(,)……(,))。

6.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：巡检步骤B中所述的位置测量模块：根据里程计得出巡检机器人在T到T+1时刻沿行进方向运动的距离;根据陀螺仪测得行进方向和水平X轴的夹角；根据T时刻的全局坐标（,）计算得到T+1时刻的全局坐标（,）。

7.根据权利要求1所述的基于激光扫描仪的自主导航方法，其特征在于：巡检步骤B中的所述几何特征抽象模块：对于激光扫描仪采集的数据点，使用最小二乘法进行拟合，得到直线段和圆弧；此时：对于拟合出的直线段，得到其(L,)，其中L为线段的长度，为线段与x轴的夹角；对于拟合出的圆弧，得到其R，R为圆弧半径。