一种智慧城市传感器探测主动同步定位与建图方法

1.本发明属于城市定位与导航技术领域，具体涉及一种智慧城市传感器探测主动同步定位与建图方法。


背景技术：

2.受探测技术以及成本限制，截至目前全球城市测绘的比例较低，受地形遮挡，城市的地形变化难以呈现细节，而传感器作为人们测绘的最佳工具之一被广泛地应用于城市地形测量。其工作范围广泛且能够获得分辨率较高的地形测量数据。但是由于能耗限制，有必要采用合适的算法对测绘效率进行优化。
3.现有方法多是通过感测器，比如光学镜头、激光光达、彩色相机或深度相机，进行周围环境的感测，并将环境状况记录于地图当中，而在建立地图的同时，根据环境特征在地图上定位待定位物件。然而现有方法只是对探测同步定位建图算法的后端进行改善，并未涉及主动决策回环与探索动作选择方面。


技术实现要素：

4.本发明的目的在于提供一种智慧城市传感器探测主动同步定位与建图方法。
5.本发明的目的是这样实现的：
6.一种智慧城市传感器探测主动同步定位与建图方法，包括以下步骤：
7.(1)设定任务区域；
8.(2)对所设定的任务区域进行全覆盖路径规划；
9.(3)执行传感器同步定位与地图绘制，并构建轨迹地图map
t
；
10.(4)根据轨迹地图确定候选回环目标和候选探索目标；
11.(5)根据效用方程计算到达每个候选点的收益，选择对应收益最优的候选点，根据传感器向收益最优候选点行驶的动作n
*
，判断执行回环任务或探索任务；
12.(6)执行传感器向收益最优候选点行驶的动作n
*
，判断设定的任务区域是否探索完成；若完成探索，则结束；否则，返回步骤(3)。
13.所述步骤(4)包括：
14.(4.1)根据传感器测线的平均宽度j将轨迹地图map
t
分成子地图集合；
15.(4.2)提取传感器当前位置(x,y)和位置协方差矩阵ξ
16.(4.3)确定候选点搜索半径：
[0017][0018]
(4.4)搜索半径e内的所有子地图subi，计算子地图的数量num，
[0019]
(4.5)计算子地图的数据矩阵的行列值zi，ai；
[0020]
(4.6)采集对应位置(n,o)的地形高程u
no
；
[0021]
(4.7)计算在搜索半径e内的所有子地图的地形费舍尔信息量{gi|i＝1,2,
…
,
num}
[0022][0023]
||
·
||表示欧式范数；
[0024]
(4.8)提取预设的地形费舍尔信息量阈值g
t
；
[0025]
(4.9)取所有满足gi》g
t
的子地图中心点作为回环目标点集合根据当前传感器位置到回环目标的距离，在前进方向上选取相同距离的点作为探索目标点，生成探索目标点集合；
[0026]
(4.10)输出回环候选点集合和探索候选点集合；
[0027]
所述步骤(5)具体为：
[0028]
(5.1)计算受动作n
′
影响的地图体积in′
；
[0029]
(5.2)计算受到每个重访动作相应探索行动影响的地图体积i
ex
；
[0030]
(5.3)计算系统平衡因子τ，
[0031][0032]
(5.4)提取历史控制向量的集合h和历史观测集合m；
[0033]
(5.5)提取执行动作n
′
而探测得到的地图信息z(n
′
)；z表示某一栅格的地图信息；
[0034]
(5.6)计算通过预测得到的到达目标点时传感器的位置协方差当前时刻传感器的位置协方差
[0035]
(5.7)采集目标子地图subi的地形费舍尔信息量gi和搜索范围内最大的地形费舍尔信息量g
max
；
[0036]
(5.8)计算执行动作n
′
后传感器的位姿不确定性
[0037][0038]
(5.9)计算栅格z的香农熵：
[0039][0040]
(5.10)计算栅格z的瑞利熵：
[0041][0042][0043]
(5.11)执行动作n
′
时的互信息vn′
：
[0044]
[0045]
(5.12)计算到达每个候选点的收益τvn′
；
[0046]
(5.13)选择收益最优时传感器向对应的候选点行进的动作n
*
[0047][0048]
本发明的有益效果在于：
[0049]
与传统的传感器探测信息同步定位与建图方法相比，本发明通过主动对地形进行有目的性地回溯，平衡了回溯和探索之间的关系，从而使得传感器能够更有效的在不依赖载体的情况下获得高精度的位置信息和城市地形图。
附图说明
[0050]
图1为本发明的总体流程图。
具体实施方式
[0051]
下面结合附图对本发明做进一步描述。
[0052]
本发明涉及水下机器人主动同步定位与建图方法，该方法通过主动选择进行回环动作或探索动作，使auv在探索效率得到提高的同时兼顾所构建地图的准确性。
[0053]
一种智慧城市传感器探测主动同步定位与建图方法，包括以下步骤：
[0054]
(1)设定任务区域；
[0055]
(2)对所设定的任务区域进行全覆盖路径规划；
[0056]
(3)执行传感器同步定位与地图绘制，并构建轨迹地图map
t
；
[0057]
(4)根据轨迹地图确定候选回环目标和候选探索目标；
[0058]
(4.1)根据传感器测线的平均宽度j将轨迹地图map
t
分成子地图集合；
[0059]
(4.2)提取传感器当前位置(x,y)和位置协方差矩阵ξ
[0060]
(4.3)确定候选点搜索半径：
[0061][0062]
(4.4)搜索半径e内的所有子地图subi，计算子地图的数量num，
[0063]
(4.5)计算子地图的数据矩阵的行列值zi，ai；
[0064]
(4.6)采集对应位置(n,o)的地形高程u
no
；
[0065]
(4.7)计算在搜索半径e内的所有子地图的地形费舍尔信息量{gi|i＝1,2,
…
,num}
[0066][0067]
||
·
||表示欧式范数；
[0068]
(4.8)提取预设的地形费舍尔信息量阈值g
t
；
[0069]
(4.9)取所有满足gi》g
t
的子地图中心点作为回环目标点集合根据当前传感器位置到回环目标的距离，在前进方向上选取相同距离的点作为探索目标点，生成探索目标点集合；
[0070]
(4.10)输出回环候选点集合和探索候选点集合；
[0071]
(5)根据效用方程计算到达每个候选点的收益，选择对应收益最优的候选点，根据传感器向收益最优候选点行驶的动作n
*
，判断执行回环任务或探索任务；
[0072]
(5.1)计算受动作n
′
影响的地图体积in′
；
[0073]
(5.2)计算受到每个重访动作相应探索行动影响的地图体积i
ex
；
[0074]
(5.3)计算系统平衡因子τ，
[0075][0076]
(5.4)提取历史控制向量的集合h和历史观测集合m；
[0077]
(5.5)提取执行动作n
′
而探测得到的地图信息z(n
′
)；z表示某一栅格的地图信息；
[0078]
(5.6)计算通过预测得到的到达目标点时传感器的位置协方差当前时刻传感器的位置协方差
[0079]
(5.7)采集目标子地图subi的地形费舍尔信息量gi和搜索范围内最大的地形费舍尔信息量g
max
；
[0080]
(5.8)计算执行动作n
′
后传感器的位姿不确定性
[0081][0082]
(5.9)计算栅格z的香农熵：
[0083][0084]
(5.10)计算栅格z的瑞利熵：
[0085][0086][0087]
(5.11)执行动作n
′
时的互信息vn′
：
[0088][0089]
(5.12)计算到达每个候选点的收益τvn′
；
[0090]
(5.13)选择收益最优时传感器向对应的候选点行进的动作n
*
[0091][0092]
(6)执行传感器向收益最优候选点行驶的动作n
*
，判断设定的任务区域是否探索完成；若完成探索，则结束；否则，返回步骤(3)。
[0093]
本发明不需要物理场数据库的支持，通过研究传感器定向算法，能够在智慧城市复杂非结构化的环境中完成自主同步定位。该同步定位模式隐蔽性高，鲁棒性强。相较于普通slam的应用场景，智慧城市环境复杂，对于以视觉传感器为主的slam系统有着诸多限制。
因此本发明充分利用传感器的位置信息，为场景识别提供位置数据，显著提高组合定位定向精度，并有效抑制自主定位过程中的定位累计误差。目前定位简图系统数据融合多采用卡尔曼滤波器，对于误差参数的估计精度严重依赖所建模型的准确度。本发明避免了状态或量测向量的线性化，通过主动对地形进行有目的性地回溯，使算法确保在未知和时变噪声情况下能够有效抑制滤波器发散，提高传感快速跟踪能力，从而改善系统性能。