基于ARMA模型实现的机器人的自主定位方法与流程

本发明涉及机器人技术领域，具体地，涉及一种基于arma模型实现的机器人在长期变化环境下的自主定位方法。

背景技术：

定位问题一直是自主移动机器人领域研究的核心内容，定位层面的性能也将很大程度地影响机器人路径规划、自主导航等任务执行的表现。在静态的环境中，可以用经典的粒子滤波算法建立静态概率栅格地图，并在此基础上用激光雷达和里程计作为传感器进行自主定位。但在现实的很多实际场景中，环境往往不是一成不变的，而是受到人为的日常行动的影响，以一定的规律在发生变化。这时经典的粒子滤波算法由于建立在静态的地图之上，往往会因为环境改变导致先验地图信息的不准确而失效。例如停车场进进出出的车和来来往往的人，工业车间里被取放和搬运的货物等，都将通过环境的变化而影响粒子滤波的定位效果。

国外有基于动态定位能力矩阵进行定位的算法，并在地铁站、校园餐厅等人流量较大的动态环境下进行了测试，验证了该算法的有效性。但是以上的方法还是基于机器人所处环境静态不变的假设，这种假设使得这类方法在环境长期改变的状况下将不再适用。

对于存在用临时地图和静态地图相结合的方式进行定位，当环境信息与地图信息不匹配时，会根据当前观测信息创建临时地图，并作为定位匹配的依据；而当环境信息与地图信息匹配时，临时地图会被释放，并采用静态地图进行定位匹配。这种建模的方法虽然考虑到了环境变化对定位所造成的影响并通过临时地图进行修正，但是当环境发生结构性和大范围变化时，静态地图将完全失效，长期依靠临时地图定位不能保证定位算法的长期鲁棒性。

对于使用动态概率栅格地图的模型，并用scan-matching的匹配方式计算匹配得分，在静态地图的基础上进行实时的地图更新。当匹配得分大于设定阈值且检测到环境变化后，就进行地图更新，从而得到与当前真实环境更为吻合的地图，提高定位的鲁棒性。该算法也存在相同的问题，当环境变化与预先的静态地图相差较大时，观测信息与地图信息匹配度太差，从而无法得到机器人当前的定位信息反馈，导致静态地图的更新失效。

国外学者认为人类的许多日常活动带有周期性，有一定的规律可循，将未知的环境变化过程定义为周期性函数，利用频谱对环境的时空变化进行建模表述。同时将时域转化为频域，可高效地辨别分析并保存规律性的环境变化过程。这样机器人能在模型建立后，对环境的状态有一个预测，从而帮助机器人实现长期的定位。该方法的缺点是从方法层面而言，需要假设环境特征具有较强的周期性，建模过程相对复杂。

基于前人的工作，本发明创新性地提出了一种能在长期变化的环境下，通过对环境建模，进行地图预测与更新，从而实现机器人长期定位的算法。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于arma模型实现的机器人的自主定位方法。

根据本发明提供的一种基于arma模型实现的机器人的自主定位方法，包括以下步骤：

步骤1：环境建模步骤：使用机器人自身携带的传感器采集地图数据，通过自回归滑动平均模型arma对地图数据进行参数估计与建模，得到地图模型。

步骤2：地图预测步骤：利用地图模型进行栅格在时间域上的预测，对预测置信度进行评估，得到带有预测置信度标签的栅格地图。

步骤3：地图更新步骤：针对栅格地图与实际地图的偏差，采用贝叶斯法则将实时的观测信息融合到预测信息中，进行实时的地图更新，得到更新地图。

步骤4：自主定位步骤：基于更新地图，利用粒子滤波实现机器人的自主长期定位。

优选地，所述环境建模步骤包括：

模型定阶步骤：根据环境特性确定arma模型的第一参数p和第二参数q，所述第一参数p表征物理环境特征状态在时间上的状态关联性，所述第二参数q表征地图预测状态误差项在时间序列上的序列相关性；

