一种基于中间变量的机械臂非线性运动状态估计方法

本发明属于机械臂控制领域，涉及一种基于中间变量的机械臂非线性运动状态估计方法，特别是具有未知输入的非线性非高斯噪声条件下机械臂运动的状态估计方法。

背景技术：

1、随着制造业的发展，机械设备的重要性越来越高，其中机械臂尤为突出。它们可以在生产线中发挥重要作用，而且由于其高度的灵活性和可编程性，可以进一步提高生产效率。然而，在实际使用过程中，由于受到各种因素的影响，如外部干扰、内部摩擦力、传感器噪声等，准确估计机械臂的状态始终是一项重要而又艰巨的任务。虽然已经有了许多方法用于解决此问题，但由于传感器数据和机械臂动力学模型之间的不匹配，传感器数据中存在的噪声和不确定性，以及硬件设备的限制等原因，现有的方法难以估计具有未知输入的非线性非高斯噪声条件下的机械臂运动状态。因此，继续研究和创新机械臂状态估计算法是非常重要的。

2、当前，状态估计方法通常可以分为基于数据驱动的状态估计方法和基于模型的状态估计方法两类。基于数据驱动的状态估计方法是分析系统的数据和估计系统状态的方法，而不依赖于系统的数学模型。它的主要思想是从收集到的大量数据中学习系统的行为，并根据这些数据进行状态估计。这类方法使用机器学习算法和数据处理技术，实时对系统状态进行估计，不需要事先建立系统的数学模型，且可以处理高维状态空间和大规模数据。但是基于数据驱动的状态估计方法通常需要大量高质量的数据用于训练和学习，对数据的质量和多样性要求较高，同时需要复杂的训练过程和算法调优。

3、基于模型的状态估计方法利用系统的数学模型进行状态估计，通过模型的预测和更新步骤捕捉系统的动态行为和各种影响因素，可以有效地利用系统的输入和测量数据，减少计算负担，提高状态估计的实时性，从而准确预测和估计系统的状态。该方法通过调整模型的参数或改变假设条件，评估状态估计结果对模型假设的敏感程度，从而进一步理解系统行为，比较适用于机器臂的信息物理系统。在基于模型的状态估计方法中，常见的方法是基于观测器的状态估计方法和基于滤波器的状态估计方法。基于观测器的状态估计方法能够有效处理模型的不确定性和外界干扰，但观测器的设计较难，且其收敛性证明较为复杂。而基于滤波器的状态估计方法具有较低的计算复杂度和实时性，并且无需复杂的收敛性证明过程，更容易应用和实现。

4、目前基于滤波器的状态估计方法主要采用标准卡尔曼滤波、扩展卡尔曼滤波、自适应卡尔曼滤波、基于中间变量的卡尔曼滤波等滤波方法进行状态估计。其中标准卡尔曼滤波是一种基于线性系统的滤波方法，需要假设系统的噪声是高斯分布的，但是无法应用于非线性系统的状态估计。

5、基于扩展卡尔曼滤波的状态估计方法根据泰勒级数的原理，将非线性模型转化成线性模型进行状态估计。如：如针对汽车电池的安全问题，yang等建立等效电路模型，引入衰落因子，改进扩展卡尔曼滤波，对参数进行在线评估。naseri等基于维纳结构对电路模型建模，提出基于扩展核递归最小二乘法的扩展卡尔曼滤波方法，对电池充电状态进行估计。lai等提出扩展卡尔曼滤波和数据驱动结合的方法。该方法采用三阶扩展卡尔曼滤波方法与动态数据驱动方法结合的方法共同估计电池的荷电状态和容量，并建立离散阿伦尼乌斯老化模型估计容量，解决其参数失配问题。但是这些方法没有考虑系统未知输入的干扰和非高斯观测噪声的情况。

6、基于自适应卡尔曼滤波的状态估计方法可自动调整协方差矩阵和状态转移矩阵的能力，能够更好地适应系统的变化和噪声的变化，从而提高状态估计的准确性和鲁棒性，如xu等基于等效电路，通过老化实验分析锂离子电池的容量衰减特性，采用自适应扩展卡尔曼滤波进行两次滤波，协同估计锂电池的健康状态和充电状态。wang等针对车辆自动化控制中传感器数据受到噪声影响问题，采用自适应扩展卡尔曼滤波平滑汽车跟随模型的传感器数据，再通过单类支持向量机对传感器异常数据进行检测，实现更低延迟的异常检测性能。zhang等利用遗忘因子对噪声的统计特性进行自适应校正，并根据新息序列对误差协方差矩阵进行自适应校正，实现噪声未知情况下的充电状态估计。shrivastava等分别设置系统状态的遗忘因子和测量误差的遗忘因子，根据系统状态及测量误差动态调整遗忘因子，能够在不同环境和系统变化下提供状态估计。sun等采用智能自适应扩展卡尔曼滤波方法，通过最大似然函数检测误差新息协方差矩阵，从而提高充电状态估计的精度，具有较强的鲁棒性。jiang等利用自适应滤波方法更新噪声变量，对协方差矩阵进行平方根分解，并使用遗忘因子递归最小二乘法进行参数调整，提高估计结果准确率，消除计算误差。虽然自适应卡尔曼滤波适用于线性和非线性系统，但是无法解决系统未知输入的干扰。

