(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211143128.4 (22)申请日 2022.09.20 (71)申请人 哈尔滨工业大 学 地址 150001 黑龙江省哈尔滨市南岗区西 大直街92号 (72)发明人 杨国财　韩旭　马正航　史士财　 程思博　张德志　赵亚卿　 (74)专利代理 机构 哈尔滨华夏松花江知识产权 代理有限公司 23213 专利代理师 岳昕 (51)Int.Cl. G06K 9/00(2022.01) G06F 17/11(2006.01) G06F 17/16(2006.01) (54)发明名称 一种机械臂传感器的滤波及高阶状态观测 方法 (57)摘要 一种机械臂传感器的滤波及高阶状态观测 方法， 涉及机械臂传感器信号处理领域， 针对现 有技术中在对测量信号滤波的同时， 不能获得信 号的高阶状态的问题， 包括： 步骤一： 获取机械臂 传感器的测量噪声vn； 步骤二： 获取机械臂传感 器信号， 并基于机械臂传感器信号的泰勒级数， 展开建立机械臂传感器的状态方程： 步骤三： 基 于机械臂传感器的测量噪声vn建立机械臂传感 器的测量方程： 步骤四： 基于机械臂传感器的状 态方程以及机械臂传感器的测量方程设计卡尔 曼观测器， 利用卡尔曼观测器进行高阶状态观 测， 所述高阶状态为一阶状态和二阶状态。 本申 请可以消除被测 信号的噪声影 响， 同时获得信号 的高阶状态(信号的一阶状态和信号的二阶状 态)。 权利要求书3页 说明书6页 附图4页 CN 115496099 A 2022.12.20 CN 115496099 A 1.一种机 械臂传感器的滤波及高阶状态 观测方法， 其特 征在于包括以下步骤： 步骤一： 获取机 械臂传感器的测量噪声vn； 步骤二： 获取机械臂传感器信号， 并基于机械臂传感器信号的泰勒级数， 展开建立机械 臂传感器的状态方程； 步骤三： 基于 机械臂传感器的测量噪声vn建立机械臂传感器的测量方程； 步骤四： 基于机械臂传感器的状态方程以及机械臂传感器的测量方程设计卡尔曼观测 器， 利用卡尔曼观测器进行高阶状态 观测， 所述高阶状态为 一阶状态和二阶状态。 2.根据权利要求1所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述步骤一的具体步骤为： 步骤一一： 将机械臂传感器静置， 并使其测量稳定目标， 采集1000次有效数据， 记为 N1000； 步骤一二： 计算10 00个数据的标准差， 并将其作为测量噪声： vn＝std(N1000) 其中， std()为 求解数据标准差的运 算函数。 3.根据权利要求2所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述机械臂传感器的状态方程表示 为： 其中， 下标k表示上一时刻传感器测量值， k+1表示当前时刻传感器测量值， k的取值范 围为0， 1， 2， …， n的整数； Δt为采样周期； X为传感器测量值， 为机械臂传感器测量值的一 阶状态， 为机械臂传感器测量 值的二阶状态； Qp为状态在变化过程中的过程噪声。 4.根据权利要求3所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述测量方程表示 为： 其中， Yk+1是机械臂传感器的测量 值。 5.根据权利要求4所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述步骤四中卡尔曼观测器具体执 行如下步骤： 状态后验估计值的初始值记为 和 其中， 是传感器测量的初始值， 和 均为0； 状态后验 估计协方差矩阵的初始值记为 抑制模型误差的系数记为γ； 开始循环， 每 个周期内， 做如下运 算： 步骤四一： 根据状态后验估计值的初始值 和 获取状态先验估计值 权　利　要　求　书 1/3 页 2 CN 115496099 A 2和 步骤四二： 根据状态后验估计协方差矩阵的初始值 以及抑制模型误差的系数γ获取 状态先验 估计协方差矩阵 步骤四三： 根据状态先验 估计协方差矩阵 以及测量噪声vn得到卡尔曼增益 值Kk+1； 步骤四四： 根据状态先验估计值 机械臂传感器的测量值Yk+1以及卡 尔曼增益 值Kk+1得到状态后验 估计值 和 步骤四五： 根据状态先验估计协方差矩阵 以及卡尔曼增益值Kk+1得到状态后验估 计协方差矩阵 步骤四六： 将状态后验估计协方差矩 阵 状态后验估计值 和 作为 下一循环的初始值， 重新执 行步骤四一， 进行循环； 步骤四四中得到的状态后验估计值 即为传感器测量信息的滤波值， 和 即 为观测到的传感器信息一阶状态和二阶状态。 6.根据权利要求5所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述状态先验 估计值 和 表示为： 7.根据权利要求6所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述状态先验 估计协方差矩阵 表示为： 8.根据权利要求7所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述卡尔曼增益 值Kk+1表示为： 9.根据权利要求8所述的一种机械臂传感器的滤波及高阶状态观测方法， 其特征在于 所述状态后验 估计值 和 表示为： 权　利　要　求　书 2/3 页 3 CN 115496099 A 3