[发明专利]一种野外机器人航迹推算方法

权利要求书

1.一种野外机器人航迹推算方法，其特征在于，包括以下步骤：

步骤一：对采样点序号k、后验状态估计椭球地形特征向量P_k、过程噪声和电子罗盘噪声的包络矩阵Q_k和R_k、采样间隔T、车轮半径Φ以及车身宽度B进行初始化，其中，后验状态估计椭球的椭球中心中的为航向角的后验状态估计椭球的中心和为转动阻力系数的后验状态估计；

步骤二：将采样点序号自增k←k+1，采集加速度计关于垂直于地面轴向的加速度数据，在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度ω_L,k与ω_R,k；采集电子罗盘数据，获得机器人航向角y_k；

步骤三：根据步骤一中获得的地形特征向量以及步骤二中获得的加速度数据集合和地面照片像素矩阵进行地形检测，判断地形是否发生显著变化；

步骤四：若判定地形发生显著变化，在接下来五个采样点内将转动阻力系数的过程噪声包络矩阵，乘以设定倍数；若地形没有发生显著变化，则保持原来的包络矩阵；

步骤五：根据步骤一中获得的后验状态估计椭球、采样间隔、车轮半径以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声包络矩阵，进行状态预测，得到先验状态估计椭球；

步骤六：根据步骤一中获得的电子罗盘噪声包络矩阵，步骤二中获得的机器人航向角以及步骤五中获得的先验状态估计椭球，进行状态更新，得到后验状态估计椭球；

步骤七：根据步骤一获得的采样间隔、车轮半径以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤六中获得的后验状态估计椭球，进行航迹推算定位，输出机器人位置坐标，即东向坐标e_k和北向坐标n_k；以及

步骤八：重复执行步骤二至步骤七，获得每一个采样点的机器人位置坐标。

2.根据权利要求1所述的野外机器人航迹推算方法，其特征在于，所述步骤三包括以下子步骤：

2.1)消除加速度数据集合的直流分量：将加速度数据集合的所有元素分别减去该集合所有元素的均值：得到消除了直流分量的加速度数据集合

2.2)提取地面照片的主色：从地面照片像素矩阵M_k中随机抽取50个像素并求平均，得到地形主色的红绿蓝分量l_R,k，l_G,k与l_B,k；

2.3)求取地形特征向量其中的元素如下：

2.4对地形特征向量P_k进行归一化处理；以及

2.5判断地形是否变化：计算地形相似度距离

其中，ω_i∈(0 ， 1] 为每个特征分量的权重，如果则判断地形没有变化，否则判断地形发生了显著变化。

3.根据权利要求2所述的野外机器人航迹推算方法，其特征在于，在步骤四中所述设定倍数为10倍。

4.根据权利要求3所述的野外机器人航迹推算方法，其特征在于，在步骤五中所述先验状态估计椭球如下：

其中，状态转移方程f(·)具体为：

其中，矩阵为状态转移方程f(·)的雅克比矩阵，tr( · ) 表示矩阵的迹。