[发明专利]一种机械臂传感器的滤波及高阶状态观测方法

权利要求书

1.一种机械臂传感器的滤波及高阶状态观测方法，其特征在于包括以下步骤：

步骤一：获取机械臂传感器的测量噪声v_n；

步骤二：获取机械臂传感器信号，并基于机械臂传感器信号的泰勒级数，展开建立机械臂传感器的状态方程；

步骤三：基于机械臂传感器的测量噪声v_n建立机械臂传感器的测量方程；

步骤四：基于机械臂传感器的状态方程以及机械臂传感器的测量方程设计卡尔曼观测器，利用卡尔曼观测器进行高阶状态观测，所述高阶状态为一阶状态和二阶状态；

所述步骤一的具体步骤为：

步骤一一：将机械臂传感器静置，并使其测量稳定目标，采集1000次有效数据，记为N₁₀₀₀；

步骤一二：计算1000个数据的标准差，并将其作为测量噪声：

V_n＝std(N_i000)

其中，std()为求解数据标准差的运算函数；

所述机械臂传感器的状态方程表示为：

其中，下标k表示上一时刻传感器测量值，k+1表示当前时刻传感器测量值，k的取值范围为0，1，2，…，n的整数；Δt为采样周期；X为传感器测量值，为机械臂传感器测量值的一阶状态，为机械臂传感器测量值的二阶状态；Q_p为状态在变化过程中的过程噪声；

所述测量方程表示为：

其中，Y_k+1是机械臂传感器的测量值；

所述步骤四中卡尔曼观测器具体执行如下步骤：

状态后验估计值的初始值记为和其中，是传感器测量的初始值，和均为0；状态后验估计协方差矩阵的初始值记为抑制模型误差的系数记为γ；

开始循环，每个周期内，做如下运算：

步骤四一：根据状态后验估计值的初始值和获取状态先验估计值和

步骤四二：根据状态后验估计协方差矩阵的初始值以及抑制模型误差的系数γ获取状态先验估计协方差矩阵

步骤四三：根据状态先验估计协方差矩阵以及测量噪声v_n得到卡尔曼增益值K_k+1；

步骤四四：根据状态先验估计值机械臂传感器的测量值Y_k+1以及卡尔曼增益值K_k+1得到状态后验估计值和

步骤四五：根据状态先验估计协方差矩阵以及卡尔曼增益值K_k+1得到状态后验估计协方差矩阵

步骤四六：将状态后验估计协方差矩阵状态后验估计值和作为下一循环的初始值，重新执行步骤四一，进行循环；

步骤四四中得到的状态后验估计值即为传感器测量信息的滤波值，和即为观测到的传感器信息一阶状态和二阶状态；

所述状态先验估计值和表示为：

所述状态先验估计协方差矩阵表示为：

所述卡尔曼增益值K_k+1表示为：

所述状态后验估计值和表示为：

所述状态后验估计协方差矩阵表示为：