CN115166635A - 基于风险敏感fir滤波的机器人定位方法 - Google Patents

基于风险敏感fir滤波的机器人定位方法 Download PDF

Info

Publication number
CN115166635A
CN115166635A CN202210724152.0A CN202210724152A CN115166635A CN 115166635 A CN115166635 A CN 115166635A CN 202210724152 A CN202210724152 A CN 202210724152A CN 115166635 A CN115166635 A CN 115166635A
Authority
CN
China
Prior art keywords
performance criterion
criterion
final performance
risk
alpha
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210724152.0A
Other languages
English (en)
Other versions
CN115166635B (zh
Inventor
赵顺毅
刘义
栾小丽
刘飞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangnan University
Original Assignee
Jiangnan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangnan University filed Critical Jiangnan University
Priority to CN202210724152.0A priority Critical patent/CN115166635B/zh
Publication of CN115166635A publication Critical patent/CN115166635A/zh
Application granted granted Critical
Publication of CN115166635B publication Critical patent/CN115166635B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0252—Radio frequency fingerprinting

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明涉及一种基于风险敏感FIR滤波的机器人定位方法，包括：建立机器人运动的状态空间模型，建立测量值和当前时刻的系统状态；根据测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用联合概率密度函数优化初始化性能准则得到最终的性能准则；将最终的性能准则进行代价函数变形；根据α的不同取值将最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值。本发明可以在具有不确定性时实现对扫地机器人位置的有效、精确定位。

Description

基于风险敏感FIR滤波的机器人定位方法
技术领域
本发明涉及机器人定位技术领域，尤其是指一种基于风险敏感FIR滤波的机器人定位方法。
背景技术
目前家用扫地机器人的应用十分普及，在机器人的工作过程中，定位功能是一个不可忽视的功能。而扫地机器人在实际工作中时会遇到许多具有不确定性的干扰，这给机器人的定位带来了许多负面影响，严重影响定位的准确性。而一旦扫地机器人出现了定位偏差，就会出现清扫不到位甚至破坏家具的情况。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中的不足，提供一种基于风险敏感FIR滤波的机器人定位方法，可以在具有不确定性时实现对扫地机器人位置的有效、精确定位。
为解决上述技术问题，本发明提供了一种基于风险敏感FIR滤波的机器人定位方法，包括以下步骤：
S1：建立机器人运动的状态空间模型；
S2：在所述机器人运动的状态空间模型下建立测量值和当前时刻的系统状态；
S3：根据所述测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用所述联合概率密度函数优化所述初始化性能准则得到最终的性能准则；
S4：将所述最终的性能准则进行代价函数变形；
S5：根据α的不同取值将所述最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值。
作为优选的，所述机器人运动的状态空间模型为：
xn+1＝Axn+Bun+Gwn，
yn＝Cxn+vn；
其中，xn表示n时刻的系统状态，xn的维数为r；un为n时刻的系统输入，un的维数为l；wn是过程噪声，wn的维数为p；yn表示n时刻的系统输入，yn的维数为q；A为状态转移矩阵，B为输入矩阵，G为噪声增益矩阵，C为测量矩阵；vn是测量噪声，vn的维数是q，wn和vn均是零均值且不相关的高斯白噪声，wn的噪声协方差矩阵为Q，vn的噪声协方差矩阵为R；xn和yn在时间区间[n-N,n]内以批处理形式表达，N是窗口长度。
作为优选的，所述测量值Yn-1为：
Figure BDA0003712695050000021
其中，Yn-1、
Figure BDA0003712695050000022
Un-1、
Figure BDA0003712695050000023
Wn-1和Vn-1分别定义为：
Figure BDA0003712695050000024
Figure BDA0003712695050000025
Figure BDA0003712695050000031
Figure BDA0003712695050000032
Figure BDA0003712695050000033
Figure BDA0003712695050000034
Figure BDA0003712695050000035
作为优选的，所述
Figure BDA0003712695050000036
是噪声项，
Figure BDA0003712695050000037
的协方差ΠN为：
Figure BDA0003712695050000038
其中，QN、RN定义为：
Figure BDA0003712695050000039
Figure BDA00037126950500000310
表示形成以N个Q为对角线，其余元素为0的矩阵；
Figure BDA00037126950500000311
Figure BDA0003712695050000041
表示形成以N个R为对角线，其余元素为0的矩阵。
作为优选的，所述当前时刻的系统状态xn为：
xn＝ANxn-N+MBUn-1+MGWn-1；
其中，MB、MG的定义为：
Figure BDA0003712695050000042
Figure BDA0003712695050000043
作为优选的，所述根据所述测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用所述联合概率密度函数优化所述初始化性能准则得到最终的性能准则，具体为：
S3-1：建立所述含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则
Figure BDA0003712695050000044
其中，
Figure BDA0003712695050000045
表示系统状态的估计值，
Figure BDA0003712695050000046
表示存在
Figure BDA0003712695050000047
使
Figure BDA0003712695050000048
最小，E()是数学期望，en是误差；
S3-2：建立所述联合概率密度函数p(xn-N,Wn-1,Vn-1)为：
Figure BDA0003712695050000049
其中，D、Jn的定义为：
Figure BDA0003712695050000051
Figure BDA0003712695050000052
其中，det()表示行列式的值，
Figure BDA0003712695050000053
表示xn-N的协方差，
Figure BDA0003712695050000054
为xn-N的均值；
Figure BDA0003712695050000055
的计算公式为：
Figure BDA0003712695050000056
Figure BDA0003712695050000057
的计算公式为：
Figure BDA0003712695050000058
S3-3：将
Figure BDA0003712695050000059
转换成
Figure BDA00037126950500000510
使用联合概率密度函数求取期望计算得到
Figure BDA00037126950500000511
得到最终的性能准则为：
Figure BDA00037126950500000512
其中，K2表示计算得到的常数，
Figure BDA00037126950500000513
表示存在xn-N和Wn-1使
Figure BDA00037126950500000514
最小，
Figure BDA00037126950500000515
作为优选的，所述将所述最终的性能准则进行代价函数变形，具体为：
根据所述
Figure BDA00037126950500000516
和
Figure BDA00037126950500000517
得到：
Figure BDA00037126950500000518
Figure BDA0003712695050000061
其中，
Figure BDA0003712695050000062
对
Figure BDA0003712695050000063
进行变形，得到代价函数的形式：
Figure BDA0003712695050000064
其中，
Figure BDA0003712695050000065
Figure BDA0003712695050000066
作为优选的，所述根据α的不同取值将所述最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值，具体为：
判断α的正负，当α大于0时，所述最终的性能准则为风险寻求准则，根据代价函数的对应理论求出此时的状态估计值；当α小于0时，所述最终的性能准则为风险规避准则，根据代价函数的对应理论求出此时的状态估计值。
作为优选的，所述当α大于0时，所述最终的性能准则为风险寻求准则，根据代价函数的对应理论求出此时的状态估计值，具体为：
当α大于0时，所述最终的性能准则为风险寻求准则，此时
Figure BDA0003712695050000067
需满足：
Figure BDA0003712695050000071
结合所述
Figure BDA0003712695050000072
化简得到
Figure BDA0003712695050000073
和
Figure BDA0003712695050000074
需满足：
Figure BDA0003712695050000075
其中，
Figure BDA0003712695050000076
表示存在xn-N、Wn-1和
Figure BDA0003712695050000077
使
Figure BDA0003712695050000078
最小；
根据引理：有代价函数
Figure BDA0003712695050000079
其中S11＞0，a是向量变量，b是给定的常向量，则J(a,b)最小化时a需满足：
Figure BDA00037126950500000710
在所述
Figure BDA00037126950500000711
中，
Figure BDA00037126950500000712
Figure BDA00037126950500000713
根据式
Figure BDA00037126950500000714
计算得到所述
Figure BDA00037126950500000715
中的a，结合包含
Figure BDA00037126950500000716
的
Figure BDA00037126950500000717
反推出满足
Figure BDA00037126950500000718
时的所述此时的状态估计值
Figure BDA00037126950500000719
作为优选的，所述当α小于0时，所述最终的性能准则为风险规避准则，根据代价函数的对应理论求出此时的状态估计值，具体为：
当α小于0时，所述最终的性能准则为风险规避准则，此时
Figure BDA00037126950500000720
需满足：
Figure BDA0003712695050000081
结合所述
Figure BDA0003712695050000082
化简得到
Figure BDA0003712695050000083
和
Figure BDA0003712695050000084
需满足：
Figure BDA0003712695050000085
其中，
Figure BDA0003712695050000086
表示存在xn-N、Wn-1使
Figure BDA0003712695050000087
最小，在所述存在xn-N、Wn-1使
Figure BDA0003712695050000088
最小的基础上存在
Figure BDA0003712695050000089
使
Figure BDA00037126950500000810
最大；
根据引理：有代价函数
Figure BDA00037126950500000811
其中，a和b是向量变量，c是给定的常向量，当满足V11＞0且
Figure BDA00037126950500000812
时，可得到使J(a,b,c)最小化时的a，并在此基础上得到使J(a,b,c)最大化时的b，此时a、b需满足：
Figure BDA00037126950500000813
在所述
Figure BDA00037126950500000814
中，
Figure BDA00037126950500000815
Figure BDA00037126950500000816
V22＝αI，
Figure BDA00037126950500000817
V23＝[0αMB]，
Figure BDA00037126950500000818
根据V11＞0且
Figure BDA00037126950500000819
计算得到b，结合
Figure BDA00037126950500000820
得到满足
Figure BDA00037126950500000821
时的所述此时的状态估计值
Figure BDA00037126950500000822
本发明的上述技术方案相比现有技术具有以下优点：
本发明在机器人运动的状态空间模型下，通过风险敏感FIR滤波器和代价函数理论得到机器人位置的状态估计值，可以在具有不确定性时实现对扫地机器人位置的有效、精确定位。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中
图1是本发明的流程图；
图2是本发明实施例中扫地机器人在x轴和y轴方向上的位移曲线图；
图3是本发明实施例中扫地机器人在x轴和y轴方向上的位移误差曲线图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
FIR(Finite Impulse Response)滤波器是有限长单位冲激响应滤波器，又称为非递归型滤波器，是数字信号处理系统中最基本的元件，可以在保证任意幅频特性的同时具有严格的线性相频特性。FIR滤波器因其稳定的性能在通信、图像处理、模式识别等领域都有着广泛的应用。因此，本发明中通过引入FIR滤波器来实现扫地机器人的定位功能。
如图1流程图所示，本发明公开了一种基于风险敏感FIR滤波的机器人定位方法，包括以下步骤：
S1：建立机器人运动的状态空间模型。
机器人运动的状态空间模型为：
xn+1＝Axn+Bun+Gwn (1)，
yn＝Cxn+vn (2)；
其中，下标n表示时间，xn+1表示n+1时刻的系统状态，xn表示n时刻的系统状态，xn的维数为r；un为n时刻的系统输入，un的维数为l；wn是过程噪声，wn的维数为p；yn表示n时刻的系统输入，yn的维数为q。A为状态转移矩阵，B为输入矩阵，G为噪声增益矩阵，C为测量矩阵；vn是测量噪声，vn的维数是q，wn和vn均是零均值且不相关的高斯白噪声，wn的噪声协方差矩阵为Q，vn的噪声协方差矩阵为R；xn和yn在时间区间[n-N,n]内以批处理形式表达，N是窗口长度，N的取值根据实际情况而定。
S2：在所述机器人运动的状态空间模型下建立测量值Yn-1，在所述机器人运动的状态空间模型下建立当前时刻的系统状态xn。
S2-1：建立测量值Yn-1为：
Figure BDA0003712695050000101
其中，Yn-1、
Figure BDA0003712695050000102
Un-1、
Figure BDA0003712695050000103
Wn-1和Vn-1分别定义为：
Figure BDA0003712695050000104
Figure BDA0003712695050000105
Figure BDA0003712695050000111
Figure BDA0003712695050000112
Figure BDA0003712695050000113
Figure BDA0003712695050000114
Figure BDA0003712695050000115
Figure BDA0003712695050000116
Un-1、
Figure BDA0003712695050000117
Wn-1和Vn-1是为了缩写的过程参数。
测量值Yn-1中的
Figure BDA0003712695050000118
是噪声项，
Figure BDA0003712695050000119
的协方差ΠN为：
Figure BDA00037126950500001110
其中，QN、RN定义为：
Figure BDA00037126950500001111
Figure BDA00037126950500001112
表示形成以N个Q为对角线，其余元素为0的矩阵；
Figure BDA00037126950500001113
Figure BDA0003712695050000121
表示形成以N个R为对角线，其余元素为0的矩阵。QN、RN是为了缩写的过程参数。
S2-2：建立当前时刻的系统状态xn为：
xn＝ANxn-N+MBUn-1+MGWn-1 (14)；
其中，MB、MG的定义为：
Figure BDA0003712695050000122
Figure BDA0003712695050000123
MB、MG是为了缩写的过程参数。
S3：根据所述测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用所述联合概率密度函数优化所述初始化性能准则得到最终的性能准则。
S3-1：建立所述含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则：
Figure BDA0003712695050000124
其中，
Figure BDA0003712695050000125
表示存在
Figure BDA0003712695050000126
使
Figure BDA0003712695050000127
最小；
由于指数函数和期望都不影响式(17)的单调性，因此
Figure BDA0003712695050000128
实际上最终的大小由
Figure BDA0003712695050000131
决定；即式(17)可以简化成只与
Figure BDA0003712695050000132
相关，简化式(17)得到：
Figure BDA0003712695050000133
其中，
Figure BDA0003712695050000134
表示存在
Figure BDA0003712695050000135
使
Figure BDA0003712695050000136
最小，
Figure BDA0003712695050000137
表示系统状态的估计值；α为预设常数，α的值决定性能准则是风险寻求准则还是风险规避准，本实施例中α预设值为-1；E()是数学期望，en表示误差。
S3-2：由于Un-1、Yn-1是已知变量，xn-N、Wn-1、Vn-1是随机变量。式(17)的期望可由联合高斯随机变量xn-N、Wn-1、Vn-1代替。由于xn-N、Wn-1、Vn-1都是独立高斯随机变量，建立所述联合概率密度函数p(xn-N,Wn-1,Vn-1)为：
Figure BDA0003712695050000138
其中，D、Jn的定义为：
Figure BDA0003712695050000139
其中，det()表示行列式的值，
Figure BDA00037126950500001310
表示xn-N的协方差；
Figure BDA00037126950500001311
其中，
Figure BDA00037126950500001312
为xn-N的均值；D、Jn是为了缩写的过程参数。
Figure BDA00037126950500001313
的计算公式为：
Figure BDA00037126950500001314
Figure BDA0003712695050000141
的计算公式为：
Figure BDA0003712695050000142
S3-3：因为指数函数和期望都不影响式(17)的单调性，实际上最终的大小由
Figure BDA0003712695050000143
决定。因此，将
Figure BDA0003712695050000144
表示成
Figure BDA0003712695050000145
使用联合概率密度函数求取期望计算得到
Figure BDA0003712695050000146
Figure BDA0003712695050000147
得到最终的性能准则为：
Figure BDA0003712695050000148
其中，K1表示计算得到的常数，K2表示计算得到的常数，
Figure BDA0003712695050000149
表示存在xn-N和Wn-1使
Figure BDA00037126950500001410
最小，
Figure BDA00037126950500001411
第二个等式
Figure BDA00037126950500001412
的由来基于Jn对所有积分变量都是二次的，而一个指数二次函数从负无穷到正无穷积分可以由公式
Figure BDA00037126950500001413
计算出来，由此计算得到了该等式。
S4：将所述最终的性能准则进行代价函数变形。
S4-1：根据式(3)
Figure BDA00037126950500001414
得到
Figure BDA00037126950500001415
将
Figure BDA00037126950500001416
和式(14)xn＝ANxn-N+MBUn-1+MGWn-1代入
Figure BDA00037126950500001417
中，得到：
Figure BDA00037126950500001418
Figure BDA0003712695050000151
其中，
Figure BDA0003712695050000152
S4-2：将式(25)中的
Figure BDA0003712695050000153
进行变形，得到代价函数的形式：
Figure BDA0003712695050000154
其中，
Figure BDA0003712695050000155
Figure BDA0003712695050000156
S5：根据α的不同取值将所述最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值。
本实施例中判断α的正负，当α大于0时，执行S5-1；当α小于0时，执行S5-2。
S5-1：所述最终的性能准则为风险寻求准则，根据代价函数的对应理论求出此时的状态估计值。
S5-1-1：所述最终的性能准则为风险寻求准则，此时
Figure BDA0003712695050000157
需满足：
Figure BDA0003712695050000161
将式
Figure BDA0003712695050000162
代入式(29)得到
Figure BDA0003712695050000163
化简得到
Figure BDA0003712695050000164
和
Figure BDA0003712695050000165
需满足：
Figure BDA0003712695050000166
其中，
Figure BDA0003712695050000167
表示存在xn-N、Wn-1和
Figure BDA0003712695050000168
使
Figure BDA0003712695050000169
最小；
S5-1-2：根据引理：有代价函数
Figure BDA00037126950500001610
其中，S11＞0，a是向量变量，b是给定的常向量，则J(a,b)最小化时a需满足：
Figure BDA00037126950500001611
S5-1-3：在所述
Figure BDA00037126950500001612
中，
Figure BDA00037126950500001613
Figure BDA00037126950500001614
Figure BDA00037126950500001615
根据式(32)
Figure BDA0003712695050000171
计算得到所述
Figure BDA0003712695050000172
中的a，结合包含
Figure BDA0003712695050000173
的
Figure BDA0003712695050000174
反推出满足式(30)
Figure BDA0003712695050000175
的所述此时的状态估计值
Figure BDA0003712695050000176
S5-2：所述最终的性能准则为风险规避准则，根据代价函数的对应理论求出此时的状态估计值。
S5-2-1：所述最终的性能准则为风险规避准则，此时
Figure BDA0003712695050000177
需满足：
Figure BDA0003712695050000178
将式
Figure BDA0003712695050000179
代入式(33)得到
Figure BDA00037126950500001710
化简得到
Figure BDA00037126950500001711
和
Figure BDA00037126950500001712
需满足：
Figure BDA00037126950500001713
其中，
Figure BDA00037126950500001714
表示存在xn-N、Wn-1使
Figure BDA00037126950500001715
最小，在所述存在xn-N、Wn-1使
Figure BDA00037126950500001716
最小的基础上存在
Figure BDA00037126950500001717
使
Figure BDA00037126950500001718
最大；
S5-2-2：根据引理：有代价函数
Figure BDA00037126950500001719
其中，a和b是向量变量，c是给定的常向量，当满足V11＞0且
Figure BDA00037126950500001720
时，可得到使J(a,b,c)最小化时的a，并在此基础上得到使J(a,b,c)最大化时的b，此时a、b需满足：
Figure BDA0003712695050000181
S5-2-3：在所述
Figure BDA0003712695050000182
中，
Figure BDA0003712695050000183
Figure BDA0003712695050000184
V22＝αI，
Figure BDA0003712695050000185
V23＝[0αMB]；
根据V11＞0且
Figure BDA0003712695050000186
计算得到b，结合
Figure BDA0003712695050000187
得到满足式(34)
Figure BDA0003712695050000188
的所述此时的状态估计值
Figure BDA0003712695050000189
本发明在机器人运动的状态空间模型下，通过风险敏感FIR滤波器和代价函数理论得到机器人位置的状态估计值，可以在具有不确定性时实现对扫地机器人位置的有效、精确定位。
为了进一步说明本发明的有益效果，本实施例中对扫地机器人的工作状态进行模拟。扫地机器人在工作时，由于仅在平面运动，不涉及高度参数，因此本实施例中将机器人的运动分解成平面上的x轴方向运动与y轴方向运动的叠加。机器人在x轴方向的位移等于上一时刻的位移加上时间间隔与x轴方向速度的乘积，同理，机器人在y轴方向的位移等于上一时刻的位移加上时间间隔与y轴方向速度的乘积，而x轴方向速度等于上一时刻的速度加上时间间隔与x轴方向加速度的乘积，y轴方向速度等于上一时刻的速度加上时间间隔与y轴方向加速度的乘积。因此，机器人运动方程可表示为：
xn+1＝xn+vx,nΔt (37)，
yn+1＝yn+vy,nΔt (38)，
vx,n＝vx,n-1+ax,n-1Δt (39)，
vy,n＝vy,n-1+ay,n-1Δt (40)；
其中，xn表示机器人n时刻在x轴方向的位移，yn表示机器人n时刻在y轴方向的位移。vx,n表示机器人n时刻在x轴方向的速度，vy,n表示机器人n时刻在y轴方向的速度。ax,n表示机器人n时刻在x轴方向的加速度，ay,n表示机器人n时刻在y轴方向的加速度，Δt表示时间间隔。当机器人是匀速运动时ax,n和ay,n都为0m/s2，时间间隔Δt＝1，则可将式(37)，(38)，(39)，(40)转换为机器人状态空间方程为：
Figure BDA0003712695050000191
Figure BDA0003712695050000192
机器人在运动过程中受到地面起伏影响，存在噪声扰动，系统噪声Wn均值为0，协方差Q＝0.1I4，I4是四阶单位矩阵。测量噪声Vn的均值为0，协方差R＝0.01I2，I2是二阶单位矩阵。状态量Xn由四个变量组成，即Xn＝[xn yn vx,n vy,n]T，xn表示为x轴方向上的位移，yn表示y轴方向的位移。vx,n表示x轴方向的速度，vy,n表示y轴方向的速度。位移数据和速度可通过光电编码器测量得到。由于光电编码器和驱动轮同步旋转，利用码盘、减速器、电机和驱动轮之间的物理参数，可将检测到的脉冲数转换成驱动轮旋转的角度，则可得到机器人的瞬时位置和速度。因为光电编码器是本体感受式的传感器，在机器人参考框架中，它的位置估计是最佳的。本实施例中，机器人在x轴和y轴方向的加速度为0m/s2，实际运行过程中的加速度数据可以通过三轴加速度计获得，三轴加速度计可以获得机器人在x轴、y轴和z轴方向上加速度的大小和方向。Zn是测量值，Zn＝[xn yn]T。
根据实测值和通过本发明方法得出的估计值绘制如图2所述的扫地机器人在X轴和Y轴方向上的位移曲线图和如图3所示的扫地机器人在X轴和Y轴方向上的位移误差曲线图，图2和图3中True表示实测值，RSFF表示使用本发明方法得出的估计值。从图2可以看出，估计值与实测值拟合度很好；从图3可以看出，x轴和y轴方向上的误差基本控制在了1米内，本发明方法的误差较小，模拟实验的结果均表明了本发明可以在具有不确定性时实现对扫地机器人位置的有效、精确定位。
术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。

Claims (10)

1.一种基于风险敏感FIR滤波的机器人定位方法，其特征在于，包括以下步骤：
S1：建立机器人运动的状态空间模型；
S2：在所述机器人运动的状态空间模型下建立测量值和当前时刻的系统状态；
S3：根据所述测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用所述联合概率密度函数优化所述初始化性能准则得到最终的性能准则；
S4：将所述最终的性能准则进行代价函数变形；
S5：根据α的不同取值将所述最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值。
2.根据权利要求1所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述机器人运动的状态空间模型为：
xn+1＝Axn+Bun+Gwn，
yn＝Cxn+vn；
其中，xn表示n时刻的系统状态，xn的维数为r；un为n时刻的系统输入，un的维数为l；wn是过程噪声，wn的维数为p；yn表示n时刻的系统输入，yn的维数为q；A为状态转移矩阵，B为输入矩阵，G为噪声增益矩阵，C为测量矩阵；vn是测量噪声，vn的维数是q，wn和vn均是零均值且不相关的高斯白噪声，wn的噪声协方差矩阵为Q，vn的噪声协方差矩阵为R；xn和yn在时间区间[n-N,n]内以批处理形式表达，N是窗口长度。
3.根据权利要求2所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述测量值Yn-1为：
Figure FDA0003712695040000021
其中，Yn-1、
Figure FDA0003712695040000022
Un-1、
Figure FDA0003712695040000023
Wn-1和Vn-1分别定义为：
Figure FDA0003712695040000024
Figure FDA0003712695040000025
Figure FDA0003712695040000026
Figure FDA0003712695040000027
Figure FDA0003712695040000028
Figure FDA0003712695040000029
Figure FDA00037126950400000210
4.根据权利要求3所述的基于风险敏感FIR滤波的机器人定位方法，其特
征在于：所述
Figure FDA0003712695040000031
是噪声项，
Figure FDA0003712695040000032
的协方差ΠN为：
Figure FDA0003712695040000033
其中，QN、RN定义为：
Figure FDA0003712695040000034
Figure FDA0003712695040000035
表示形成以N个Q为对角线，其余元素为0的矩阵；
Figure FDA0003712695040000036
Figure FDA0003712695040000037
表示形成以N个R为对角线，其余元素为0的矩阵。
5.根据权利要求4所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述当前时刻的系统状态xn为：
xn＝ANxn-N+MBUn-1+MGWn-1；
其中，MB、MG的定义为：
Figure FDA0003712695040000038
Figure FDA0003712695040000039
6.根据权利要求5所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述根据所述测量值和当前时刻的系统状态建立含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则和联合概率密度函数，使用所述联合概率密度函数优化所述初始化性能准则得到最终的性能准则，具体为：
S3-1：建立所述含有判断性能准则类型α的风险敏感FIR滤波器的初始化性能准则
Figure FDA0003712695040000041
其中，
Figure FDA0003712695040000042
表示系统状态的估计值，
Figure FDA0003712695040000043
表示存在
Figure FDA0003712695040000044
使
Figure FDA0003712695040000045
最小，E()是数学期望，en是误差；
S3-2：建立所述联合概率密度函数p(xn-N,Wn-1,Vn-1)为：
Figure FDA0003712695040000046
其中，D、Jn的定义为：
Figure FDA0003712695040000047
Figure FDA0003712695040000048
其中，det()表示行列式的值，
Figure FDA0003712695040000049
表示xn-N的协方差，
Figure FDA00037126950400000410
为xn-N的均值；
Figure FDA00037126950400000411
的计算公式为：
Figure FDA00037126950400000412
Figure FDA00037126950400000413
的计算公式为：
Figure FDA00037126950400000414
S3-3：将
Figure FDA00037126950400000415
转换成
Figure FDA00037126950400000416
使用联合概率密度函数求取期望计算得到
Figure FDA0003712695040000051
得到最终的性能准则为：
Figure FDA0003712695040000052
其中，K2表示计算得到的常数，
Figure FDA0003712695040000053
表示存在xn-N和Wn-1使
Figure FDA0003712695040000054
最小，
Figure FDA0003712695040000055
7.根据权利要求6所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述将所述最终的性能准则进行代价函数变形，具体为：
根据所述
Figure FDA0003712695040000056
xn＝ANxn-N+MBUn-1+MGWn-1和
Figure FDA0003712695040000057
得到：
Figure FDA0003712695040000058
其中，
Figure FDA0003712695040000059
对
Figure FDA00037126950400000510
进行变形，得到代价函数的形式：
Figure FDA00037126950400000511
其中，
Figure FDA0003712695040000061
Figure FDA0003712695040000062
8.根据权利要求7所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述根据α的不同取值将所述最终的性能准则分为不同类别，在不同类别的所述最终的性能准则下根据代价函数理论求出此时的状态估计值，具体为：
判断α的正负，当α大于0时，所述最终的性能准则为风险寻求准则，根据代价函数的对应理论求出此时的状态估计值；当α小于0时，所述最终的性能准则为风险规避准则，根据代价函数的对应理论求出此时的状态估计值。
9.根据权利要求8所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述当α大于0时，所述最终的性能准则为风险寻求准则，根据代价函数的对应理论求出此时的状态估计值，具体为：
当α大于0时，所述最终的性能准则为风险寻求准则，此时
Figure FDA0003712695040000063
需满足：
Figure FDA0003712695040000064
结合所述
Figure FDA0003712695040000065
化简得到
Figure FDA0003712695040000066
和
Figure FDA0003712695040000067
需满足：
Figure FDA0003712695040000071
其中，
Figure FDA0003712695040000072
表示存在xn-N、Wn-1和
Figure FDA0003712695040000073
使
Figure FDA0003712695040000074
最小；
根据引理：有代价函数
Figure FDA0003712695040000075
其中S11＞0，a是向量变量，b是给定的常向量，则J(a,b)最小化时a需满足：
Figure FDA0003712695040000076
在所述
Figure FDA0003712695040000077
中，
Figure FDA0003712695040000078
Figure FDA0003712695040000079
根据式
Figure FDA00037126950400000710
计算得到所述
Figure FDA00037126950400000711
中的a，结合包含
Figure FDA00037126950400000712
的
Figure FDA00037126950400000713
反推出满足
Figure FDA00037126950400000714
时的所述此时的状态估计值
Figure FDA00037126950400000715
10.根据权利要求8所述的基于风险敏感FIR滤波的机器人定位方法，其特征在于：所述当α小于0时，所述最终的性能准则为风险规避准则，根据代价函数的对应理论求出此时的状态估计值，具体为：
当α小于0时，所述最终的性能准则为风险规避准则，此时
Figure FDA00037126950400000716
需满足：
Figure FDA00037126950400000717
结合所述
Figure FDA0003712695040000081
化简得到
Figure FDA0003712695040000082
和
Figure FDA0003712695040000083
需满足：
Figure FDA0003712695040000084
其中，
Figure FDA0003712695040000085
表示存在xn-N、Wn-1使
Figure FDA0003712695040000086
最小，在所述存在xn-N、Wn-1使
Figure FDA0003712695040000087
最小的基础上存在
Figure FDA0003712695040000088
使
Figure FDA0003712695040000089
最大；
根据引理：有代价函数
Figure FDA00037126950400000810
其中，a和b是向量变量，c是给定的常向量，当满足V11＞0且
Figure FDA00037126950400000811
时，可得到使J(a,b,c)最小化时的a，并在此基础上得到使J(a,b,c)最大化时的b，此时a、b需满足：
Figure FDA00037126950400000812
在所述
Figure FDA00037126950400000813
中，
Figure FDA00037126950400000814
Figure FDA00037126950400000815
Figure FDA00037126950400000816
V22＝αI，
Figure FDA00037126950400000817
V23＝[0 αMB]，
Figure FDA00037126950400000818
根据V11＞0且
Figure FDA00037126950400000819
计算得到b，结合
Figure FDA00037126950400000820
得到满足
Figure FDA00037126950400000821
时的所述此时的状态估计值
Figure FDA00037126950400000822
CN202210724152.0A 2022-06-24 2022-06-24 基于风险敏感fir滤波的机器人定位方法 Active CN115166635B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210724152.0A CN115166635B (zh) 2022-06-24 2022-06-24 基于风险敏感fir滤波的机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210724152.0A CN115166635B (zh) 2022-06-24 2022-06-24 基于风险敏感fir滤波的机器人定位方法

Publications (2)

Publication Number Publication Date
CN115166635A true CN115166635A (zh) 2022-10-11
CN115166635B CN115166635B (zh) 2023-03-28

Family

ID=83486911

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210724152.0A Active CN115166635B (zh) 2022-06-24 2022-06-24 基于风险敏感fir滤波的机器人定位方法

Country Status (1)

Country Link
CN (1) CN115166635B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
WO2021082571A1 (zh) * 2019-10-29 2021-05-06 苏宁云计算有限公司 机器人跟踪方法、装置、设备及计算机可读存储介质
CN113326616A (zh) * 2021-05-31 2021-08-31 上海航天测控通信研究所 抗慢变量测粗差的容错卡尔曼滤波方法
CN113792411A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法
CN114034303A (zh) * 2021-11-11 2022-02-11 华南农业大学 一种基于卡尔曼滤波的移动目标物定位方法、装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109916407A (zh) * 2019-02-03 2019-06-21 河南科技大学 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法
WO2021082571A1 (zh) * 2019-10-29 2021-05-06 苏宁云计算有限公司 机器人跟踪方法、装置、设备及计算机可读存储介质
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
CN113326616A (zh) * 2021-05-31 2021-08-31 上海航天测控通信研究所 抗慢变量测粗差的容错卡尔曼滤波方法
CN113792411A (zh) * 2021-08-13 2021-12-14 中国人民解放军军事科学院国防科技创新研究院 一种基于中心误差熵准则无迹卡尔曼滤波的航天器姿态确定方法
CN114034303A (zh) * 2021-11-11 2022-02-11 华南农业大学 一种基于卡尔曼滤波的移动目标物定位方法、装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘飞等: "线性离散状态时滞系统UFIR滤波算法" *

Also Published As

Publication number Publication date
CN115166635B (zh) 2023-03-28

Similar Documents

Publication Publication Date Title
KR101724872B1 (ko) 빠르고 강건한 궤적 설계를 통한 산업용 로봇의 파라미터 추정 방법
CN109940622B (zh) 一种基于电机电流的机器人机械臂无传感碰撞检测方法
CN108897226B (zh) 基于干扰观测器的mems陀螺仪预设性能非奇异滑模控制方法
CN106828005B (zh) 一种预规定瞬态性能的汽车主动悬架自适应控制方法
CN108710296B (zh) 微陀螺仪的分数阶自适应快速终端滑模控制方法
CN108972553B (zh) 一种基于粒子滤波算法的空间机械臂故障检测方法
CN110941183B (zh) 一种基于神经网络的工业机器人动力学辨识方法
CN108469730B (zh) 一种基于均值耦合的多电机固定时间自适应滑模控制方法
CN108931233B (zh) 一种道路侧向坡度值检测方法及装置
CN112873207A (zh) 一种基于未知系统动态估计器的柔性关节机械臂预设性能控制方法
CN104503246A (zh) 微陀螺仪系统的间接自适应神经网络滑模控制方法
CN109067381A (zh) 一种mems陀螺仪随机噪声的实时滤波系统及方法
CN110703610A (zh) 微陀螺仪的递归模糊神经网络非奇异终端滑模控制方法
CN110702093B (zh) 基于粒子滤波的定位方法、装置、存储介质及机器人
CN109062048B (zh) 基于复合学习的mems陀螺仪预设性能非奇异滑模控制方法
CN115166635B (zh) 基于风险敏感fir滤波的机器人定位方法
CN107340026A (zh) 非稳态液面高度测量值滤波方法
CN105652795B (zh) 一种基于残差观测器的3ptt-2r串并联数控机床伺服系统故障预测装置及方法
CN116872197A (zh) 单杆机械臂的自适应神经网络反演控制方法及其控制系统
CN114407022B (zh) 一种基于模型参数误差观测器的机械臂碰撞检测方法
CN110471293A (zh) 一种估计时变角速度的z轴陀螺仪滑模控制方法
CN111856941B (zh) 一种基于主动抗扰的自适应终端动态滑模控制方法
CN109084751B (zh) 一种基于盒粒子滤波的高能效卫星姿态确定算法
CN110579966B (zh) 一种基于神经网络辨识参数的z轴陀螺仪控制方法
Yuan et al. Robot position realization based on multi-sensor information fusion algorithm

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant