CN108896050B - 一种基于激光传感器的移动机器人长期定位系统及方法 - Google Patents

Abstract

本发明提供了一种基于激光传感器的移动机器人长期定位系统及方法，系统包括：长期系统模块，通过历史数据和当前观测数据建立地图栅格的动态因子，区分动态障碍物和半动态障碍物在地图更新中的影响；地图更新模块，通过激光传感器的观测信息与地图环境特征建立匹配度，结合地图信息定位能力衡量地图不同位置处观测信息可信度来建立地图更新机制，将其作为动态栅格地图更新的触发条件,并采用动态栅格模型进行状态更新；定位模块，对未更新到地图中的动态障碍物采用动态定位能力衡量动态因素对定位影响，通过修正粒子建议分布函数降低动态障碍物对定位干扰。本发明在动态环境下通过长期信息和观测信息实时更新先验地图，保证长期作业下的定位精度。

Description

一种基于激光传感器的移动机器人长期定位系统及方法

技术领域

本发明涉及机器人定位领域，尤其涉及到一种基于激光传感器的移动机器人长期定位系统及方法。

背景技术

国内人口红利消退，劳动力成本攀升，机器换人趋势加快，越来越多的移动机器人地被应用于工业和服务业，如车间自主搬运叉车的AGV、用于停车场的巡检安保机器人等。机器人应用的场景越来越复杂，而环境的变化对机器人长时间工作的性能提出了更多的挑战，其中长期定位是核心技术难点。现有的主要定位技术包括激光定位、视觉定位等，其使用在长期变化的环境下存在很多的缺陷，具体技术如下：

公开号为107356932A的中国发明专利申请公开了一种利用激光传感器进行定位的方法。这种方法的缺点是依赖于静态的环境，在长时间的作业中，当环境发生改变，其先验地图将失效，从而造成长期定位的失效。

公开号为105404842A的中国发明专利申请公开了一种基于地表二维码的定位方法。这种方法虽然不受环境变化的影响，但是需要对地面环境进行重布置，并且二维码易受损而导致无法长期应用。

公开号为102538779B的中国发明专利申请公开了一种用于移动机器人的地图更新的方法。它的主要缺点是地图更新仅依赖于观测数据，而尚未考虑历史的长期信息对地图状态的更新的影响，长时间会造成更新地图精度的下降，从而影响机器人长期定位的精度。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种能在长期变化的复杂环境下进行定位系统以及方法，本发明不需要对环境进行人工路标或二维码的布置，通过历史数据和观测数据，其先验地图能随着环境的变化而更新，从而实现长期的定位，同时其对动态环境也具有较强的适应性。

本发明是根据以下技术方案实现的：

根据本发明的一个方面，一种基于激光传感器的移动机器人长期定位系统，其特征在于，包括：长期系统模块、地图更新模块、定位模块，三个模块之间通过以下关系进行连接：

所述长期系统模块被配置成通过历史数据和观测数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响，通过动态因子的传递为地图更新模块服务，以区分半动态和动态障碍物；

所述地图更新模块被配置成通过匹配度与定位能力结合的更新置信度，建立地图更新机制，并将其作为触发动态栅格更新的判据；同时，使用动态栅格概率模型，并结合由长期系统模块提供的动态因子进行栅格状态的更新，通过提供实时更新的地图为定位模块服务，以提供准确的定位先验信息；

所述定位模块被配置成对于地图更新模块中提供的实时地图，考虑未更新到地图中的动态障碍物，采用动态定位能力矩阵来衡量动态障碍物的影响，通过修正粒子建议分布函数来降低动态障碍物的干扰，提高复杂环境下的定位精度和鲁棒性，从而实现长期定位的目标。

根据本发明的另一个方面，一种基于激光传感器的移动机器人长期定位方法，根据权利要求1所述的定位系统的三大模块实现的，其特征在于，包括如下步骤：

步骤S1：首先，在长期系统模块中，通过历史数据和观测数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响。

步骤S2：其次，在地图更新模块中，通过将匹配度和定位能力结合的更新置信度反应在当前位姿进行地图更新的可信度，并将其作为地图更新的触发条件，在此基础上，采用动态栅格模型进行栅格状态的更新。

