[发明专利]一种基于位姿图优化进行状态估计的处理方法

权利要求书

1.一种基于位姿图优化进行状态估计的处理方法，其特征在于，所述方法包括：

接收运动模型类型作为第一模型类型；

接收时刻t-k到t-1的第一数量k个位姿观测量作为第一观测量m_i组成第一观测量序列；1≤i≤k；

根据所述第一模型类型和所述第一观测量序列构建第一位姿图；

通过优化所述第一位姿图得到与所述第一观测量序列对应的第一估计状态量序列；

接收时刻t的位姿观测量作为第二观测量m’；

根据所述第二观测量m’和所述第一估计状态量序列对所述第一位姿图进行位姿图变换得到第二位姿图；

通过优化所述第二位姿图得到时刻t的第二估计状态量。

2.根据权利要求1所述的基于位姿图优化进行状态估计的处理方法，其特征在于，所述根据所述第一模型类型和所述第一观测量序列构建第一位姿图，具体包括：

构建所述第一位姿图的k个第一状态节点Ns_i；并将所述第一状态节点Ns_i的对应状态量作为对应的第一估计状态量s_i；并将各个所述第一状态节点Ns_i顺次连接组成里程计运动关系节点链，将节点链上从所述第一状态节点Ns_i到所述第一状态节点Ns_i+1的节点边作为对应的第一状态节点边Ls_i,i+1；

并根据所述第一模型类型构建所述第一状态节点边Ls_i,i+1的里程计运动约束关系为s_i+1＝A_i|i+1·s_i+w_i；其中，A_i|i+1为从所述第一状态节点Ns_i到所述第一状态节点Ns_i+1的第一状态转换矩阵，w_i为所述第一状态节点Ns_i的第一状态噪声矩阵；所述第一状态转换矩阵A_i|i+1为里程计运动状态转换矩阵，其结构与所述第一模型类型对应；

构建所述第一位姿图的k个第一观测节点Nm_i；并将各个所述第一观测节点Nm_i作为对应的所述第一状态节点Ns_i的误差约束节点；并将所述第一观测节点Nm_i的对应观测量初始化为所述第一观测量序列中对应的所述第一观测量m_i；并将从所述第一观测节点Nm_i到所述第一状态节点Ns_i的节点边作为对应的第一误差节点边Lm-s_i；

并构建所述第一误差节点边Lm-s_i的误差约束关系为其中，f(·)为损失函数。

3.根据权利要求2所述的基于位姿图优化进行状态估计的处理方法，其特征在于，

所述损失函数f(·)基于Huber Loss函数构建。

4.根据权利要求2所述的基于位姿图优化进行状态估计的处理方法，其特征在于，所述通过优化所述第一位姿图得到与所述第一观测量序列对应的第一估计状态量序列，具体包括：

以所述第一状态节点边Ls_i,i+1的所述里程计运动约束关系和所述第一误差节点边Lm-s_i的所述误差约束关系作为对应的所述第一状态节点Ns_i的两个约束条件，构建所述第一位姿图非线性最小二乘问题的第一优化目标函数；

将各个所述第一观测节点Nm_i的对应观测量作为已知量代入所述第一优化目标函数，并对使所述第一优化目标函数达到最优值的各个所述第一状态节点Ns_i的对应状态量进行估计，并将估计得到各个所述第一状态节点Ns_i的对应状态量作为对应的第一估计状态量s_i；并由得到的所有所述第一估计状态量s_i组成对应的所述第一估计状态量序列并保存。