一种测深信息同步定位与建图的鲁棒后端图优化方法与流程

本发明属于水下机器人的水下导航领域，具体涉及一种测深信息同步定位与建图的鲁棒后端图优化方法。

背景技术：

随着多波束测深技术的发展，高精度海底地形测绘成为可能，因而以多波束声纳测绘得到的海底地形为信息源的同步定位与建图技术已经得到了世界各国的广泛重视。然而由于多波束声纳测量精度受海洋环境影响严重，出现无效数据关联是难以避免的。因而，在应用图优化算法求解多波束同步定位与建图时，需要提高图优化算法框架的鲁棒性，使图优化算法框架能够解决无效数据关联问题。本发明提出了一种能够解决无效数据关联的粒子群图优化方法，该方法通过粒子保存的协方差对数据关联的置信度进行评估，通过图优化算法结合每个粒子保存的协方差求解该粒子对应的最优位姿序列，并计算地图一致性作为单个粒子的适应度函数，通过粒子群优化算法求解所有数据关联的最优协方差，使用该结果实时计算auv最优位姿序列，解决多波束同步定位与建图问题。

目前现有的图优化算法框架中无效数据关联判别方法大多是使用有效/无效两种状态对数据关联进行判别，但这种方法无法对数据关联中噪声占有效值的比重进行表示，且某些基于贪心算法的迭代寻优方法会存在局部最优问题。本发明使用协方差对数据关联有效性进行判别，协方差为0到1之间的自然数，能更好的反应数据关联的置信程度；使用粒子群方法进行协方差值的寻优，可以克服局部最优的陷阱。

公开日为2017年9月5日、公开号为cn107132521a、发明名称为“一种bslam中地形匹配结果正确性判别方法”的专利申请，该方法通过不断循环进行位姿图优化以达到剔除无效数据关联并修正位姿图的目的，但该方法使用有效/无效两z种状态对数据关联进行判别，忽略了数据关联中噪声与有效值之间的比例关系，导致建图结果精度不足。

技术实现要素：

本发明的目的在于提供一种测深信息同步定位与建图的鲁棒后端图优化方法，该方法通过粒子群算法对数据关联的协方差进行评估，并通过图优化算法结合粒子群结果解决多波束同步定位与建图问题。

本发明的目的是这样实现的：

一种测深信息同步定位与建图的鲁棒后端图优化方法，包含以下步骤：

步骤一：设t为计时参数，初始化t等于0；

步骤二：水下机器人航行过程中通过多波束声纳和惯性导航系统采集海底地形信息和水下机器人位姿状态记录在子地图中，每隔一秒令t＝t+1，并执行步骤三；

步骤三：判断t是否大于100，若t大于100，则保存当前子地图，并与历史子地图通过地形匹配检测数据关联，开启新子地图并执行步骤四；否则执行步骤二；

步骤四：判断是否检测得到新的数据关联，是则执行步骤五，否则执行步骤一；

步骤五：判断当前获得的数据关联是否为任务执行过程中获取的第一个数据关联，是则执行步骤六，否则执行步骤七；

步骤六：初始化粒子并执行步骤八；

步骤七：根据检测到的数据关联与所有历史数据关联更新粒子并执行步骤八；

步骤八：对所有粒子通过图优化求解地图并计算适应度函数，根据适应度函数应用粒子群算法更新粒子，并根据粒子群输出结果通过图优化求解并输出地图，执行步骤九；

步骤九：判断水下机器人是否到达终点，是则算法结束，否则执行步骤一。

所述的步骤六包含以下步骤：

步骤6.1：将数据关联j表示为(tj,1,tj,2,ej,1,ej,2)，其中tj,1,tj,2表示该数据关联所包含路径的起始时刻和终止时刻，ej,1,ej,2表示该数据关联表示的两个时刻在东向和北向的偏移量，执行步骤6.2；

步骤6.2：设定粒子数为m个，粒子状态xi(i＝1,2...,m)表示为：

xi(i＝1,2...,m)＝{covi,1}

其中covi,1为第i个粒子保存的第1个数据关联的协方差，该值初始化为0到1之间的随机数。

所述的步骤七包含以下步骤：

步骤7.1：设n和index为计数参数，初始化n＝1，index＝0，执行步骤7.2；

步骤7.2：计算数据关联j与第n组数据关联的距离dj,n，其可以计算为：

其中系数c1、c2控制了时间和偏移量的权重，取c1＝100c2，分别为第n组数据关联所有起始时刻、终止时刻、东向偏移量、北向偏移量的算数平均值；执行步骤7.3；

步骤7.3：判断dj,n是否小于阈值dmin，若dj,n大于dmin，执行步骤5.6，否则令dmin＝dj,n、index＝n并执行步骤7.4；

步骤7.4：设n为计数上限，令n＝n+1，判断n是否大于n，是则执行步骤7.5，否则执行步骤7.2；

步骤7.5：若index等于0，则将数据关联j的协方差covj插入到所有粒子的状态的末尾，即令xi(i＝1,2...,m)＝{covi,1,covi,2,...,covi,n,covj}，对于每个粒子covj都为0到1之间的随机数；否则将数据关联j归入第index组数据关联中。

所述的步骤八包含以下步骤：

步骤8.1：使用粒子i(i＝1,2...,m)内保存的各个数据关联协方差，通过图优化算法求解auv的最优位姿序列poseⁱ＝{pose1ⁱ,pose2ⁱ,...,posekⁱ}，其中：

pose1ⁱ＝{pose1,xⁱ,pose1,yⁱ}，

pose1,xⁱ,pose1,yⁱ分别表示1时刻auv在东向、北向位置；

