多楼层环境机器人静态自动重定位初始位姿确定方法和系统

本发明涉及移动机器人定位，具体涉及多楼层环境机器人静态自动重定位初始位姿确定方法和系统。

背景技术：

1、移动机器人在医院、办公楼、教学楼、仓储楼宇和商业建筑等多楼层环境中执行巡检、配送、引导和作业任务时，需要根据地图确定机器人基座在地图坐标系下的初始位姿。初始位姿包含机器人基座的位置和朝向。机器人发生定位丢失、完成跨层转运后重新放置以及接收新的重定位请求时，若未能获得可靠初始位姿，后续定位、路径规划和导航控制缺少可用的起始条件。

2、三维激光重定位技术通常根据场景描述子从地图关键帧中检索地点候选，再通过局部点云刚体配准获得候选位姿。对于结构差异明显的单层场景，该技术路线能够提供地点候选及配准后的候选位姿。在多楼层环境中，走廊、门洞、立柱、楼梯间入口和设备间外部结构会在不同高度层重复出现，多个候选位姿能够同时通过局部点云刚体配准。

3、现有方案中，一类方案按照单个候选位姿的配准质量选择初始位姿；一类方案扩大局部地图范围后重新配准；一类方案根据人工位置、人工朝向、楼层信息和历史位置缩小候选范围。单个候选位姿的配准质量不能表达跨高度候选位姿之间是否存在当前传感器方向可观测的差异；扩大局部地图范围会引入重复结构和地图边界区域；人工信息和历史位置不适用于未接收人工位置、朝向和楼层信息的静态自动重定位任务。

技术实现思路

1、本发明要克服现有技术的上述不足，提供一种面向多楼层环境机器人静态自动重定位初始位姿确定方法和系统。

2、本发明在未接收人工位置、朝向和楼层信息时，获取静态多帧三维激光数据，构造机器人基座坐标系下的静态查询点云；从多楼层地图获得经基础几何验证的候选位姿，依据竖直坐标和结构相似性形成竞争候选对，并预先定义偏航扰动集合；将候选局部地图分别变换至各自机器人基座坐标系，在统一方向单元内确定反事实预测差异方向单元；依据实际首个有效回波形成支持证据、矛盾证据和未知证据，经确定性扰动模板复核形成差异可见性见证单元及合格有向证据边；构造候选有向证据图，仅在存在唯一严格支配节点时输出认证通过状态并发布对应初始位姿，未满足条件时输出拒绝发布状态并重新获取静态观测窗口。本发明能够在多楼层重复结构场景中依据候选间的实际可见性差异确定初始位姿。

3、本发明的第一个方面涉及多楼层环境机器人静态自动重定位初始位姿确定方法，包括以下步骤：

4、步骤s1：在未接收人工位置、朝向和楼层信息时，获取静态多帧三维激光数据，构造机器人基座坐标系下的静态查询点云。

5、步骤s2：从多楼层地图获得经基础几何验证的候选位姿，按竖直坐标和结构相似性形成竞争候选对，并将两个候选位姿分别命名为候选位姿a和候选位姿b，并预先定义每一竞争候选对的偏航扰动集合。

6、步骤s3：将两个候选位姿对应的候选局部地图分别变换至各自机器人基座坐标系；在统一的方位角—俯仰角方向单元中，有效地图回波对应预测首个有效回波状态，无有效地图回波对应无预测回波状态；将两个候选位姿的预测状态不同的方向单元，以及两个候选位姿均处于预测首个有效回波状态且预测首个有效回波距离之差达到预设反事实距离差阈值的方向单元，确定为反事实预测差异方向单元。

7、步骤s4：利用静态查询点云中的实际首个有效回波对两个候选位姿的预测首个有效回波进行比较，根据比较结果形成支持证据、矛盾证据和未知证据，未知证据不计入支持证据和矛盾证据；对两个候选位姿施加预设平移扰动和偏航扰动集合中的每一偏航扰动，保留稳定的差异可见性见证单元，并结合候选特有体素证据形成合格有向证据边，合格有向证据边的有效分数为组合边分数。

