一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法

专利主权项内容

1.一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，包括以下步骤：步骤一、根据轮式机器人的动力学特性，构建纵向、横向、横摆非线性三自由度动力学模型及车轮模型；步骤二、根据车轮动力学特性，定义车轮纵向力观测滑模面，构建自适应滑模观测器对车轮纵向力进行估计，实现机器人的车轮纵向力稳定观测估计；步骤三、基于步骤一中的非线性三自由度动力学模型和步骤二中观测估计的车轮纵向力，构建平方根容积卡尔曼滤波器，将平方根容积卡尔曼滤波器与机器人动力学系统对接，确定平方根容积卡尔曼滤波器的待估计量、输入量和观测量，并采用平方根准则对平方根容积卡尔曼滤波器进行待估计参数的修正及更新；步骤四、基于步骤三中构建的平方根容积卡尔曼滤波器，采用蜣螂优化算法对平方根容积卡尔曼滤波器的非高斯噪声进行寻优处理，实现轮式机器人行进状态参数的精准估计；步骤一建立的非线性三自由度动力学模型如下式所示：
，
，
，式中，为横摆角速度；/>为横摆角速度的微分形式；/>为前轮转角；/>为机器人总质量；/>为机器人质心绕/>轴的转动惯量；/>和/>分别为纵向力和横向力，/>表示左前轮，/>表示右前轮，/>表示左后轮，/>表示右后轮；/>表示前轮距，/>表示后轮距；质心与前后轴之间的距离分别为/>和/>；/>为纵向加速度；/>为侧向加速度；步骤一中建立的车轮模型如下：因车轮侧向力与车轮侧偏角和垂向载荷有关，因此各轮的车轮侧偏角和垂向载荷表示为：
，
，
，
，
，
，
，
，
，式中，和/>分别为轮式机器人的纵向速度和横向速度；/>为垂向载荷；/>为车轮侧偏角；为质心侧偏角；/>为质心到地面的高度；根据半经验魔术轮胎公式来计算车轮的横向力，车轮横向力模型为：
，式中，表示车轮；/>为刚度因子；/>为曲线形状因子；/>为峰值因子；/>为曲线曲率因子；步骤二中，根据车轮动力学特性，定义的车轮纵向力观测滑模面为；/>是滑模面函数；/>是车轮旋转角速度；/>为车轮旋转角速度估计值，其表达式为：
，式中，为车轮转动惯量；/>为车轮有效转动半径；/>为车轮驱动力矩；/>为车轮纵向力估计值；/>为滑模增益系数；/>为观测器增益因子；/>为符号函数；/>为车轮角速度估计值的微分形式；以作为Lyapunov函数候选，分析自适应滑模观测器的稳定性条件：
，式中，为Lyapunov判定函数；/>为Lyapunov判定函数的微分形式；/>为滑模面函数的微分形式；由于是有界的，所以存在正值/>使得：
，式中，为正值常数项；进一步写为：
，根据自适应滑模观测器的稳定性条件，即，使得：
，即得到：
，为了减少估计结果抖动，选择自适应饱和函如下：
，式中，表示决定函数变化速度的因子；/>为自适应饱和函数，该自适应饱和函数连续且平滑地在[-1, 1]之间转换值，避免突然值变化的现象；进一步修正车轮纵向力估计误差值，最终得到滑模面为：
，当状态误差趋于稳定时，存在，因此，得到车轮纵向力准确估计结果如下：
；步骤三中，构建的非线性系统的平方根容积卡尔曼滤波器的离散状态方程和观测方程如下式所示：
，其中状态变量；测量变量/>；输入变量/>；其中输入的车轮纵向力通过步骤二估计获得；/>为状态过程噪声；/>为测量过程噪声；/>为/>时刻状态向量；/>为状态传递函数；/>为测量传递函数；上角标/>表示对所示量的转置操作；基于轮式机器人动力学模型，采用一阶欧拉离散方法，系统的状态方程表示为：
，式中，为系统采样时间；系统的测量方程被设计为：
；步骤三中，平方根容积卡尔曼滤波器设计过程如下：1)设置状态和协方差矩阵平方根形式的初始值：
，其中，为状态初始值；/>为状态协方差矩阵的平方根因子；/>为状态协方差矩阵；2)计算当前状态容积点：
，其中，为对应/>时刻的状态容积点；/>为对应/>时刻的状态协方差矩阵的平方根；/>为/>时刻状态估计量，/>为容积点集；3)容积点的传播：
，其中，为传播后的容积点；/>为相应状态传递函数；/>为大于0的自然数；4)估计状态预测均值和协方差矩阵的平方根：
，式中，表示对矩阵的三角分解；/>表示过程噪声协方差矩阵/>的平方根；/>为传播过程矩阵；/>为对应时刻的权重系数；/>为根据/>时刻估计的/>时刻状态量；/>为根据/>时刻预测的/>时刻状态协方差矩阵平方根；5)计算更新状态容积点：
，其中，为更新阶段的状态容积点；6)容积点通过测量方程传播：
，其中，为更新阶段传播的容积向量；/>为更新阶段测量方程传递函数；为/>时刻输入量；7)估计量测预测均值和协方差矩阵的平方根：
<式中，表示过程噪声协方差矩阵/>的平方根；矩阵/>为过程矩阵，/>为测量的均值估计值；/>为更新阶段协方差矩阵的平方根；8)计算互协方差矩阵：
<其中，为互协方差矩阵，/>为过程矩阵；9)计算卡尔曼增益：
<其中，为卡尔曼增益；10)更新状态估计量：
，其中，为最终的状态估计值；11)估计更新误差方差的平方根：
，其中，为最终误差方差的平方根；然后得到轮式移动机器人的估计值；步骤四中，蜣螂优化算法用于优化滤波器算法中的噪声协方差矩阵，矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：
，
，
，式中，分别表示纵向车速、侧向车速、横摆角速度的状态噪声值；/>分别表示纵向加速度、侧向加速度的过程噪声值；/>表示对角矩阵；将步骤三中搭建的滤波器算法的估计值与传感器测量值之间的均方根误差作为优化算法目标函数，其适应度函数为：
，式中，为采样时间；/>为/>时刻的真实测量变量；/>为/>时刻由滤波器输出的估计值。