[发明专利]定位方法、定位装置、移动机器人及存储介质

权利要求书

1.一种移动机器人的定位方法，其特征在于，所述方法包括：

采集所述移动机器人的若干定位信息，所述定位信息包括里程计信息、惯性信息和深度视觉信息；

根据所述定位信息，经卡尔曼滤波法获得所述移动机器人的三维空间姿态，包括：

步骤A：根据里程计信息初始化系统状态量、协方差矩阵，并结合所述系统状态量和所述协方差矩阵，基于系统模型进行状态的估计，确定预测状态量与预测协方差矩阵；其中，所述系统模型为基于里程计信息建立的模型，所述系统模型如下：

系统状态量为[x,y,z,pitch,roll,raw]^T；

系统输入为[u₁,u₂]^T；

系统方程为：

x(t)＝x(t-1)+u₁(t)*cos(yaw(t-1))

y(t)＝y(t-1)+u₁(t)*sin(yaw(t-1))

yaw(t)＝yaw(t-1)+u₂(t)

其中，x、y、z为物体的三维空间坐标，pitch为俯仰角，roll为翻滚角，raw为偏航角，yaw为角度信息；

步骤B：根据所述预测状态量和所述预测协方差矩阵，在三个所述定位信息中选择任一项作为观测量计算观测协方差，若所述观测协方差符合更新条件，则更新所述预测状态量和所述预测协方差，否则返回步骤A，通过所述系统模型估计所述预测状态量和所述预测协方差；

步骤C：根据所述步骤B获得的预测状态量和预测协方差矩阵，在未被选择的两个定位信息中选择任一项作为观测量计算所述观测协方差，若所述观测协方差符合更新条件，则更新所述预测状态量和所述预测协方差，否则返回步骤A，通过所述系统模型估计所述预测状态量和所述预测协方差；

步骤D：根据所述步骤C获得的预测状态量和预测协方差矩阵，将未被选择的定位信息作为观测量计算所述观测协方差，若所述观测协方差符合更新条件，则更新所述预测状态量和所述预测协方差，并将更新后的所述预测状态量作为所述移动机器人的三维空间姿态，否则返回步骤A，通过所述系统模型估计所述预测状态量和所述预测协方差；其中，所述三维空间姿态包括三维空间坐标、俯仰角、翻滚角和偏航角

2.根据权利要求1所述的移动机器人的定位方法，其特征在于，所述里程计信息的观测量和观测矩阵的内容如下：

观测量为[x,y,z,pitch,roll,raw]^T；

观测矩阵为[1 0 0 0 0 0；0 1 0 0 0 0；0 0 0 0 0 0；0 0 0 0 0 0；0 0 0 0 0 0；00 0 0 0 0 1]；

所述惯性信息的观测量和观测矩阵的内容如下：

观测量为[pitch,roll,raw]^T；

观测矩阵为[0 0 0 1 0 0；0 0 0 0 1 0；0 0 0 0 0 1]；

所述深度视觉信息的观测量和观测矩阵的内容如下：

观测量为[x,y,z,pitch,roll，raw]^T；

观测矩阵为[1 0 0 0 0 0；0 1 0 0 0 0；0 0 1 0 0 0；0 0 0 1 0 0；0 0 0 0 1 0；00 0 0 0 0 1]。

3.根据权利要求1所述的移动机器人的定位方法，其特征在于，所述更新条件包括：

当前状态的观测协方差与前一状态的观测协方差进行比较，若不同则更新。