参数估计步骤：基于arma模型的第一参数p和第二参数q，对第三参数(α，β)进行估计，所述第三参数(α，β)表征栅格状态在时间域上的前后状态关系，其中，α∈r^p且β∈r^q。

优选地，所述环境建模步骤还包括：

建模步骤：选定第一参数p和第二参数q的公共上界p值，即0≤p≤p且0≤q≤p；由栅格样本数迭代求出σ²的最小二乘估计，即将代入得到a(0,0),a(0,1),a(1,1),…a(p,p)，则有其中i表示地图数据中栅格的第i行，j表示地图数据中栅格的第j列，下标t表示样本个数，表示在第i行第j列的第t个样本，σ²表示最小二乘估计的值，k表示地图数据中栅格的第k行，表示第k行第j列的栅格样本的最小二乘估计值，表示栅格样本的均值，表示栅格样本最小二乘估计的回归系数a，表示栅格样本最小二乘估计的回归系数b，n表示时间序列的总数，表示第k行第j列的栅格样本的aic准则值，a(p,q)表示模型定阶确定的p和q的值所对应的aic准则值，表示第k行第j列的栅格样本的aic准则值。

优选地，所述参数估计步骤包括：

拟合步骤：利用栅格样本数作高阶自回归滑动模型ar(p)的拟合；

递推步骤：递推计算残差序列即

t＝p+1,p+2,…t

回归步骤：将残差序列作为独立序列，利用线性回归模型：

得到第三参数表示如下所示：

下标t表示采样时间序列，t＝p+1,p+2,…t；表示状态误差；表示p时刻栅格在arma模型下的回归参数；αi表示arma模型的回归参数；βj表示arma模型的误差滑动参数；z′表示z的转置，表示的转置，表示概率栅格地图的状态值；

模型更换步骤：设置模型参数检测，当参数变化大于设定阈值进行模型更换，即

s＝|p1-p2|+|q1-q2|＞sth

其中下标为1和2分别代表计算的两组参数模型；当其阶数差值或者参数大于设定值后认为模型发生改变，用当前模型参数替换前模型参数。

优选地，所述地图预测步骤包括：

状态预测步骤：利用已有的地图数据进行栅格在时间域上的预测，得到每个栅格在t时刻的栅格状态，得到整张预测栅格地图。

置信度评估步骤：通过预测状态与真实观测状态的比较，确定每个栅格状态的预测置信度，每次置信度评估前赋予初始的置信度，再通过数据库更新得到的真实地图状态进行比较，得到最终的预测置信度。最终输出带有预测置信度标签的栅格地图。

优选地，所述地图更新步骤包括：

引入步骤：引入定位能力矩阵以衡量不同位置处的可信度；

归一化步骤：对定位能力矩阵的求行列式并归一化后得到m2，m2反应了地图不同位置处对应的数据可信度的大小；修正后的匹配度作为有效观测数据的判别标准，即：

m＝λm1+(1-λ)m2

其中，λ是观测权重项，表示对观测匹配的可行度，λ值越大，代表观测匹配在整个匹配度的计算权重中所占的比例越大；m表示修正后的地图与观测数据的匹配度，m1表示地图与观测数据的匹配度，m2表示地图不同位置处对应的数据可信度；当m大于设定的阈值后，即m＞mth，则定义观测得到的数据是有效的，从而将m作为可靠数据。

优选地，所述地图更新步骤包括:

数据融合步骤：应用贝叶斯法则进行数据融合，将长期预测信息与短期观测信息相融合，获得实时准确的地图；使用动态概率栅格地图作为地图数据储存的形式，使用hmms作为基本模型，融入长期信息中的置信度作为权重项，进行状态更新，得到t+1时刻的状态向量，融合公式如下所示：

其中qt为融合数据后输出的地图栅格状态，ac是状态转移矩阵，g是预测置信度，x(t)是预测状态，bz是观测信息，η是归一化因子,i是单位矩阵；下标t表示时间序列t，下标k表示地图栅格的第k行，下标c表示矩阵，下标z表示观测