步骤S3：最后，在定位模块中，对于未更新的动态障碍物，采用动态定位能力矩阵来衡量动态障碍物的影响，通过修正粒子建议分布函数来降低动态障碍物的干扰。

上述技术方案中，步骤S1中，动态栅格的动态因子λ是通过栅格的观测重复性来建立的，通过在观测窗口内统计栅格占有属性和空闲属性的比例进行计算，计算公式如下：

其中s是观测窗口内该栅格属于占有属性的次数，f是观测窗口内该栅格属于空闲属性的次数，T是观测窗口内该栅格的状态总次数，abs()表示绝对值的计算。

上述技术方案中，栅格的动态因子λ通过实时的观测数据进行更新，即观测窗口是动态的，随着新观测数据的输入而变化，保证动态属性能实时适应环境的变化，动态因子的更新方程如下：

其中，

是更新权重，该值大小决定了λ值更新的快慢。

上述技术方案中，步骤S2中所述更新置信度计算方式如下：

步骤S201：将匹配度M₁和定位能力M₂加权平均后得到更新置信度M，即

M＝αM₁+βM₂；

步骤S202：设置α和β，其代表匹配度和定位能力的权重因子，只有当M超过一定阈值时观测才会被认定有效，从而进行相应的地图更新，避免地图更新错乱、失效。

上述技术方案中，其步骤S201中涉及到的匹配度M₁计算方法如下：

步骤S20101：得到各激光束的距离值与期望距离之值，并将其表示成矩阵形式r_k，其中r_N表示的是第N束激光的距离值和期望值之差，激光距离值通过传感器直接获得，而期望值通过预先载入的栅格地图获得，即

步骤S20102：根据观测的噪声协方差，得到关于r_k的马氏距离D_k，即

步骤S20103：根据D_k中各对角线元素d_N,计算各激光点的权重w_N，即

步骤S20104：由此得到激光观测与环境的匹配度M₁，即

上述技术方案中，步骤S201中所述定位能力M₂的计算方式如下：

步骤S20111：获得机器人的当前位置p＝(x,y,θ)，激光观测模型中第i_th束激光在地图中的期望距离

观测的协方差

激光观测模型的激光束N₀；对于

是当机器人改变Δx时，其期望距离的改变量；

步骤S20112：根据以上量，得到在线定位能力矩阵

即

步骤S20113：将定位能力矩阵

的行列式值归一化后得到M₂；

上述技术方案中，步骤S2中的状态更新模型采用了动态栅格地图模型，栅格更新方程为：

其中Q_t＝[p(c_t＝occ|z_1:t)p(c_t＝free|z_1:t)]为栅格状态，

为状态转移矩阵，通过在线EM算法求得，

为观测模型，η为归一化参数。

上述技术方案中，步骤S3采用的动态定位能力矩阵修正粒子建议分布函数，计算如下：

步骤S301：计算动态定位能力矩阵

其中s_i表示激光传感器打到未知障碍物的概率，

表示观测方差，其他变量与步骤S20101所述的变量一致；

步骤S302：计算增量的定位方差

其中

表示里程计输入信息的误差方差，其对角线各项分别表示右轮、左轮的线位置误差方差，以及旋转误差方差；

步骤S303：粒子建议分布函数修正，得到

其中，

h是里程计项的权重系数，而

是用激光匹配的粒子滤波得到的机器人位姿；由此可得，当动态定位能力矩阵

越大时，先验信息会更多依赖于观测匹配得到的位姿；而当

越小时，先验信息会更多依赖于里程计的值，降低动态障碍物的影响。

与现有技术相比，本发明具有如下的有益效果：

本发明使得机器人在长期变化的动态环境下，能通过长期信息和观测信息，实时更新先验地图，适应环境的变化，保证了长期作业下的定位精度和鲁棒性。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明基于激光传感器进行长期定位方法示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

如图1所示，本发明的一种基于激光传感器的移动机器人长期定位系统，包括：长期系统模块、地图更新模块定位模块，三大模块通过如下的关系相连接：

其中，所述长期系统模块被配置成通过历史数据和观测数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响，通过动态因子的传递为地图更新模块服务，以区分半动态和动态障碍物；

