本发明专利技术公开了一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，首先，根据轮式机器人的动力学特性，构建纵向、侧向、横摆三自由度动力学模型及车轮模型；然后，定义车轮纵向力观测滑模面，构建自适应滑模观测器，实现机器人的车轮纵向力稳定观测估计；再然后，构建平方根容积卡尔曼滤波器，采用平方根准则对平方根容积卡尔曼滤波器进行待估计参数的修正及更新；最后，采用蜣螂优化算法对平方根容积卡尔曼滤波器的非高斯噪声进行寻优处理，实现轮式机器人行进状态参数的精准估计。本发明专利技术可用于轮式机器人控制领域，在滤波器估计中增加了机器人车轮纵向力，并对滤波器参数进行自适应优化，具有良好的状态估计准确性及鲁棒性。
【技术实现步骤摘要】
本专利技术涉及轮式机器人监测，特别是涉及一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法。
技术介绍
1、机器人具有控制精确、节省人力成本、安全高效的优点，是实现智能制造转型的重要手段。轮式智能移动机器人（以下简称轮式机器人）是机器人发展的主要类型，与传统机器人相比，其驱动方式可独立控制，动力传递效率高、速度快，更容易实现轮式智能移动机器人的主动安全控制，是未来轮式智能移动机器人的主要发展方向。
2、轮式机器人的主动安全控制前提是准确地获取行进过程中的状态参数信息。由于技术限制或部分传感器价格昂贵，其轮胎力及横摆运动状态等信息难以直接测量。因此，利用部分现有低成本的传感器来准确的估计这些不易测量状态参数，是本领域技术人员需要解决的技术问题。
3、目前，轮式机器人行进状态估计方法主要包括卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波等，其中，卡尔曼滤波广泛应用于线性模型，而扩展卡尔曼滤波则克服了卡尔曼滤波无法处理非线性模型的问题，无迹卡尔曼滤波则使用无迹变换将非线性系统转化为线性系统进行处理，但滤波器系统结构参数变得更为复杂，在噪声影响、传感器失效等情况下难以确定最优滤波参数。
技术实现思路
1、本申请实施例通过提供一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，通过结合自适应滑模观测器及卡尔曼滤波器，实现轮式机器人状态的实时准确估计。
2、本申请实施例提供了一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，包括以下步骤：
>3、步骤一、根据轮式机器人的动力学特性，构建纵向、横向、横摆非线性三自由度动力学模型及车轮模型；4、步骤二、根据车轮动力学特性，定义车轮纵向力观测滑模面，构建自适应滑模观测器对车轮纵向力进行估计，实现机器人的车轮纵向力稳定观测估计；
5、步骤三、基于步骤一中的非线性三自由度动力学模型和步骤二中观测估计的车轮纵向力，构建平方根容积卡尔曼滤波器，将平方根容积卡尔曼滤波器与机器人动力学系统对接，确定平方根容积卡尔曼滤波器的待估计量、输入量和观测量，并采用平方根准则对平方根容积卡尔曼滤波器进行待估计参数的修正及更新；
6、步骤四、基于步骤三中构建的平方根容积卡尔曼滤波器，采用蜣螂优化算法对平方根容积卡尔曼滤波器的非高斯噪声进行寻优处理，增强滤波器的鲁棒性，最终实现轮式机器人行进状态参数的精准估计。
7、作为本申请的一些实施例，步骤一中，建立的非线性三自由度动力学模型如下式所示：
8、
9、
10、
11、式中，为横摆角速度；为横摆角速度的微分形式；为前轮转角；为机器人总质量；为机器人质心绕轴的转动惯量；和分别为纵向力和横向力，表示左前轮，表示右前轮，表示左后轮，表示右后轮，例如：表示左前轮的纵向力，表示右前轮的纵向力，表示左后轮的纵向力，表示右后轮的纵向力；表示前轮距，表示后轮距；质心与前后轴之间的距离分别为和；为纵向加速度；为侧向加速度。
12、作为本申请的一些实施例，步骤一中，车轮侧向力与车轮侧偏角和垂向载荷有关，因此各轮的车轮侧偏角和垂向载荷分别为：
13、
14、
15、
16、
17、
18、
19、
20、
21、
22、式中，和分别为轮式机器人的纵向速度和横向速度；为垂向载荷；为车轮侧偏角；为质心侧偏角；为质心到地面的高度。
23、根据半经验魔术轮胎公式来计算车轮的横向力，横向力模型为：
24、
25、式中，表示车轮，表示车轮的横向力；为刚度因子；为曲线形状因子；为峰值因子；为曲线曲率因子。
26、作为本申请的一些实施例，步骤二中，首先，定义车轮纵向力观测滑模面为；是滑模面函数；是车轮旋转角速度；为车轮旋转角速度估计值，其表达式为：
27、
28、式中，为车轮转动惯量；为车轮有效转动半径；为车轮驱动力矩；为车轮纵向力估计值；为滑模增益系数；为观测器增益因子；为符号函数；为车轮角速度估计值的微分形式。
29、以作为lyapunov函数候选，分析自适应滑模观测器的稳定性条件：
30、
31、式中，为lyapunov判定函数；为lyapunov判定函数的微分形式；为滑模面函数的微分形式。
32、由于是有界的，所以存在正值使得：
33、
34、式中，为正值常数项。
35、进一步写为：
36、
37、根据自适应滑模观测器的稳定性条件，即，使得：
38、
39、即得到：
40、
41、为了减少估计结果抖动，选择自适应饱和函如下：
42、
43、式中，表示决定函数变化速度的因子；为自适应饱和函数，该自适应饱和函数连续且平滑地在[-1,1]之间转换值，避免突然值变化的现象。
44、进一步修正车轮纵向力估计误差值，最终得到滑模面为：
45、
46、当状态误差趋于稳定时，存在；因此，得到车轮纵向力准确估计结果如下：
47、
48、作为本申请的一些实施例，步骤三中，构建的非线性系统的平方根容积卡尔曼滤波器的离散状态方程和观测方程如下式所示：
49、
50、其中状态变量；测量变量；输入变量；其中输入的车轮纵向力通过步骤二估计获得；为状态过程噪声；为测量过程噪声；为时刻状态向量；为状态传递函数；为测量传递函数；上角标表示对所示量的转置操作。
51、作为本申请的一些实施例，步骤三中，基于轮式机器人动力学模型，采用一阶欧拉离散方法，系统的状态方程表示为：
52、
53、式中，为系统采样时间。
54、系统的测量方程被设计为：
55、
56、作为本申请的一些实施例，步骤三中，平方根容积卡尔曼滤波器设计过程如下：
57、1）设置状态和协方差矩阵平方根形式的初始值：
58、
59、其中，为状态初始值；为状态协方差矩阵的平方根因子；为状态协方差矩阵。
60、2）计算当前状态容积点：
61、
62、其中，为对应时刻的状态容积点；为对应时刻的状态协方差矩阵的平方根；为时刻状态估计量；为容积点集。
63、3）容积点的传播：
64、
65、其中，为传播后的容积点；为相应状态传递函数；为大于0的自然数。
66、4）估计状态预测均值和协方差矩阵的平方根：
67、
68、式中，表示对矩阵的三角分解；表示过程噪声协方差矩阵的平方根；为传播过程矩阵；为对应时刻的权重系数；为根据时刻估计的时刻状态量；为根据时刻预测的时刻本文档来自技高网...
【技术保护点】
1.一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤一建立的非线性三自由度动力学模型如下式所示：
3.根据权利要求2所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤一中建立的车轮模型如下：
4.根据权利要求3所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤二中，根据车轮动力学特性，定义的车轮纵向力观测滑模面为；是滑模面函数；是车轮旋转角速度；为车轮旋转角速度估计值，其表达式为：
5.根据权利要求4所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤三中，构建的非线性系统的平方根容积卡尔曼滤波器的离散状态方程和观测方程如下式所示：
6.根据权利要求5所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤三中，平方根容积卡尔曼滤波器设计过程如下：
7.根据权利要求6所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤四中，蜣螂优化算法用于优化滤波器算法中的噪声协方差矩阵，矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：
...
【技术特征摘要】
1.一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤一建立的非线性三自由度动力学模型如下式所示：
3.根据权利要求2所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤一中建立的车轮模型如下：
4.根据权利要求3所述的一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法，其特征在于，步骤二中，根据车轮动力学特性，定义的车轮纵向力观测滑模面为；是滑模面函数；是车轮旋转角速度；为车轮旋转角速度估...
【专利技术属性】
技术研发人员：刘前结，辛汉藩，
申请(专利权)人：江西省汉达隆科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。