[发明专利]一种定位方法、装置、机器人及计算机可读存储介质

权利要求书

1.一种定位方法，用于对目标对象进行定位，其特征在于，包括：

获取通过扩展卡尔曼滤波模型预测出的所述目标对象在上一时刻的第一位姿信息；其中，所述上一时刻为与当前时刻相邻的上一时刻；

获取传感器采集到的所述目标对象在当前时刻的第二位姿信息；

基于所述第一位姿信息和所述第二位姿信息，通过所述扩展卡尔曼滤波模型预测所述目标对象在当前时刻的第三位姿信息；

确定预设约束条件下所述第三位姿信息的误差值，根据所述误差值对所述第三位姿信息进行修正，得到所述目标对象在当前时刻的最终位姿信息；其中，所述预设约束条件根据用于表征所述目标对象当前运动方式的约束方程确定；

所述确定预设约束条件下所述第三位姿信息的误差值，包括：

根据以下公式计算预设约束条件下所述第三位姿信息的误差值：

d_k＝D^T(D·W^-1·D^T)^-1·(D·x_k-d)；

其中，d_k为预设约束条件下所述第三位姿信息的误差值，D和d构成了用于表征所述目标对象当前运动方式的约束方程的约束条件，D和d均为常量，D和d根据所述目标对象当前的运动方式确定，W为权重矩阵，用于表征位姿信息中各个参量的权重；x_k为第三位姿信息；用于表征所述目标对象当前运动方式的约束方程可以根据所述目标对象在当前定位过程中的运动方式以及用于描述位姿信息的各个参量之间的映射关系来确定。

2.根据权利要求1所述的定位方法，其特征在于，所述位姿信息包括位置信息、线速度信息、线加速度信息、角速度信息及偏角信息。

3.根据权利要求1所述的定位方法，其特征在于，所述基于所述第一位姿信息和所述第二位姿信息，通过所述扩展卡尔曼滤波模型预测所述目标对象在当前时刻的第三位姿信息，包括：

基于所述第一位姿信息和所述第二位姿信息，根据所述扩展卡尔曼滤波模型中的状态方程和观测方程计算所述目标对象在当前时刻的第三位姿信息；

所述状态方程为：x_k＝f(x_k-1,u_k-1)；

所述观测方程为：z_k＝h(x_k)+v_k；

其中，x_k为第三位姿信息，x_k-1为第一位姿信息，u_k-1为用于描述位姿信息的参量中的预设参量在上一时刻的控制量，f(x,u)为预设的理想状态转移函数，z_k为第二位姿信息，h(x)为根据所述传感器的属性确定的理想状态测量函数，v_k为当前时刻用于描述位姿信息的各个参量所对应的测量噪声。

4.根据权利要求3所述的定位方法，其特征在于，所述基于所述第一位姿信息和所述第二位姿信息，根据所述扩展卡尔曼滤波模型中的状态方程和观测方程计算所述目标对象在当前时刻的第三位姿信息，包括：

根据以下公式计算所述目标对象在当前时刻的预测位姿信息：

x_k|k-1＝F_k-1·x_k-1+B_k-1·u_k-1+w_k；

其中，x_k|k-1为所述目标对象在当前时刻的预测位姿信息，F_k-1为上一时刻的理想状态转移矩阵，所述理想状态转移矩阵为所述理想状态转移函数的矩阵形式，B_k-1为控制量u_k-1对应的控制模型矩阵，w_k为当前时刻的过程噪声，所述当前时刻的过程噪声用于表征当前预测所述目标对象的位姿信息时的预测误差；

根据以下公式计算所述目标对象在当前时刻的第三位置信息：

x_k＝x_k|k-1+K_k·(z_k-h(x_k|k-1))；

其中，K_k为最优卡尔曼增益。