[发明专利]基于模糊-自适应的SLAM数据关联方法

权利要求书

1.基于模糊-自适应的SLAM数据关联方法，其特征在于：将归一化新息以及观测误差椭圆和估计误差椭圆的重叠比例进行模糊化后作为输入变量，将数据关联结果作为模糊推理的执行结果；通过建立适当的模糊推理规则，进行模糊推理，最终将输入映射到输出，实现数据关联；建立三输入单输出的模糊系统，由模糊逻辑思想，可以看到围绕其中的两个关键要素：1)误差椭圆；2)模糊化；

误差椭圆即描述待定点位置各方向上误差分布规律的椭圆，它表示估计值以一定概率落入以真实值为圆心的椭圆区域；误差椭圆由半长轴、半短轴和长轴方向3个参数确定，由误差协方差矩阵可以求出相应的误差椭圆参数；

以某一目标起始位置为坐标原点建立坐标轴，设其真实位置矢量为X＝[x_e,y_e]^T，其估计矢量为其中x_e,y_e表示目标的真实位姿坐标，表示目标的估计位姿坐标，则估计的误差协方差矩阵为：

C=σx2ρσxσxρσxσxσy2---(1)]]>

其中σ_x，σ_y分别为矢量坐标的方差，ρ是协方差相关系数，由于C是正定矩阵，所以|ρ|≤1；估计矢量以概率P落入以真实位置矢量X为中心的椭圆区域，其椭圆区域表达式和概率积分公式如下，其中D为椭圆区域；

(X^-X)TC-1(X^-X)≤D2---(2)]]>

P=∫∫(X^-X)TC-1(X^-X)≤D212π|C|exp{-12(X^-X)TC-1(X^-X)}dxdy---(3)]]>

为推导椭圆区域D与概率P的关系,进行变量替换,令对式(3)的积分区域进行替换，则该式可以写成：

P=∫∫ζ2+η2≤D212πexp{-12(ζ2+η2)}dζdη---(4)]]>

再将ζ和η转换到极坐标下，令ζ＝r sinθ，η＝r cosθ可得：

p=12π∫02π{∫0Dexp(-r22)rdr}dθ=1-exp(-D2/2)---(5)]]>

式中θ为长轴方向角，可得椭圆区域D与概率P具有如下关系

D=-2ln(1-P)---(6)]]>

通常使用半长轴a、半短轴b和长轴方向θ三个参数确定椭圆，这三个参数通过协方差C和概率P计算；利用式(1)可将椭圆表示为以下形式：

11-ρ2[(x-x^)2σx2+(y-y^)2σy2-2ρ(x-x^)(y-y^)σx2σy2]=D2---(7)]]>

若ρ＝0，上式变为:

(x-x^)2σx2+(y-y^)2σy2=D2---(8)]]>

利用σ_x和σ_y可直接得到：

当σ_x≥σ_y时，a＝Dσ_x，b＝Dσ_y，θ＝0；

当σ_x＜σ_y时，a＝Dσ_y，b＝Dσ_x，

若ρ≠0，进行坐标变换：

u=(x-x^)cosθ+(y-y^)sinθ---(9)]]>

v=-(x-x^)sinθ+(y-y^)cosθ---(10)]]>

椭圆在新坐标系的表达式为：

u2σu2+v2σν2=D2---(11)]]>

此时下式成立：

E|dudv|＝ρσ_xσ_y(cos²θ^-sin²θ)+(σ_x²+σ_y²)cosθsinθ＝0    (12)

并且

σu2=σx2cos2θ+σy2sin2θ+2ρσxσysinθcosθ---(13)]]>

σv2=σx2sin2θ+σy2cos2θ-2ρσxσysinθcosθ---(14)]]>

最终获得误差椭圆的参数：

半长轴：a＝max(Dσ_u,Dσ_v)               (15)

半短轴：b＝min(Dσ_u,Dσ_v)             (16)

长轴方向角：θ=12tg-1(2σxσyσx2-σy2)σx>σy12tg-1(2σxσyσx2-σy2)+π2σx<σy---(17)]]>

当σ_x＝σ_y时，若ρ＞0，则若ρ＜0，则

表1列出了几种误差椭圆以及所对应估计值落入其椭圆区域的概率；

表1误差椭圆及其概率

若选择较小的概率值，则椭圆区域面积较小，有较多的估计点被排除在区域之外；若选择较大的概率值，则椭圆区域面积较大，落入误差椭圆的估计点也比较多；为了获得较高的数据关联正确率，同时考虑到算法计算量问题，要选择适当的误差椭圆进行计算；对于特征密度较大的环境，选择较小的误差椭圆可以降低计算量，同时避免一些错误数据关联；对于特征分布稀疏的环境，选择较大的误差椭圆能保证关联正确率，同时减少丢弃正确关联假设的可能；另外环境中噪声干扰和传感器观测的不确定性也会对误差椭圆的选取产生一定的影响；