7、因此部分学者侧重于引入中间变量，描述系统状态及未知输入的扩展状态，研究基于中间变量的卡尔曼滤波方法实现状态估计。如zhou等引入中间变量，通过动态扩展状态对未知输入和系统状态之间的关系进行建模，设计一个修正的卡尔曼滤波来估计系统状态和未知输入。hou等通过坐标系变换，将具有未知输入的线性系统观测器问题转换成标准观测器问题，采用龙伯格观测器进行状态估计。zhou等针对网络物理系统中的传感器和执行器攻击问题，引入中间变量，设计攻击重构方法，建立凸优化问题获取估计方法的增益，从而确定受到攻击的大小。huang等针对单个执行器故障，引入一个中间变量对系统进行状态估计，针对同时发生执行器和传感器故障的情况，引入两个中间变量，设计系统扩展状态，同时估计系统状态、故障和干扰。但是这些方法主要考虑的线性系统，且假设噪声均为高斯白噪声。

8、综上所述，由于具有未知输入的机械臂运动状态大多表现为非线性非高斯形式，目前针对机械臂运动的状态估计问题的大多数方法适用于线性高斯系统，且基于滤波器的状态估计方法没有考虑非线性运行过程中会受到未知输入的干扰且存在非高斯的观测噪声的情况，这些方法难以精确和高效地实现具有未知输入的非线性非高斯噪声条件下机械臂运动的状态估计。

技术实现思路

1、为了克服具有未知输入的非线性非高斯噪声条件下机械臂运动的状态估计问题，本发明提供一种基于中间变量的机械臂运动状态估计方法，即提供具有未知输入的机械臂非线性运动状态估计模型、观测噪声符合高斯分布和非高斯分布下的状态估计方法，从而实现高精度和高鲁棒性的状态估计。

2、本发明解决其技术问题所采用的技术方案是：

3、一种基于中间变量的机械臂非线性运动状态估计方法，包括以下步骤：

4、步骤1、建立具有未知输入的机械臂非线性运动系统的状态空间模型，其状态方程为线性方程，观测方程为非线性方程；

5、步骤2、引入中间变量，得到同时包含系统状态和未知输入的系统状态zn，并将系统模型转化成系统状态zn对应的系统模型；

6、步骤3、基于步骤2得到的系统模型，对系统状态估计和未知输入估计，此时从非线性模型变化为包含扩展状态的线性模型，基于扩展卡尔曼滤波，判断系统观测噪声vn是否高斯；若系统噪声vn为高斯噪声，则执行步骤4，否则执行步骤14；

7、步骤4：选择单隐层前馈神经网络，模型结构包括输入层、隐藏层和输出层，采用tanh函数作为激活函数，从而构建状态值误差补偿卷积神经网络模型和观测值误差补偿卷积神经网络模型；通过均方误差损失函数进行训练，利用反向传播算法优化网络权重和偏置；完成训练后，获得状态值误差补偿卷积神经网络模型和观测值误差补偿卷积神经网络模型的网络权重和偏置；

8、步骤5：将状态值误差补偿卷积神经网络模型和观测值误差补偿卷积神经网络模型与运动模型相结合，将当前运动状态值扩展为其中表示扩展的运动状态值，φz,n-1表示状态值参数集合，φy,n-1表示观测值参数集合，nnz(zn-1,φz,n-1)表示状态值误差补偿卷积神经网络模型输出值，将观测值转变为其中nny(zn,φy,n-1)表示观测值误差补偿卷积神经网络模型；

9、步骤6：设置所述状态估计模型在初始时刻的各参数的初始值；

10、步骤7：将卡尔曼滤波的后验估计值输入到训练好的状态值误差补偿卷积神经网络模型；

11、步骤8：根据状态值误差补偿nnz和观测值误差补偿nny，计算模型的先验状态估计和观测预测值

12、步骤9、计算先验状态误差ek|k-1，获得先验误差协方差矩阵pn|n-1；

13、步骤10、计算后验状态误差en，得到后验误差协方差矩阵pn；

14、步骤11、根据后验误差协方差计算n时刻的最优卡尔曼增益kn；

15、步骤12：通过卡尔曼增益kn简化后验误差协方差矩阵；