所述自主定位步骤在得到实时更新的动态地图之后，结合动态定位能力来评估环境中未更新到地图中的动态属性，用基于定位能力的粒子滤波算法实现机器人的自主长期定位。

优选地，环境建模步骤采用离线形式，地图预测步骤和地图更新步骤采用在线形式。

优选地，模型定阶步骤采用aic准则确定，参数估计步骤采用自回归逼近。

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

能够使机器人在长期变化的环境下具有很好的定位精度和定位鲁棒性。

附图说明

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

图1为本发明的方法流程图。

具体实施方式

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

针对原有技术的不足，本文创新性地提出了一种基于arma模型实现的机器人的自主定位方法，适用于长期变化环境下的机器人自主定位问题。

本发明提出的基于arma模型实现的机器人在长期变化环境下的自主定位方法，主要包括以下步骤：

步骤1：建模，使用机器人自身携带的传感器采集地图数据，通过自回归滑动平均模型(arma)对地图数据进行参数估计与建模。

步骤2：预测，利用建立好的模型进行栅格在时间域上的预测，并对预测置信度进行评估，最终输出带有预测置信度标签的栅格地图。

步骤3：更新，针对一些不确定因素所导致的预测地图与实际地图的偏差，采用贝叶斯法则将实时的观测信息融合到预测信息中，进行实时的地图更新，作为粒子滤波定位的先验地图依据。使用粒子滤波算法实现机器人的自主长期定位。

具体地，所述步骤1的建模过程主要包括以下步骤：

步骤1.1：模型定阶，在arma模型建立前，要确定模型的参数p和q。参数p表征的是物理环境特征状态在时间上的状态关联性，参数q表征的是地图预测状态误差项在时间序列上的相关性。对于地图中第i行，第j列的栅格，建模过程如下所示：

步骤1.1.1选定阶数p和q的公共上界p值，即0≤p,q≤p

步骤1.1.2由样本数迭代求出σ²的最小二乘估计，即

步骤1.1.3将代入得到a(0,0),a(0,1),a(1,1),…a(p,p)，则有

则此时p,q即为需要的模型阶数。

步骤1.2：参数估计，当有了自回归滑动平均模型的参数p和q后，需要对其模型的参数进行估计。这里需要确定的参数是α以及β，其中，α∈r^p，β∈r^q，表征的物理意义是栅格状态在时间域上的前后状态关系。参数估计采用了自回归逼近的方法，具体的步骤如下所示：

步骤1.2.1首先利用栅格原始数据作高阶自回归滑动模型ar(p)的拟合

步骤1.2.2由上述估计的ar(p)模型递推计算残差序列即

t＝p+1,p+2,…t

步骤1.2.3将残差列为独立序列，利用线性回归模型：

其中t＝p+1,p+2,…t

从而得到估计参数表示如下所示：

其中，

考虑到环境模型的变化，需要设置模型参数检测，当参数变化大于一定阈值进行模型更换，即

s＝|p1-p2|+|q1-q2|＞sth

其中下标为1和2分别代表计算的两组参数模型。当其阶数差值或者参数大于设定值后都认为模型发生改变，用当前模型参数替换前模型参数。

具体地，所述步骤2的预测过程主要包括以下步骤：

步骤2.1：状态预测，利用已有的地图数据进行栅格在时间域上的预测，从而可以得到每个栅格在t时刻的栅格状态，从而可以得到整张预测栅格地图。

步骤2.2：置信度评估，对栅格状态的预测后，还需要对预测置信度进行评估，用来衡量每个栅格预测的准确性。通过预测状态与真实观测状态的比较，来确定每个栅格状态的预测置信度，每次置信度评估前都会为其赋予初始的置信度，再结合通过数据库更新得到的真实地图状态进行比较，得到最终的预测置信度。最终输出带有预测置信度标签的栅格地图。

具体地，所述步骤3的更新过程主要包括以下步骤：

步骤3.1：引入定位能力矩阵来衡量不同处位置处的可信度

步骤3.2：对定位能力矩阵求行列式并归一化后得到m2，其反应了地图不同位置处对应的数据可信度的大小。修正后的匹配度作为有效观测数据的判别标准，即：