对于每一时刻传感器获得的观测信息中的某一观测值和环境地图中某一待匹配特征状态估计值，利用上述方法分别计算它们对应的误差椭圆；为了保证误差椭圆具有实际意义，要避免长轴出现“无穷大”的情况，因此设定一个阈值，当计算得到的椭圆半长轴值超过阈值时，认为该观测值为无效数据，无需对其进行关联匹配；当半长轴在阈值范围内，才继续对其进行关联处理；

①归一化新息及其模糊化

对于每一个特征的一对椭圆间有一个最大的重叠区域，这个区域在椭圆中所占比例可以反映观测——估计对之间的相关性；两个误差椭圆相离(可将两椭圆相切视为这类相对位置的特殊情况)，分别为两椭圆中心连线与椭圆的交点，此时residual≥1；两个误差椭圆相交，且两椭圆中心不在重叠区域中，A仍为两椭圆中心连线与椭圆的交点，此时1/2≤residual≤1；两个椭圆相交，且圆心落入重叠区域内为两椭圆圆中心连线的延长线与椭圆的交点，此时0≤residual≤1/2；两椭圆的圆心重合，将此时的归一化新息定义为0；

定义归一化新息如下，其中O₁，O₂为两椭圆的中心；

residual=|O1O2||O1A1|+|O2A2|---(18)]]>

将上述清晰的归一化新息按照语言模糊集进行模糊化，用于描述归一化新息的模糊语言变量为Very close、Close、Medium、Far和Very far，模糊化后的变量也将被映射到五个模糊集合中；

②两误差椭圆重合比例及其模糊化

对于几种误差椭圆位置关系中，特征状态估计值和观测值对应的误差椭圆的重叠区域能反映出它们之间的关联关系；直观上来看，重叠区域的面积越大，估计值和观测值的关联程度越大，但估计值和观测值的不确定性受到环境噪声、观测噪声、系统噪声等多种因素的影响，因此其误差椭圆的大小也是不断变化的；为了对误差椭圆重叠区域能反映出的关联程度进行定量考察，将重叠区域面积与两椭圆面积之比作为输入变量；

对于两椭圆相交形成的重叠区域，难以直接计算其面积；本发明使用八边形近似的方法，首先在椭圆内根据半长轴和半短轴与椭圆交点建立对称八边形，就可以将两个椭圆相交的重叠区域近似为两八边形的重叠区域，然后再将重叠区域分解为若干个三角形，三角形的面积是易于计算的，这样就可以得到两椭圆重叠面积的近似值；将两误差椭圆的重叠区域近似为两个八边形重叠区域，这是一个多边形区域，将其再划分为四个三角形，每个三角形的面积可通过海伦公式计算：

Si=p(p-a)(p-b)(p-c)---(19)]]>

式中：p＝(a+b+c)/2，a、b、c分别为三角形的三条边长；最终两椭圆重叠区域面积area近似为三角形面积之和：

area=Σi=1nSi---(20)]]>

再分别计算重叠区域在每个椭圆中所占比例：

per_obs＝area/ellipse_obs              (21)

per_est＝area/ellipse_est               (22)

其中per_obs和per_est分别为特征观测值和特征估计值对应的误差椭圆的重叠区域在各自椭圆中所占比例，ellipse_obs和ellipse_est分别为两误差椭圆的面积；

定义描述椭圆重叠区域比例的模糊语言变量为Seldom、Little、Medium、Most和Major，这些模糊集合对应的隶属函数，将清晰的重叠比例量映射到上述模糊集合中进行模糊化处理；

将待匹配的特征观测值和特征估计值的关联程度作为模糊推理系统的输出值，记作Degree，其取值在[0,1]之间；定义描述关联程度的模糊语言变量为VeryLow、Low、Medium、High和Very High，这些模糊集合对应的隶属函数，将清晰地输出变量映射到模糊集合中进行模糊化；

数据关联算法除了要有正确的关联率之外，还需要防止引入虚假的新路标；其由两种途径产生：a、将已有环境路标的观测错误判断为新路标；b、将虚假的环境路标观测判断为新的路标；其中第一种途径的影响更为严重，在特征环境中，将一个路标识别为两个路标将导致在两者之间分配该路标的观测数据错误；