所述地图更新模块被配置成通过匹配度与定位能力结合的更新置信度，建立地图更新机制，并将其作为触发动态栅格更新的判据；同时，使用动态栅格概率模型，并结合由长期系统模块提供的动态因子进行栅格状态的更新，通过提供实时更新的地图为定位模块服务，以提供准确的定位先验信息；

所述定位模块被配置成对于地图更新模块中提供的实时地图，考虑未更新到地图中的动态障碍物，并用动态定位能力矩阵来衡量动态障碍物的影响，提高复杂环境下的定位精度和鲁棒性，从而实现长期定位的目标。

具体地，根据本发明的一个具体实施例，本发明的工作原理如下：长期系统模块，移动机器人的地图状态更新不仅仅取决于短期传感器的观测，也需要通过长期的状态数据积累和分析，来确定地图栅格由于属性不同造成的状态变化的快慢。具体而言，通过对历史数据的离散化处理，记录观测窗口内栅格的占有和空闲状态，根据其状态之比，建立栅格的动态属性，以此作为栅格状态稳定性的定量依据。当栅格的动态因子较低时，其栅格状态一般比较稳定，当有动态障碍物出现在此栅格，或者因传感器和定位误差带来该栅格状态的突变，通过动态因子的约束，都会限制该栅格发生状态变化；而当栅格的动态因子较高时，其栅格状态往往处于易变状态，同样通过动态因子的约束，该栅格的状态会根据新观测的变化进行实时的栅格状态更新。长期的观测数据一般往往能将动态、半动态障碍物的区分与动态因子的高低相关联，而结合观测的可重复性，一般都能将障碍物信息对地图更新的影响做合理区分。同时，栅格的动态因子不是一成不变的，其会随着作业过程中观测的不断累加而进行相应更新。

地图更新模块，移动机器人在创建地图后，其先验地图不是一成不变的，而是能应对环境的变化，通过激光传感器的观测作为新数据的来源进行更新。具体而言，通过匹配度与定位能力的结合，建立地图更新机制，并将其作为触发动态栅格更新的判据，其中提出的匹配度是基于观测距离与地图中期望距离之差的马氏距离来表示，并将其归一化后转化成百分比的形式，更能反应观测与地图的相似程度；而定位能力则通过激光模型，考虑了在地图不同位姿处观测的可信度，两者结合来反应在当前位姿下定位的可信度，即进行地图更新的置信度，结合动态栅格地图的模型，进行地图栅格状态的更新，从而保障地图能实时更新且能有效降低因定位偏差带来的地图更新错乱的风险。

定位模块，移动机器人在有了能实时反应周围环境的地图后，需要利用该地图进行机器人的自定位，除了更新到地图中的半动态障碍物，还需要考虑未更新的动态障碍物的影响。具体而言，在定位中，通过动态定位能力矩阵来衡量动态障碍物对粒子滤波定位的干扰，当环境中同时存在动态障碍物和半动态障碍物时，半动态障碍物通过其前后观测的一致性和其占有栅格的高动态因子会被及时更新到地图中；相反，动态障碍物因其前后观测的不一致性不会被更新到地图中，这时需要用动态定位能力矩阵来评估动态障碍物对定位的影响。当动态障碍物占据观测信息比例较高时，其对应行列式较高的动态定位能力矩阵，该值会被作为约束考虑到粒子建议分布函数中，从而会降低观测对定位的影响；相反，当动态障碍物占据观测信息比例较低时，其对应行列式较低的动态定位能力矩阵，该值也促进定位的先验信息更多地依赖于里程计的值，从而在复杂的环境下进行长期精确、鲁棒的定位。

本发明的一种基于激光传感器的移动机器人长期定位方法，根据上述的定位系统实现的，包括如下步骤：

步骤S1：首先，在长期系统模块中，通过历史数据和观测数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响。

步骤S2：其次，在地图更新模块中，通过将匹配度和定位能力结合的更新置信度反应在当前位姿进行地图更新的可信度，并将其作为地图更新的触发条件。在此基础上，采用动态栅格模型进行栅格状态的更新。

步骤S3：最后，在定位模块中，对于未更新的动态障碍物，采用动态定位能力矩阵修正粒子建议分布函数。

其中，步骤S1中，动态栅格的动态因子λ是通过栅格的观测重复性来建立的，通过在观测窗口内统计栅格占有属性和空闲属性的比例进行计算，计算公式如下：