m＝λm1+(1-λ)m2

其中，λ是观测权重项，其值代表着对观测匹配的可行度，该值越大，代表观测匹配在整个匹配度的计算权重中所占的比例越大。只有当m当于设定的阈值后，即m＞mth才认为观测得到的数据是有效的，从而将其作为可靠数据。

步骤3.3应用贝叶斯法则进行数据融合，将长期预测信息与短期观测信息相融合，获得实时、准确的地图。使用动态概率栅格地图作为地图数据储存的形式，并使用hmms作为其基本模型。同时融入长期信息中的置信度作为权重项，进行状态更新，得到t+1时刻的状态向量，融合公式如下所示：

其中qt为融合数据后输出的地图栅格状态，ac是状态转移矩阵，g是预测置信度，x(t)是预测状态，bz是观测信息，η是归一化因子,i是单位矩阵。

在得到实时更新的动态地图之后，结合动态定位能力来评估环境中未更新到地图中的动态属性，用基于定位能力的粒子滤波算法实现机器人的自主长期定位。

具体地，步骤1建模过程采用离线形式，步骤2预测过程和步骤3更新过程采用在线形式，所述传感器为激光雷达，所述环境特征数据保存在地图数据库中。

定阶过程采用aic准则确定，参数估计采用自回归逼近的方法，状态预测所用的预测表达式如下式

具体地，只有当激光数据与地图的匹配吻合程度达到一定阈值时，才认为在当前的定位下获得的观测激光数据是有效的，才会将观测数据融合到预测信息中。

具体地，引入了定位能力矩阵来衡量不同处位置处的可信度，如下式所示：

其中，p＝(x,y,θ)是机器人的位姿。是在lrf模型中的ith激光束中的期望距离。n0是lrf模型激光束的总数量。而对于表示机器人移动δx后lrf模型的激光束返回值的改变。

具体地，地图信息融合公式如下所示：

其中qt为融合数据后输出的地图栅格状态，ac是状态转移矩阵，g是预测置信度，x(t)是预测状态，bz是观测信息，η是归一化因子,i是单位矩阵。

具体地，最终机器人的坐标可以表示成下式

在具体的实施过程中，如图1所示，本发明分为(1)建模(2)预测(3)更新三个部分进行，主要有以下步骤：

步骤1：通过机器人自身携带的传感器对环境数据进行采集，建立时间维度上的环境特征数据库。通过自回归滑动平均模型(arma)进行参数估计与建模，对时间维度上的环境特征状态的关联性作评估，进而预测环境状态的未来变化和走向。

步骤1.1：模型定阶，进行arma模型的建立前，首先要确定的是模型的参数p和q。参数p表征的是物理环境特征状态在时间上的状态关联性，参数q表征的是地图预测状态误差项在时间序列上的相关性。定阶过程这里采用aic准则进行确定，对于地图中第i行，第j列的栅格，建模过程如下所示：

步骤1.1.1选定阶数p和q的公共上界p值，即0≤p,q≤p

步骤1.1.2由样本数迭代求出σ²的最小二乘估计，即

步骤1.1.3将代入得到a(0,0),a(0,1),a(1,1),…a(p,p)，则有

则此时p,q即为需要的模型阶数。

步骤1.2：参数估计，当有了自回归滑动平均模型的参数p和q后，需要对其模型的参数进行估计。这里需要确定的参数是α以及β，其中，α∈r^p，β∈r^q，表征的物理意义是栅格状态在时间域上的前后状态关系。参数估计采用了自回归逼近的方法，具体的步骤如下所示：

步骤1.2.1首先利用栅格原始数据作高阶自回归滑动模型ar(p)的拟合

步骤1.2.2由上述估计的ar(p)模型递推计算残差序列即

t＝p+1,p+2,…t(3)

步骤1.2.3将残差列为独立序列，利用线性回归模型：

其中t＝p+1,p+2,…t

从而得到估计参数表示如下所示：

其中，

考虑到环境模型的变化，需要设置模型参数检测，当参数变化大于一定阈值进行模型更换，即