定义传感器返回M个观测量，记为z＝{z₁,...,z_m}；对于第m个观测量z_m，计算z_m与地图中每一个路标之间的马氏距离D_e，记其中最小的为D_m。为了有效地防止虚假路标，在基于固定关联阈值G₁的基础上，引入增广阈值G₂；若D_m＜G₁，则观测值与环境中的路标可作为关联对；若G₂＜D_m，则其观测值作为一个新路标的观测；若G₁≤D_m≤G₂，则丢弃该观测；用自适应的方法来调节增广阈值G₂；

其方法如下：

1)计算当前传感器观测中距离最近的两个观测：z_m,z_n；

2)定义当前机器人位姿估计为X^v和P^v，将z_m作为虚拟的新路标加入到系统状态中，即

X＝[X^v g(z_m)]                    (23)

P=PvGv·PvPv·GvTGv·Pv·GvT+Gz·R·GzT---(24)]]>

其中，g(z_m)为逆观测模型，G_v和G_z分别为g关于X^v和z_m的雅克比矩阵，R为观测噪声；

3)将z_n作为虚拟路标的观测量，并计算记录z_n与虚拟路标之间的马氏距离D_e；

4)如果观测只有一个观测量，则D_e取为所有已得到的{D_e}中的最小值；

5)G₂＝max(D_e,G_2low)，其中G_2low为设定的下限值；

在最近邻关联算法中，错误的数据关联将导致SLAM过程的发散，因此通常设置较低的关联阈值来保证数据关联的正确性；但是较低的关联阈值将导致较高的观测丢弃率，为了使数据关联算法能够根据系统状态和环境中路标的分布情况来自适应的调节关联阈值，提出与自适应增广阈值类似的关联阈值设定方法；

其方法如下：

1)首先将关联阈值G₁初始化为预先设定的关联阈值下限D_low，然后按下面设置自适应关联阈值G₁；

2)当首次观测到多个路标时，在上节中得到的马氏距离D_e后，取G₁＝D_e/4；

3)在后续观测中，G₁＝min{G₁,D_e/4}，即G₁取为所有{D_e}中最小值的1/4；

4)当G₁≤D_low时，为防止过低的关联成功率，取G₁＝D_low；

通过以上内容的介绍，基于模糊-自适应的SLAM数据关联方法其特征在于将模糊逻辑规则运用到特征观测值与估计值的数据关联中，并能够根据环境和噪声的变化自适应地确定阈值，从而改善数据关联效果，具体方法步骤如下：

Step1：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j；

Step2：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

Step3：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

Step4：将z_n作为虚拟路标的观测量；

Step5：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

Step6：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

Step7：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

Step8：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

Step9：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

Step10：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

Step11：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

Step12：根据前一时刻的自适应的增广阈值G₂及关联阈值G₁来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测；当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测。

2.根据权利要求1所述的基于模糊-自适应的SLAM数据关联方法，其特征在于：系统围绕一个闭环的轨迹运动，在移动机器人移动过程中，用激光传感器持续测量了120个路标；具体步骤如下：

步骤一：获取特征地图信息F_j(j＝1,2,...,n)，传感器测量信息Z_i(i＝1,2,...,m)，通过传感器观测到的特征为g_j；

步骤二：计算当前观测中欧氏距离最近的两个观测：z_m,z_n；

步骤三：将z_m作为虚拟的新路标加入到系统状态中，如式(23)与(24)；

步骤四：将z_n作为虚拟路标的观测量；

步骤五：根据特征观测和估计的误差协方差矩阵，计算特征观测值z_n与虚拟路标间对应的误差椭圆，其误差椭圆的参数如式(15)与(17)；

步骤六：计算误差椭圆中心距离即新息，根据式(18)进行归一化处理，然后将其映射到模糊集合；

步骤七：计算误差椭圆的重叠区域在两椭圆中所占的面积比例，并进行模糊化处理；

步骤八：将模糊化输入变量应用到模糊规则中，输出值是反映关联程度的模糊变量；

步骤九：对模糊化的输出结果进行处理，得到单一关联变量，并由此得出一对观测——估计的马氏距离D_e；

步骤十：取关联阈值G₁＝D_e/4，并在后续观测中根据反馈回来的D_e自适应的改变关联阈值；若G₁≤D_low，则取G₁＝D_low；

步骤十一：增广阈值G₂＝max(D_e,G_2low)，在后续观测中根据反馈回来的D_e自适应的改变增广阈值；

步骤十二：根据前一时刻的自适应的增广阈值G₂及关联阈值G1来确定当前时刻z_m是否作为关联路标进行系统更新，还是作为一个新的路标的观测；当D_e＜G₁时，则观测值z_m与路标F_i为可接受关联对；当D_e＞G₂，则z_m将作为一个新的路标观测。