[发明专利]一种履带式机器人滑动参数检测方法

权利要求书

1.一种履带式机器人滑动参数检测方法，其特征在于，包括以下步骤：

步骤一：对采样点序号k、后验状态估计后验误差协方差P_k、新息协方差ε_k、地形特征向量p_k、过程噪声和观测噪声的方差Q_k和R_k、采样间隔T以及车身宽度B进行初始化，其中，后验状态估计中的六个元素为：与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为左右履带滑动比以及机器人本体滑动角的后验状态估计；

步骤二：将采样点序号自增k←k+1，采集加速度计关于垂直于地面轴向的加速度数据，并在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度v_L,k与v_R,k；采集电子罗盘数据与GPS模块，获得观测向量z_k＝[z_e,k z_n,kz_θ,k]′，其中z_e,k与z_n,k为东向坐标与北向坐标的观测值，由GPS模块采集获得，z_θ,k为航向角的观测值，由电子罗盘采集获得；

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

步骤四：若判定地形发生显著变化，在接下来五个采样点内将滑动系数的过程噪声方差乘以设定倍数；如果地形没有发生变化，则保持原来的方差；

步骤五：根据步骤一中获得的后验状态估计、后验误差协方差、采样间隔以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声方差，进行状态预测，得到先验状态估计与先验误差协方差；

步骤六：根据步骤一中获得的新息协方差、观测噪声方差,步骤二中获得的观测向量以及步骤五中获得的先验状态估计和先验误差协方差，进行状态更新，得到新息协方差ε_k、后验状态估计和后验误差协方差P_k；以及

步骤七：重复执行步骤二至步骤六，以获得每一个采样点的后验状态估计其中中第4至6元素即滑动系数的估计值。

2.根据权利要求1所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤三包括以下子步骤：

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

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

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

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

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

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

3.根据权利要求2所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤四中的设定倍数为10倍。

4.根据权利要求3所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤五包括以下子步骤：

4.1)创建一组Sigma点集合如下：其中m为状态维度，λ为比例因子；

4.2)用4.1中的Sigma点预测状态，得到一组状态预测Sigma点的集合如下：其中，与分别表示集合与中的第i个元素，状态转移方程f(·)具体为：

4.3)对4.2中的状态预测Sigma点加权，得到先验状态估计与先验误差协方差，如下：

其中，W_i^m与W_i^c分别为均值与协方差的权重系数。

5.根据权利要求4所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤六包括以下子步骤：

5.1)计算新息∈_k如下：其中，为观测矩阵；

5.2)计算新息协方差ε_k如下：

5.3)计算卡尔曼增益K_k如下：

5.4)计算后验状态估计如下：以及

5.5)计算后验误差协方差P_k如下：P_k＝(I_6×6-K_kC)P_k,k-1，其中，矩阵I_6×6为6维单位矩阵。