s＝|p1-p2|+|q1-q2|＞sth

其中下标为1和2分别代表计算的两组参数模型。当其阶数差值或者参数大于设定值后都认为模型发生改变，用当前模型参数替换前模型参数。

步骤2：利用建立好的模型进行栅格在时间域上的预测，并对预测置信度进行评估，最终输出带有预测置信度标签的栅格地图。

步骤2.1：状态预测，利用已有的地图数据进行栅格在时间域上的预测，从而可以得到每个栅格在t时刻的栅格状态，从而可以得到整张预测栅格地图。预测表达式如下所示

步骤2.2：置信度评估，对栅格状态的预测后，还需要对预测置信度进行评估，用来衡量每个栅格预测的准确性。通过预测状态与真实观测状态的比较，来确定每个栅格状态的预测置信度，每次置信度评估前都会为其赋予初始的置信度，再结合通过数据库更新得到的真实地图状态进行比较，得到最终的预测置信度。最终输出带有预测置信度标签的栅格地图。具体实现过程如下所示：

步骤3：针对一些不确定因素所导致的预测地图与实际地图的偏差，采用贝叶斯法则将实时的观测信息融合到预测信息中，进行实时的地图更新，作为粒子滤波定位的先验地图依据。具体步骤如下

步骤3.1引入定位能力矩阵来衡量不同处位置处的可信度，如下所示：

其中，p＝(x,y,θ)是机器人的位姿。是在lrf模型中的ith激光束中的期望距离。n0是lrf模型激光束的总数量。而对于表示机器人移动δx后lrf模型的激光束返回值的改变。

步骤3.2对定位能力矩阵求行列式并归一化后得到m2，其反应了地图不同位置处对应的数据可信度的大小。当m2较大时，即地图信息特征丰富，数据可信度高；而当m2较小时，即地图信息特征少，数据可信度低。修正后的匹配度作为有效观测数据的判别标准，即：

m＝λm1+(1-λ)m2(9)

其中，λ是观测权重项，其值代表着对观测匹配的可行度，该值越大，代表观测匹配在整个匹配度的计算权重中所占的比例越大。只有当m当于设定的阈值后，即m＞mth才认为观测得到的数据是有效的，从而将其作为可靠数据。

步骤3.3应用贝叶斯法则进行数据融合，将长期预测信息与短期观测信息相融合，获得实时、准确的地图。使用动态概率栅格地图作为地图数据储存的形式，并使用hmms作为其基本模型。同时融入长期信息中的置信度作为权重项，进行状态更新，得到t+1时刻的状态向量，融合公式如下所示：

其中qt为融合数据后输出的地图栅格状态，ac是状态转移矩阵，g是预测置信度，x(t)是预测状态，bz是观测信息，η是归一化因子,i是单位矩阵。

基于此，在置信度较高的栅格，将更多地依赖于预测的状态作为地图的最终形态；而对于置信度较低的栅格，将更多地依赖于实时的观测数据进行迭代更新。以此得到更符合真实环境的地图。

步骤4：在得到实时更新的动态地图之后，结合动态定位能力来评估环境中未更新到地图中的动态属性，用基于定位能力的粒子滤波算法实现机器人的自主长期定位。具体实现如下：

从初始地图中体现定位能力的动态定位能力矩阵表示如下：

其中si是激光束扫到未知障碍物的概率。是观测信息方差。p＝(x,y,θ)是机器人的位姿。是在lrf模型中的ith激光束中的期望距离。n0是lrf模型激光束的总数量。而对于表示机器人移动δx后lrf模型的激光束返回值的改变。

通过采用经典的数据融合算法来融合观测信息和里程计信息来修正建议分布函数。之后，每个粒子的δo和p都会被δo^(k)和替换。因此，融合过后的里程计增量将变成

其中h是比例因子，可通过经典的粒子滤波算法获得，而表示在只有里程计增量下算得的机器人坐标。

当获得修正的里程计增量后，修正的采样粒子可以表示成

最后机器人的坐标可以表示成

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

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