8、步骤s5：以候选节点为节点、以合格有向证据边为边构造候选有向证据图；仅当一个候选节点具有覆盖全部保留竞争候选节点且达到支配裕量的合格出边、无合格入边并构成唯一严格支配节点时，输出认证通过状态并发布对应初始位姿；不满足前述条件时，输出拒绝发布状态并重新获取静态观测窗口。

9、其中，步骤s1中构造静态查询点云包括以下步骤：s1-1，对静态多帧三维激光数据中的原始三维激光点执行有效线束筛选、回波标签筛选、盲区筛选、重复点剔除和非有限值剔除；s1-2，依次施加激光雷达坐标系至传感器安装坐标系的第一预标定刚体变换以及传感器安装坐标系至机器人基座坐标系的第二预标定刚体变换；s1-3，在机器人经静止判定后累积多帧点云，并进行体素下采样，以使静态查询点云中的点具有统一的机器人基座坐标表达。

10、进一步，步骤s1仅在有效三维激光帧数量达到静态查询帧门控条件时构造静态查询点云，静态查询帧门控条件包括查询帧总数和最小有效帧数；累积多帧点云不以未经全局定位验证的运动里程计作为必要的全局运动补偿依据。

11、其中，步骤s2中获得经基础几何验证的候选位姿包括以下步骤：s2-1，利用包含环形键和扇区键的全局场景描述子从多楼层地图中检索地点候选；s2-2，根据地点候选关联位姿的竖直坐标进行高度聚类，并从不同高度簇中轮转保留地点候选；s2-3，对每一地点候选截取候选局部地图，执行粗尺度和细尺度点云刚体配准，依据配准质量、均方根误差、平移修正量、偏航修正量和体素占据一致性实施几何硬门控，只有通过几何硬门控的地点候选所关联的候选位姿才进入竞争候选对构造。

12、其中，步骤s2中形成竞争候选对包括以下步骤：s2-4，当两个候选位姿的竖直坐标差不小于预设跨高度分离阈值，且两者对应的候选局部地图的体素集合归一化重叠量不小于预设相似性阈值时，将两个候选位姿形成竞争候选对；s2-5，将基准偏航角、正向偏航角偏移量和负向偏航角偏移量确定为偏航扰动集合；针对偏航扰动集合中的每一偏航扰动，按照预设相邻方位单元容差在同一俯仰单元内建立相邻方位单元对应关系，俯仰单元不进行合并。

13、其中，步骤s3中形成反事实预测差异方向单元包括：s3-1，将候选局部地图在对应机器人基座坐标系的每一方向单元内，距离机器人基座原点最近且满足有效距离范围、有效回波标签和遮挡一致性条件的地图回波确定为预测首个有效回波；s3-2，将两个候选位姿在同一方向单元中的预测状态进行比较；两个候选位姿的预测状态不同时，将该方向单元确定为反事实预测差异方向单元；两个候选位姿均处于预测首个有效回波状态且预测首个有效回波距离之差达到预设反事实距离差阈值时，将该方向单元确定为反事实预测差异方向单元；s3-3，当静态查询点云中没有实际首个有效回波且至少一个候选位姿处于无预测回波状态时，不依据该方向单元形成支持证据和矛盾证据。

14、其中，步骤s4中形成支持证据、矛盾证据和未知证据包括：s4-1，比较静态查询点云中的实际首个有效回波与两个候选位姿的预测首个有效回波；当实际首个有效回波与候选位姿a预测首个有效回波的距离差不超过预设距离容差，且该距离差比实际首个有效回波与候选位姿b预测首个有效回波的距离差小出预设距离裕量时，形成候选位姿a相对于候选位姿b的支持证据；s4-2，当实际首个有效回波与候选位姿b预测首个有效回波的距离差不超过预设距离容差，且该距离差比实际首个有效回波与候选位姿a预测首个有效回波的距离差小出预设距离裕量时，形成候选位姿a相对于候选位姿b的矛盾证据；s4-3，在静态查询点云中不存在实际首个有效回波时形成未知证据，在两个候选位姿均处于无预测回波状态时形成未知证据，在实际首个有效回波无法在距离容差内匹配两个候选位姿的预测首个有效回波时形成未知证据，在实际首个有效回波同时接近两个候选位姿的预测首个有效回波时形成未知证据。