16、步骤13、计算后验扩展状态输出系统观测噪声vn为高斯情况下的机器臂非线性运动状态估计值，判断n是否大于指定阈值，如果大于，则结束本方法，否则，n＝n+1，跳到步骤7；

17、步骤14、如果系统观测噪声为非高斯噪声，则令n＝1，设置状态估计模型在初始时刻的各参数的初始值；。

18、步骤15、计算n时刻先验状态估计和观测预测值；

19、步骤16、通过先验状态误差计算先验误差协方差矩阵，引入衰减因子，利用衰减因子更新新息协方差，得到衰减后的噪声协方差；利用衰减后的噪声协方差更新当前状态最优卡尔曼增益；

20、步骤17、根据最优卡尔曼增益计算后验误差协方差矩阵和后验扩展状态，判断n是否大于指定阈值，如果大于，则结束本方法，否则，n＝n+1，跳到步骤15。

21、进一步，所述步骤1中，所述机械臂非线性运动系统的状态空间模型构建方法如下：

22、1.1)一个机械臂非线性运动模型的表达形式为：

23、

24、其中，xn＝[p1 p2 p3 α1 α2 α3]t为机械臂末端状态矩阵，p1表示械臂末端横向的n时刻位置，p2表示械臂末端纵向的n时刻位置，p3表示械臂末端垂直的n时刻位置，α1表示械臂末端横向的n时刻位置，α2表示械臂末端纵向的n时刻位置，α3表示械臂末端垂直的n时刻位置，un为机械臂输入矩阵，an∈r6×6为状态转移矩阵，bn∈r6×2为观测矩阵，yk＝[p4 p5 p6]t为机械臂末端观测矩阵，p4表示械臂末端横向n时刻观测的位置，p5表示械臂末端纵向n时刻观测的位置，p6表示械臂末端垂直n时刻观测的位置，h(x)为测量过程中的非线性函数，由双目视觉获取，vn为系统观测噪声，设置为高斯白噪声；

25、1.2)考虑系统(1)运行过程中会受到突发状况和扰动、故障和建模不准确等未知输入的干扰，因此引入未知输入变量dn和fn，将系统模型转化为：

26、

27、其中，dn和fn为未知输入，分别设置为随机值，en和fn分别为未知输入对应的矩阵。

28、再进一步，所述步骤2中，系统状态zn对应的系统模型构建方法如下：

29、2.1)引入中间变量对模型(2)进行改进，其中μ是可调节变量，得

30、

31、

32、将(2)代入(3)，可得

33、

34、其中，

35、2.2)令包含系统状态及未知输入状态的扩展状态中间变量其中δfn+1＝fn+1-fn，δd＝dn-dn-1。则系统(2)引入中间变量，采用新的扩展状态后，将新的状态方程修改为：

36、

37、其中，

38、2.3)利用泰勒一阶展开对观测方程进行线性化，解决系统观测方程为非线性方程的问题，转化后的系统观测方程为：

39、yn＝hxn+hxn|n-1+fnfn+vn  (7)

40、其中，h∈rn为雅克比矩阵，省略x的高阶项，xn|n-1为n-1时刻估计n时刻的系统状态矩阵，引入中间变量，采用新的扩展状态后，将系统观测方程修改为

41、

42、其中，

43、2.4)最终可以得到具有中间变量的系统模型，表达式为：

44、

45、再进一步，所述步骤5中，将状态值误差补偿卷积神经网络模型和观测值误差补偿卷积神经网络模型与运动模型相结合，将当前运动状态值扩展为其中表示扩展的运动状态值，φz,n-1表示状态值参数集合，φy,n-1表示观测值参数集合，nnz(zn-1,φz,n-1)表示状态值误差补偿卷积神经网络模型输出值，则其对应的状态转移矩阵变为

46、

47、将观测值转变为其中nny(zn,φy,n-1)表示观测值误差补偿卷积神经网络模型；

48、所述步骤6中，n＝1，初始化运动状态矩阵a1＝i(6)、控制输入矩阵b1＝[1 1 10.01 0.01 0.01]t、未知输入矩阵d1＝[0 cos(0.1) sin(0.1) 0 0 0]、未知输入矩阵f1＝[1 1 0]、系统观测噪声协方差矩阵r＝i(3)×0.1、中间变量τ1＝0、未知输入d1＝0、未知输入f1＝0、系统状态x1＝[113 -140 231 34.1 39.3 -41.7]t，扩展状态z1＝[x1 τ1 f1]t＝[113 -140 231 34.1 39.3 -41.7 0 0]t，可调节变量μ＝1.5、控制输入u＝1、协方差矩阵

49、所述步骤7中，将卡尔曼滤波的后验估计值输入到训练好的状态值误差补偿卷积神经网络模型，根据(11)计算状态值误差补偿nnz。

50、