其中s是观测窗口内该栅格属于占有属性的次数，f是观测窗口内该栅格属于空闲属性的次数，T是观测窗口内该栅格的状态总次数，abs()表示绝对值的计算。

其中，栅格的动态因子λ通过实时的观测数据进行更新，即观测窗口是动态的，随着新观测数据的输入而变化，保证动态属性能实时适应环境的变化，动态因子的更新方程如下：

是更新权重，该值大小决定了λ值更新的快慢。

其中，步骤S2中的更新置信度的计算方式如下：

步骤S201：将匹配度M₁和定位能力M₂加权平均后得到更新置信度M，即

M＝αM₁+βM₂；

步骤S201：设置α和β，其代表匹配度和定位能力的权重因子，只有当M超过一定阈值时观测才会被认定有效，从而进行相应的地图更新，避免地图更新错乱、失效。

其中，更新置信度中的匹配度M₁计算方法如下：

步骤S20101：得到各激光束的距离值与期望距离之值，并将其表示成矩阵形式r_k，其中r_N表示的是第N束激光的距离值和期望值之差，激光距离值通过传感器直接获得，而期望值通过预先载入的栅格地图获得，即

步骤S20102：根据观测的噪声协方差，得到关于r_k的马氏距离D_k，即

步骤S20103：根据D_k中各对角线元素d_N,计算各激光点的权重w_N，即

步骤S20104：由此得到激光观测与环境的匹配度M₁，即

其中，更新置信度中的定位能力M₂计算方法如下：

步骤S20111：获得机器人的当前位置p＝(x,y,θ)，激光观测模型中第i_th束激光在地图中的期望距离

观测的协方差

激光观测模型的激光束N₀；对于

是当机器人改变Δx时，其期望距离的改变量；

步骤S20112：根据以上量，得到在线定位能力矩阵

即

步骤S20113：将定位能力矩阵

的行列式值归一化后得到M₂；

其中，在步骤S2的状态更新中，采用了动态栅格地图模型，栅格更新方程为：

其中Q_t＝[p(c_t＝occ|z_1:t)p(c_t＝free|z_1:t)]为栅格状态，

为状态转移矩阵，通过在线EM算法求得，

为观测模型，η为归一化参数。

其中，基于动态定位能力的粒子滤波是在实时地图更新的基础值上，对于动态因子λ较高的栅格，反应了影响该栅格变化的往往是半动态障碍物，其易被更新到地图中；而对于动态因子λ较低的栅格，反应了该栅格状态较为稳定，由动态障碍物引起的观测变化将不会被更新到地图中。因此对于已经更新到地图中的半动态障碍物，不用将其考虑到动态定位能力的评估上，而只需考虑未更新的动态障碍物对定位的影响，步骤S3采用动态定位能力矩阵修正粒子建议分布函数，计算如下：

步骤S301：计算动态定位能力矩阵

其中s_i表示激光传感器打到未知障碍物的概率，

表示观测方差，其他变量与步骤S20112所述的变量一致；

步骤S302：计算增量的定位方差

其中

表示里程计输入信息的误差方差，其对角线各项分别表示右轮、左轮的线位置误差方差，以及旋转误差方差；

步骤S303：粒子建议分布函数修正，得到

其中，

h是里程计项的权重系数，而

是用激光匹配的粒子滤波得到的机器人位姿；

由此可得，当动态定位能力矩阵

越大时，先验信息会更多依赖于观测匹配得到的位姿；而当

越小时，先验信息会更多依赖于里程计的值，降低动态障碍物的影响。

此外，本发明的地图的状态更新和定位结果都是实时的、在线的，而非离线的。本发明对场景的适应性强，可以在动态障碍物和半动态障碍物共存的环境下合理进行地图更新和自定位。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (2)

1.一种基于激光传感器的移动机器人长期定位系统，其特征在于，包括：长期系统模块、地图更新模块、定位模块，三个模块之间通过以下关系进行连接：

所述长期系统模块被配置成通过历史数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响，通过动态因子的传递为地图更新模块服务，以区分半动态和动态障碍物；