15、其中，步骤s4中复核差异可见性见证单元的稳定性包括：s4-4，采用由基准位姿、预设平移扰动和偏航扰动集合构成的确定性扰动模板，对每一反事实预测差异方向单元重复获得支持证据和矛盾证据；s4-5，当一个方向单元获得支持证据的次数达到扰动样本总数与预设最小稳定比例相乘后向上取整的数量，且支持证据次数大于矛盾证据次数时，将该方向单元确定为稳定支持见证单元，并以支持证据次数除以扰动样本总数得到稳定比例；当矛盾证据满足相同数量条件且矛盾证据次数大于支持证据次数时，将该方向单元确定为稳定矛盾见证单元，并以矛盾证据次数除以扰动样本总数得到稳定比例；s4-6，将通过实际首个有效回波观测和扰动稳定性判定的方向单元确定为差异可见性见证单元，未知证据保留在扰动样本总数中且不转换为反向证据。

16、其中，步骤s4中形成合格有向证据边包括：s4-7，当稳定支持数量和稳定矛盾数量之和大于零时，将差异可见性见证单元中的稳定支持数量减去稳定矛盾数量所得的差值除以稳定支持数量和稳定矛盾数量之和，得到方向证据分数；当稳定支持数量和稳定矛盾数量均为零时，将方向证据分数确定为零；并将差异可见性见证单元的数量确定为方向观测数量；s4-8，将静态查询点云落入候选位姿a对应的候选特有体素的数量确定为候选位姿a对应的候选特有体素支持数量，将静态查询点云落入候选位姿b对应的候选特有体素的数量确定为候选位姿b对应的候选特有体素矛盾数量；当两类体素数量之和大于零时，将候选位姿a对应的候选特有体素支持数量减去候选位姿b对应的候选特有体素矛盾数量所得的差值除以两类体素数量之和，得到体素证据分数；当两类体素数量均为零时，将体素证据分数确定为零；并将两类体素数量之和确定为体素观测数量；s4-9，按照预设的非负方向权重和非负体素权重，对方向证据分数和体素证据分数进行加权，得到组合边分数；仅当方向观测数量、体素观测数量、方向证据分数、体素证据分数及组合边分数同时达到对应门槛时，生成合格有向证据边。

17、其中，步骤s5中确定唯一严格支配节点包括：s5-1，确定候选节点a对每一个保留的竞争候选节点b均存在候选节点a至候选节点b的合格有向证据边，不存在候选节点b至候选节点a的合格有向证据边，且两条相反方向边的组合边分数之差不小于预设支配裕量，即：针对候选节点a和每一个保留的竞争候选节点b，分别确定候选节点a至候选节点b的组合边分数以及候选节点b至候选节点a的组合边分数；当候选节点a至候选节点b形成合格有向证据边、候选节点b至候选节点a未形成合格有向证据边，并且两个方向的组合边分数之差不小于预设支配裕量时，确定候选节点a严格支配候选节点b；s5-2，确定候选节点a的合格出边覆盖全部保留的竞争候选节点，全部必要竞争候选对均已建立比较关系，且满足前述条件的候选节点数量为一个；s5-3，当未满足任一前述条件时，输出包含候选节点、方向单元、支持证据、矛盾证据、稳定比例、合格出边、合格入边和拒绝原因的拒绝发布状态，并重新获取静态观测窗口。