步骤8.2：对于粒子i(i＝1,2...,m)，其适应度函数根据其最优位姿序列poseⁱ＝{pose1ⁱ,pose2ⁱ,...,posekⁱ}计算得到，设auv惯性导航系统给出的位姿序列为dr＝{dr1,dr2,...,drk}且dr1＝{dr1,x,dr1,y}，其中dr1,x,dr1,y分别表示1时刻惯性导航系统给出的auv在东向、北向位置，则粒子i(i＝1,2...,m)适应度函数：

其中：

步骤8.3：根据粒子适应度函数，使用粒子群优化迭代更新粒子坐标，直到迭代次数超过阈值或粒子最优适应度函数大于1000，执行步骤8.4；

步骤8.4：使用最优适应度函数对应的粒子保存的数据关联协方差，通过图优化算法计算最优位姿序列并输出。

与现有技术相比，本发明的有益效果在于提出的应用bslam的粒子群图优化方法可以解决传统图优化方法无法处理的无效数据关联问题，且使用协方差而非简单的有效/无效对数据关联进行判断，计算精度更高，使多波束同步定位与建图技术能够真正应用于海洋环境中。

附图说明

图1是本发明的总流程图；

图2是本发明步骤七更新粒子的流程图。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

本发明公开的是一种测深信息同步定位与建图的鲁棒后端图优化方法，主要是一种对水下机器人以多波束为传感器的测深信息同步定位与建图(bathymetricsimultaneouslocalizationandmapping，bslam)问题的求解方法。

结合图1，本发明的具体流程如下：

步骤一、初始化t等于0。

步骤二、水下机器人航行过程中通过多波束声纳和惯性导航系统采集海底地形信息和水下机器人位姿状态记录在子地图中，每隔一秒令t＝t+1并执行步骤三。

步骤三、判断t是否大于100，是的话保存当前子地图并与历史子地图通过地形匹配检测数据关联，开启新子地图并执行步骤四，否则执行步骤二。

步骤四、判断是否检测得到新的数据关联，是则执行步骤五，否则执行步骤一。

步骤五、判断当前获得的数据关联是否为任务执行过程中获取的第一个数据关联，是则执行步骤六，否则执行步骤七。

步骤六、初始化粒子并执行步骤八，步骤六具体为：

步骤6.1、将数据关联j表示为(tj,1,tj,2,ej,1,ej,2)，其中tj,1,tj,2表示该数据关联所包含路径的起始时刻和终止时刻，ej,1,ej,2表示该数据关联表示的两个时刻在东向和北向的偏移量，执行步骤6.2。

步骤6.2、设定粒子数为m个，粒子状态xi(i＝1,2...,m)表示为

xi(i＝1,2...,m)＝{covi,1}

其中covi,1为第i个粒子保存的第1个数据关联的协方差，该值初始化为0到1之间的随机数。

步骤七、根据检测到的数据关联与所有历史数据关联更新粒子并执行步骤八。结合图2，步骤七具体为：

步骤7.1、初始化n＝1，index＝0，执行步骤7.2。

步骤7.2、计算数据关联j与第n组数据关联的距离dj,n，其可以计算为：

其中系数c1、c2控制了时间和偏移量的权重，通常取c1＝100c2，分别为第n组数据关联所有起始时刻、终止时刻、东向偏移量、北向偏移量的算数平均值，执行步骤7.3。

步骤7.3、判断dj,n是否小于阈值dmin，若dj,n大于dmin，执行步骤5.6，否则令dmin＝dj,n、index＝n并执行步骤7.4。

步骤7.4、令n＝n+1，判断n是否大于n，是则执行步骤7.5，否则执行步骤7.2。

步骤7.5、若index等于0，则将数据关联j的协方差covj插入到所有粒子的状态的末尾，即令xi(i＝1,2...,m)＝{covi,1,covi,2,...,covi,n,covj}，对于每个粒子covj都为0到1之间的随机数；否则将数据关联j归入第index组数据关联中。

步骤八、对所有粒子通过图优化求解地图并计算适应度函数，根据适应度函数应用粒子群算法更新粒子，并根据粒子群输出结果通过图优化求解并输出地图，执行步骤九，步骤八具体为：

步骤8.1、使用粒子i(i＝1,2...,m)内保存的各个数据关联协方差，通过图优化算法求解auv的最优位姿序列poseⁱ＝{pose1ⁱ,pose2ⁱ,...,posekⁱ}，其中

pose1ⁱ＝{pose1,xⁱ,pose1,yⁱ}，

pose1,xⁱ,pose1,yⁱ分别表示1时刻auv在东向、北向位置，执行步骤8.2。

步骤8.2、对于粒子i(i＝1,2...,m)，计算其适应度函数并执行步骤8.3。其中适应度函数可以根据其最优位姿序列poseⁱ＝{pose1ⁱ,pose2ⁱ,...,posekⁱ}计算得到，假设auv惯性导航系统给出的位姿序列为dr＝{dr1,dr2,...,drk}且dr1＝{dr1,x,dr1,y}，其中dr1,x,dr1,y分别表示1时刻惯性导航系统给出的auv在东向、北向位置，则粒子i(i＝1,2...,m)适应度函数

其中

步骤8.3、根据粒子适应度函数，使用粒子群优化迭代更新粒子坐标，直到迭代次数超过阈值或粒子最优适应度函数大于1000，执行步骤8.4。

步骤8.4、使用最优适应度函数对应的粒子保存的数据关联协方差，通过图优化算法计算最优位姿序列并输出。

步骤九、判断水下机器人是否到达终点，是则算法结束，否则执行步骤一。