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

本发明涉及轮式机器人监测，特别是涉及一种基于改进卡尔曼滤波的轮式机器人行进状态估计方法。

背景技术：

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、式中，表示对矩阵的三角分解；表示过程噪声协方差矩阵的平方根；为传播过程矩阵；为对应时刻的权重系数；为根据时刻估计的时刻状态量；为根据时刻预测的时刻状态协方差矩阵平方根。

69、5）计算更新状态容积点：

70、

71、其中，为更新阶段的状态容积点。

72、6）容积点通过测量方程传播：

73、

74、其中，为更新阶段传播的容积向量，为更新阶段测量方程传递函数；为时刻输入量。

75、7）估计量测预测均值和协方差矩阵的平方根：

76、

77、式中，表示过程噪声协方差矩阵的平方根；矩阵为过程矩阵；为测量的均值估计值；为更新阶段协方差矩阵的平方根。

78、8）计算互协方差矩阵：

79、

80、其中，为互协方差矩阵；为过程矩阵。

81、9）计算卡尔曼增益：

82、

83、其中，为卡尔曼增益。

84、10）更新状态估计量：

85、

86、其中，为最终的状态估计值。

87、11）估计更新误差方差的平方根：

88、

89、其中，为最终误差方差的平方根。

90、经过此步骤后，得到轮式移动机器人的估计值。

91、作为本技术的一些实施例，步骤四中，蜣螂优化算法用于优化滤波器算法中的噪声协方差矩阵。矩阵和矩阵上的元素被设置为算法寻优维度，如以下公式中所示：

92、

93、

94、

95、式中，分别表示纵向车速、侧向车速、横摆角速度的状态噪声值；分别表示纵向加速度、侧向加速度的过程噪声值；表示对角矩阵。

96、将步骤三中搭建的滤波器算法的估计值与传感器测量值之间的均方根误差作为优化算法目标函数，其适应度函数为：

97、

98、式中，为采样时间；为时刻的真实测量变量；为时刻由滤波器输出的估计值。

99、本技术实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

100、1、本发明根据轮式机器人的车轮动力学特性，建立非线性三自由度动力学模型，设计了一种自适应滑模观测算法快速准确估计车轮纵向力，将机器人车轮运动状态误差引入滑模面分析，为后续滤波器设计提供了稳定可靠的车轮纵向力信息。

101、2、本发明考虑轮式机器人的运动特性，以轮式机器人可实时反馈的行进参数为模型输入量，对建立的轮式机器人动力学系统模型状态进行估计。本发明在容积卡尔曼滤波的基础上，引入平方根滤波的思想对滤波器进行约束，采用蜣螂优化算法对平方根容积卡尔曼滤波器的非高斯噪声自适应寻优，提高了滤波器的鲁棒性，并使得估计精度有所提高。

102、3、本发明涉及的基于轮式移动机器人动力学模型的行进状态参数估计算法结合自适应滑模观测算法估计车轮纵向力，降低了对车载传感器的要求。同时估计过程基本不受如机器人质量等结构参数变化影响，具有较广的适用性和良好的抗干扰性。