18、本发明的第二个方面涉及多楼层环境机器人静态自动重定位初始位姿确定系统，包括以下功能模块：模块m1，用于获取静态观测窗口内的多帧激光雷达数据；模块m2，静态查询点云构造模块，用于对所述多帧激光雷达数据进行有效点筛选、坐标变换、多帧累积和体素化，输出机器人基座坐标系下的静态查询点云；模块m3，基础候选验证模块，用于从多楼层地图检索地点候选并执行局部几何验证，输出经基础几何验证的候选位姿；模块m4，竞争候选对构造模块，用于依据候选位姿的竖直坐标和候选局部地图的结构相似性形成竞争候选对，对两候选位姿命名。

19、所述系统还包括模块m5，反事实可见性签名模块，用于将候选局部地图变换至对应机器人基座坐标系，记录预测首个有效回波状态或无预测回波状态，并形成反事实预测差异方向单元；模块m6，稳定见证聚合模块，用于根据实际首个有效回波和偏航扰动形成支持证据、矛盾证据及差异可见性见证单元，结合候选特有体素证据生成合格有向证据边；模块m7，有向证据图判定模块，用于构造候选有向证据图，仅唯一严格支配节点满足无合格入边、全体出边覆盖和支配裕量条件时输出认证通过状态，否则拒绝发布；模块m8，初始位姿输出与反馈模块，用于认证通过时发布初始位姿，并根据确认消息、定位成功反馈和超时结果控制重发布、候选清除及重新获取观测窗口。

20、进一步，模块m3包括场景描述子检索子模块、高度簇组织子模块、候选位姿变体子模块、候选局部地图截取子模块、粗到细点云配准子模块和几何硬门控子模块；模块m5包括候选局部地图逆刚体变换子模块、方位角—俯仰角分桶子模块、相邻方位单元容差子模块、预测首个有效回波索引子模块和差异见证记录子模块。差异见证记录至少包括方向单元索引、两个候选位姿的预测距离及预测状态、实际首个有效回波距离、预测距离差、名义观测状态、扰动聚合状态、支持票数、矛盾票数、扰动样本数、候选位姿a对应的候选特有体素支持数量和候选位姿b对应的候选特有体素矛盾数量。

21、进一步，模块m7包括合格出边统计子模块、合格入边统计子模块、支配裕量计算子模块、唯一性判定子模块和拒绝原因记录子模块；合格入边统计子模块将达到方向、体素、稳定性和组合分数门槛的候选节点b至候选节点a的合格有向证据边计为合格入边，存在合格入边的候选节点不能被确定为唯一严格支配节点。模块m7输出结构化判定结果，结构化判定结果至少包含候选位姿的竖直坐标、竞争候选对、反事实预测差异方向单元、差异见证记录、差异可见性见证单元、支持票数、矛盾票数、候选特有体素支持数量、候选特有体素矛盾数量、扰动样本数、稳定比例、方向证据分数、体素证据分数、组合边分数、合格出边、合格入边、支配裕量、认证通过状态、拒绝发布状态和最终是否发布初始位姿的标志。

22、本发明的工作原理是：从多楼层地图中得到经基础几何验证的候选位姿后，不以单个候选位姿的配准质量决定初始位姿，而是将竖直坐标分离且局部结构相似的两个候选位姿组成竞争候选对。候选局部地图在各自机器人基座坐标系下形成预测首个有效回波状态和无预测回波状态，静态查询点云中的实际首个有效回波对两个候选位姿形成支持证据、矛盾证据和未知证据。确定性扰动模板保留稳定的差异可见性见证单元，候选有向证据图以合格有向证据边组织全部保留竞争候选节点，由唯一严格支配节点决定认证通过状态。

23、本发明的创新点是：

24、（1）针对跨高度且局部结构相似的竞争候选对，在统一方位角—俯仰角方向单元中比较预测状态和预测首个有效回波距离，确定反事实预测差异方向单元；

25、（2）将实际首个有效回波分类为支持证据、矛盾证据和未知证据，未知证据保留在扰动样本总数中且不转换为反向证据；

26、（3）通过确定性扰动模板、候选特有体素证据和候选有向证据图确定唯一严格支配节点，在证据不足时输出拒绝发布状态。

