[发明专利]一种实现移动机器人同时定位与地图构建的方法

权利要求书

1.一种实现移动机器人同时定位与地图构建的方法，其特征在于，包括：

步骤S1：确定全局坐标系以及该坐标系下的移动机器人的初始位姿值；

步骤S2：记录k-1时刻的状态估计量以及对应的协方差矩阵，其中状态估计量包括移动机器人的位姿以及路标位置；

步骤S3：判断在k时刻是否能够观测到路标，如果在k时刻不能观测到路标，则进行时间更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果在k时刻能够观测到路标，则进行数据关联，然后执行步骤S4；

步骤S4：判断观测到的路标是否为新的特征，如果观测到的路标是新的特征，则进行路标初始化，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；如果观测到的路标是已有的特征，则进行STF预测，然后执行步骤S5；

步骤S5：对STF预测的结果进行观测更新，得到k时刻的状态估计量以及对应的协方差矩阵，返回步骤S2；

步骤S6：重复步骤S2～步骤S5，得到最后时刻的状态估计量，即移动机器人的位姿以及环境中路标的位置，实现同时定位与地图构建。

2.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，在步骤S3中，移动机器人采用传感器获取环境中路标观测数据，该路标观测数据是路标相对于移动机器人的位姿信息，且该路标观测数据含有方向信息。

3.根据权利要求1所述的实现移动机器人同时定位与地图构建的方法，其特征在于，步骤S3中所述时间更新，采用扩展卡尔曼滤波法，根据上一周期移动机器人的位姿估计和航位推测传感器数据，利用移动机器人运动模型，预估当前周期移动机器人的位姿。

4.根据权利要求3所述的实现移动机器人同时定位与地图构建的方法，其特征在于，所述时间更新采用的计算公式如下：

X(k)=Xr(k|k)m(k)=g(X(k-1),u(k))+ϵ(k)]]>

公式10

=f(Xr(k-1|k-1),u(k))m(k-1)+ϵ(k)]]>

其中m(k)为路标k时刻的位姿，ε(k)为零均值高斯白噪声，ε(k)的协方差矩阵为J_r用于将3维的状态向量映射为3N+3维的向量；

公式11

将运动方程g用一阶泰勒展开式近似：

g(X(k-1),u(k))≈g(X^(k-1),u(k))+G(k)·(X(k-1)-X^(k-1))]]>公式12

其中：

G(k)=∂g(X(k),u(k))∂X(k)|X^(k|k-1),u(k)]]>公式13

则k时刻的状态向量估计值为：

X^(k)=g(X^(k-1),u(k))]]>

=X^r(k|k)m^(k)=f(X^r(k-1|k-1),u(k))m^(k-1)]]>公式14

k时刻的协方差矩阵P(k|k)为：

P(k|k)=G(k)P(k-1|k-1)GT(k)+JrTQ(k)Jr]]>公式15。