[发明专利]一种针对四轮机器人的转动瞬心估计方法

摘要

本发明公开了一种针对四轮机器人的转动瞬心估计方法，通过引入最优估计理论与地形检测算法，能够获取机器人的转动瞬心的估计值。该方法包括初始化步骤、采集传感器数据步骤、地形检测步骤、调整过程噪声方差步骤、状态预测步骤、状态更新步骤、以及输出转动瞬心的估计值的步骤。本发明优点如下：1)针对运动学方程是非线性而观测方程是线性的情况，在状态预测过程中引入无色变换，以保证在强非线性情况下的状态预测的准确性；2)引入新息协方差估计器；3)由于引入了地形检测，在地形发生明显变化的时候，本发明会调整转动瞬心的过程噪声方差，这种自适应机制能够保证转动瞬心估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景。

主权项

1.一种针对四轮机器人的转动瞬心估计方法，其特征在于，包括以下步骤：步骤一：对采样点序号k、后验状态估计后验误差协方差Pk、新息协方差εk、地形特征向量pk、过程噪声和观测噪声的方差Qk和Rk、采样间隔T以及车身宽度B进行初始化，其中，后验状态估计中的与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为转动瞬心的3个运动学参数的后验状态估计；步骤二：将采样点序号自增k←k+1，采集加速度计关于垂直于地面轴向的加速度数据，并在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{ak,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵Mk；采集左右轮编码器数据，获得左右车轮的旋转速度vL,k与vR,k；采集电子罗盘数据与GPS模块数据，获得观测向量zk＝[ze,k zn,kzθ,k]′，其中，ze,k与zn,k为东向坐标与北向坐标的观测值，由GPS模块采集获得，zθ,k为航向角的观测值，由电子罗盘采集获得；步骤三：根据步骤一中获得的地形特征向量以及步骤二中获得的加速度数据集合和地面照片像素矩阵进行地形检测，判断地形是否发生显著变化；步骤四：若判定地形发生显著变化，在接下来五个采样点内将转动瞬心的过程噪声方差，乘以设定倍数；若地形没有发生变化，则保持原来的方差；步骤五：根据步骤一中获得的后验状态估计、后验误差协方差、采样间隔以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声方差，进行状态预测，得到先验状态估计与先验误差协方差；步骤六：根据步骤一中获得的新息协方差、观测噪声方差,步骤二中获得的观测向量以及步骤五中获得的先验状态估计和先验误差协方差，计算得到后验状态估计后验误差协方差Pk；以及步骤七：重复步骤二至步骤六，以输出每一个采样点的后验状态估计中第4至6元素即转动瞬心的估计值。