所述地图更新模块被配置成通过匹配度与定位能力结合的更新置信度，建立地图更新机制，并将其作为触发动态栅格更新的判据；同时，使用动态栅格概率模型，并结合由长期系统模块提供的动态因子进行栅格状态的更新，通过提供实时更新的地图为定位模块服务，以提供准确的定位先验信息；

所述定位模块被配置成对于地图更新模块中提供的实时地图，考虑未更新到地图中的动态障碍物，采用在线定位能力矩阵来衡量动态障碍物的影响，通过修正粒子建议分布函数来降低动态障碍物的干扰，提高复杂环境下的定位精度和鲁棒性，从而实现长期定位的目标；

所述地图更新模块中每个地图栅格的动态因子λ是根据栅格的观测重复性来建立的，通过在历史观测窗口内统计栅格占有属性和空闲属性的比例进行计算，计算公式如下：

其中s是观测窗口内该栅格属于占有属性的次数，f是观测窗口内该栅格属于空闲属性的次数，T是观测窗口内该栅格的状态总次数，abs()表示绝对值的计算；

其中，栅格的动态因子λ在建立之后通过实时观测数据不断进行更新，即观测窗口是动态的，随着新观测数据的输入而变化，保证动态属性能实时适应环境的变化，动态因子的更新方程如下：

是更新权重，该值大小决定了λ值更新的快慢；

所述地图更新模块中的更新置信度的计算方式如下：

将匹配度M₁和定位能力M₂加权平均后得到更新置信度M，即

M＝αM₁+βM₂；

设置α和β，其代表匹配度和定位能力的权重因子，只有当M超过一定阈值时观测才会被认定有效，触发相应的地图更新，避免地图更新错乱、失效；

其中，更新置信度中的匹配度M₁计算方法如下：

得到各激光束的距离值与期望距离值之差，并将其表示成矩阵形式r_k，其中r_N表示的是第N束激光的距离值和期望值之差，激光距离值通过传感器直接获得，而期望值通过预先载入的栅格地图获得，即

根据观测的噪声协方差，得到关于r_k的马氏距离D_k，即

根据D_k中各对角线元素d_N,计算各激光点的权重w_N，即

由此得到激光观测与环境的匹配度M₁，即

其中，更新置信度中的定位能力M₂计算方法如下：

获得机器人的当前位置p＝(x,y,θ)，激光观测模型中第i_th束激光在地图中的期望距离

观测的协方差

激光观测模型的激光束N₀；对于

是当机器人改变Δx时，其期望距离的改变量；

根据以上量，得到在线定位能力矩阵

即

将在线定位能力矩阵

的行列式值归一化后得到M₂；

其中，在地图更新模块的状态更新中，采用了动态栅格地图模型，栅格更新方程为：

其中Q_t＝[p(c_t＝occ|z_1:t)p(c_t＝free|z_1:t)]为栅格在t+1时刻的状态；

为状态转移矩阵，通过在线EM算法求得；

为观测模型，它被提前标定，并且对于每一个栅格被认为是相同的；η为归一化参数；

其中，对于动态因子λ较高的栅格，反映了影响该栅格变化的往往是半动态障碍物，其易被更新到地图中；而对于动态因子λ较低的栅格，反映了该栅格状态较为稳定，由动态障碍物引起的观测变化将不会被更新到地图中；因此对于已经更新到地图中的半动态障碍物，不用将其考虑到动态定位能力的评估上，而只需考虑未更新的动态障碍物对定位的影响；

所述定位模块在实时地图更新的基础值上，使用基于动态定位能力的粒子滤波实现长期定位，采用在线定位能力矩阵修正粒子建议分布函数，计算如下：

计算在线定位能力矩阵

其中s_i表示激光传感器打到未知障碍物的概率，

表示观测方差，其他变量与上述在线定位能力矩阵中所述变量一致；

计算增量的定位方差

其中

表示里程计输入信息的误差方差，其对角线各项分别表示右轮、左轮的线位置误差方差，以及旋转误差方差；

粒子建议分布函数修正，得到

其中，

h是里程计项的权重系数，而

是用激光匹配的粒子滤波得到的机器人位姿；

由此可得，当在线定位能力矩阵

越大时，先验信息会更多依赖于观测匹配得到的位姿；而当

越小时，先验信息会更多依赖于里程计的值，降低动态障碍物的影响。