51、其中表示第一层到第二层的权重，表示第一层到第二层的偏置，表示第二层到第三层的权重，表示第二层到第三层的偏置。将卡尔曼滤波的观测值输入训练好的观测值误差补偿卷积神经网络模型，根据(12)计算观测值误差补偿nny。

52、

53、其中表示第一层到第二层的权重，表示第一层到第二层的偏差，表示第二层到第三层的权重，表示第二层到第三层的偏差。

54、所述步骤8中，根据状态值误差补偿nnz和观测值误差补偿nny，通过(13)计算模型的先验状态估计和观测预测值

55、

56、所述步骤9中，通过(14)计算先验状态误差ek|k-1

57、

58、则可获得误差协方差矩阵pn|n-1

59、

60、其中，e(·)为矩阵的期望值计算函数。考虑到en-1和δrn-1相互独立，则可得则可通过(16)计算误差协方差矩阵pn|n-1为

61、

62、其中，变量

63、所述步骤10中，根据系统模型(9)的观测方程，通过(17)计算后验状态误差en

64、

65、则可得到误差协方差矩阵pn

66、

67、考虑到en|n-1和hnxn|n-1+vn相互独立，令则如公式(19)所示可得到误差协方差矩阵

68、

69、所述步骤11中，对(19)两边求迹，可得

70、

71、其中，tr(·)为矩阵的迹，表示矩阵主对角线上个元素之和。考虑到协方差矩阵为对角矩阵，将卡尔曼增益kn作为自变量，对(20)两边对kn求导，且令其等于0，可通过(21)，计算卡尔曼增益kn。

72、

73、所述步骤12中，将卡尔曼增益kn带入(19)，通过(22)计算误差协方差矩阵

74、

75、所述步骤13中，通过(23)，计算后验扩展状态输出系统观测噪声vn为高斯情况下的机器臂非线性运动状态估计值，n＝n+1，判断n是否大于指定阈值，如果大于，则结束本方法，否则，跳到步骤7，

76、

77、所述步骤14中，如果系统观测噪声为非高斯噪声，则令n＝1，初始化运动状态矩阵a1＝i(6)、控制输入矩阵b1＝[1 1 1 0.01 0.01 0.01]t、未知输入矩阵d1＝[0 cos(0.1)sin(0.1) 0 0 0]、未知输入矩阵f1＝[1 1 0]、系统观测噪声协方差矩阵r＝i(3)×0.1、中间变量τ1＝0、未知输入d1＝0、未知输入f1＝0、系统状态x1＝[113 -140 231 34.1 39.3 -41.7]t，扩展状态z1＝[x1 τ1 f1]t＝[113 -140 231 34.1 39.3 -41.7 0 0]t，可调节变量μ＝1.5、控制输入u＝1、协方差矩阵

78、所述步骤15中，考虑到zn|n-1，pn|n-1，zn，pn|n中均没有包含噪声vn，且yn|n-1为忽略噪声影响计算的准确值，因此通过(9)计算先验估计值zn|n-1、通过(16)计算先验误差协方差矩阵pn|n-1，

79、所述步骤16中，令新息序列为ηn-1＝yn-1-h(zn|n-1)，可得高斯噪声下新息序列协方差公式：

80、

81、引入衰减因子αn，令可得新的新息协方差为

82、

83、令衰减因子αn值为：

84、

85、其中通过(27)计算卡尔曼增益

86、

87、所述步骤17中，令通过(28)计算后验误差协方差矩阵pn|n，通过(29)计算后验估计值zn、输出系统观测噪声vn为非高斯情况下的机器臂非线性运动状态估计值，n＝n+1，判断n是否大于指定阈值，如果大于，则结束本方法，否则，跳到步骤15；

88、

89、zn＝zn|n-1+kn(yn-yn|n-1)  (29)。

90、本发明的有益效果主要表现在：本发明考虑未知输入为非高斯信号下，引入中间变量和包含运动状态和未知输入的扩展状态，构建具有未知输入的机械臂非线性运动状态估计模型，符合实际机器臂非线性运动过程。针对模型构建过程中扩展卡尔曼滤波方法忽略高阶导数项而导致的模型误差问题，采用深度学习方法对模型进行校正，提高状态估计的准确率。针对观测噪声符合高斯分布时，采用扩展卡尔曼滤波方法对系统状态进行估计。针对观测噪声符合非高斯分布时，利用衰减因子更新卡尔曼增益，改进采用自适应卡尔曼滤波方法，从而提高估计精度。本发明所提出的方法引入和融合中间变量、卡尔曼滤波和深度学习方法的思想，有效解决了机械臂非线性运动状态下的未知输入和噪声不同约束条件的问题，实现具有未知输入的非线性非高斯噪声条件下机械臂运动的状态估计，提高了估计精度和鲁棒性。