27、本发明的核心创新点是“跨高度候选对的同方向双假设首回波见证机制”与“基于有向证据图的唯一严格支配认证发布机制”，并以“查询支持/矛盾/未知三态”和“扰动稳定见证”作为关键支撑。其技术对象不是单个候选的绝对分数，而是候选之间的可区分性。

28、首先，本发明的跨高度候选对的同方向双假设首回波见证机制针对现有技术的痛点：在多楼层相似结构中，基于环形与扇区划分的全局场景描述子、点云刚体配准、均方根误差、配准质量和体素占据一致性等方法能够形成和验证候选，但多个单候选绝对分数可能同时看似合理，普通的最高分候选排序难以说明“候选位姿a相对于候选位姿b具有可区分优势”。

29、本发明从已经通过基础几何验证的候选中构造跨高度竞争候选对，将两个候选局部地图分别变换至对应机器人基座坐标系，并在同一方位角—俯仰角方向单元分别确定候选位姿a和候选位姿b的预测首个有效回波状态或者无预测回波状态；只保留两种反事实预测存在差异且查询实际覆盖的方向单元，由静态查询点云中的实际首个有效回波对两个候选位姿形成有向的支持证据或者矛盾证据。

30、本发明把多楼层重定位从“单候选打分”转化为“候选对反事实可区分性验证”，由一对跨高度候选共同定义方向见证，再用真实查询观测验证两种假设之间的差异。

31、其次，本发明的查询三态与扰动稳定的反事实见证机制针对现有技术的痛点：一般的可见性、自由空间或匹配分数容易把未观测、无有效回波或局部位姿轻微变化造成的不稳定方向混入正反证，导致证据含义不清，尤其不利于相似楼层中的保守判定。

32、本发明在静态查询点云侧明确记录支持证据、矛盾证据和未知证据三种证据状态；无有效回波、无明确距离匹配的方向保留为 unknown，不进入支持或矛盾。对候选对的差异方向进一步实施小范围平移/偏航扰动稳定投票；偏航扰动时仅允许预设相邻方位格容差，俯仰格不合并，在一种具体实施方式中采用7个确定性扰动样本，未观测样本计入稳定性分母。

33、本发明不是只判断“是否看到自由空间”，而是把有效查询覆盖、三态证据与局部扰动稳定性绑定，使保留下来的见证方向具有明确的可观测性和稳定语义。

34、其三，本发明的方向/体素双门控的有向证据图与唯一严格支配发布，针对现有技术的痛点：一般候选排序、图模型、歧义估计和不确定性拒绝虽然已知，但若仍以单个置信度或最高分候选排序 为最终发布依据，在相向合格有向证据边同时存在、严格支配节点不唯一、必要比较关系缺失或者单一高度候选未经认证时仍可能产生不安全发布。

35、本发明将方向证据分数与体素证据分数进行加权组合；在一种具体实施方式中，方向权重和体素权重均设置为0.5，并同时实施数量、分数和稳定性门控；以候选为节点、合格有向边为边构造证据图。只有候选形成唯一严格支配、无合格入边，并对每一个保留跨高度代表具有合格出边且满足支配裕量时才认证发布；任一必要比较关系缺失或证据不足时输出明确拒绝状态。

36、本发明把最终决策从“最高分候选”改为“候选间有向支配关系是否完整成立”，并将不可观测、相向合格有向证据边同时存在、严格支配节点不唯一和证据不足转化为结构化拒绝发布状态。

37、本发明具有下述优点：

38、（1）候选位姿的发布依据由候选间可比较的实际可见性证据形成，能够适应多楼层重复结构场景；

39、（2）未知证据不被转换为支持证据，不被转换为矛盾证据，避免无实际首个有效回波的方向对候选位姿形成非真实倾向；

40、（3）认证通过状态要求合格出边覆盖全部保留竞争候选节点、无合格入边、满足支配裕量和唯一性，拒绝发布状态能够阻止证据不足时发布初始位姿。