2.一种基于激光传感器的移动机器人长期定位方法，根据权利要求1所述的定位系统的三大模块实现的，其特征在于，包括如下步骤：

步骤S1：首先，在长期系统模块中，通过历史数据，建立地图栅格的动态因子，将其作为不同栅格状态受观测信息状态变化快慢的依据来区分动态障碍物和半动态障碍物对地图更新的影响；

步骤S2：其次，在地图更新模块中，通过将匹配度和定位能力结合的更新置信度反应在当前位姿进行地图更新的可信度，并将其作为地图更新的触发条件，在此基础上，采用动态栅格模型进行栅格状态的更新；

步骤S3：最后，在定位模块中，对于未更新的动态障碍物，采用在线定位能力矩阵来衡量动态障碍物的影响，通过修正粒子建议分布函数来降低动态障碍物的干扰；

步骤S1中，每个栅格的动态因子λ是根据栅格的观测重复性来建立的，通过在历史观测窗口内统计栅格占有属性和空闲属性的比例进行计算，计算公式如下：

其中s是观测窗口内该栅格属于占有属性的次数，f是观测窗口内该栅格属于空闲属性的次数，T是观测窗口内该栅格的状态总次数，abs()表示绝对值的计算；

栅格的动态因子λ在建立之后通过实时的观测数据不断进行更新，即观测窗口是动态的，随着新观测数据的输入而变化，保证动态属性能实时适应环境的变化；动态因子的更新方程如下：

其中，

是更新权重，该值大小决定了λ值更新的快慢；

步骤S2中所述更新置信度计算方式如下：

步骤S201：将匹配度M₁和定位能力M₂加权平均后得到更新置信度M，即

M＝αM₁+βM₂；

步骤S202：设置α和β，其代表匹配度和定位能力的权重因子，只有当M超过一定阈值时观测才会被认定有效，从而进行相应的地图更新，避免引入错误的观测信息，导致地图更新错乱、失效；

其步骤S201中涉及到的匹配度M₁计算方法如下：

步骤S20101：得到各激光束的距离值与期望距离值之差，并将其表示成矩阵形式r_k，其中r_N表示的是第N束激光的距离值和期望值之差，激光距离值通过传感器直接获得，而期望值通过预先载入的栅格地图获得，即

步骤S20102：根据观测的噪声协方差，得到关于r_k的马氏距离D_k，即

步骤S20103：根据D_k中各对角线元素d_N,计算各激光点的权重w_N，即

步骤S20104：由此得到激光观测与环境的匹配度M₁，即

步骤S201中所述定位能力M₂的计算方式如下：

步骤S20111：获得机器人的当前位置p＝(x,y,θ)，激光观测模型中第i_th束激光在地图中的期望距离

观测的协方差

激光观测模型的激光束N₀；对于

是当机器人改变Δx时，其期望距离的改变量；

步骤S20112：根据以上量，得到在线定位能力矩阵

即

步骤S20113：将在线定位能力矩阵

的行列式值归一化后得到M₂；

步骤S2中的状态更新模型采用了动态栅格地图模型，栅格更新方程为：

其中Q_t＝[p(c_t＝occ|z_1:t)p(c_t＝free|z_1:t)]为栅格在t+1时刻的状态；

为状态转移矩阵，通过在线EM算法求得；

为观测模型，提前通过标定得到，并被认为对所有栅格是相同的；η为归一化参数；

步骤S3采用的在线定位能力矩阵修正粒子建议分布函数，计算如下：

步骤S301：计算在线定位能力矩阵

其中s_i表示激光传感器扫描到未知障碍物的概率，

表示观测方差，其他变量与步骤S20101所述的变量一致；

步骤S302：计算增量的定位方差

其中

表示里程计输入信息的误差方差，其对角线各项分别表示右轮、左轮的线位置误差方差，以及旋转误差方差；

步骤S303：粒子建议分布函数修正，得到

其中，

h是里程计项的权重系数，而

是用激光匹配的粒子滤波得到的机器人位姿；由此可得，当在线定位能力矩阵

越大时，先验信息会更多依赖于观测匹配得到的位姿；而当

越小时，先验信息会更多依赖于里程计的值，降低动态障碍物的影响。

Images