一种复杂场景下的智能机器人的主动伪装巡逻导航方法技术

本发明专利技术公开一种复杂场景下的智能机器人的主动伪装巡逻导航方法。该方法包括：(1)伪装巡逻语义地图构建。(2)RTK和IMU融合定位：通过ESKF对RTK和IMU传感器数据融合以估计机器人在复杂场景中的位姿。(3)DPPC运动控制：针对差分机器人运动模型，设计了一种基于几何模型的伪装巡逻运动控制方法。通过本发明专利技术主动伪装巡逻导航方法，机器人可以根据先验地图语义信息判定决策伪装巡逻当行方式，根据场景特征执行不同方法，在确保机器人不被不法分子破坏的同时尽量提升巡逻效率，最终实现快速性和安全性兼顾的巡逻。兼顾的巡逻。兼顾的巡逻。

【技术实现步骤摘要】
一种复杂场景下的智能机器人的主动伪装巡逻导航方法


[0001]本专利技术属于机器人规划与导航
具体涉及一种复杂场景下的智能机器人的主动伪装巡逻导航方法。

技术介绍

[0002]为解决安保巡逻机器人普遍缺乏主动自我保护的意识，极易在执行任务的过程中被追踪目标发现甚至被破坏的问题，中国专利CN113536934A提出了一种巡逻机器人主动隐藏方法。该方法可以降低行为的暴露性，但存在巡逻效率较低的问题，当巡逻范围较大时巡逻耗费时间较长。
[0003]由经验知，不法分子极少在开放环境或者人流密集的场所实施破坏行为，该场景中不法分子对机器人的安全威胁性较低。因此，机器人可以在该场景下执行常规巡逻方法以确保巡逻任务完成的高效性。然而，在人员较少空间相对狭隘的环境中，不法分子可以从容地将机器人破坏，该环境下执行伪装巡逻隐藏方法可以有效地降低机器人被发现甚至被破坏的概率，从而确保自身的安全。在多元化的大范围室外巡逻区域中，若机器人可以根据不同的场景特点执行不同的巡逻行为，则可以兼顾巡逻效率和安全性，更加高效安全的完成巡逻任务。

技术实现思路

[0004]针对现有技术的不足，本专利技术提出了一种复杂场景下的智能机器人的主动伪装巡逻导航方法，基于开放街道地图(Open Street Map,OSM)构建伪装巡逻语义地图，用于提供巡逻路径和场景属性等先验信息。机器人搭载有RTK和IMU等传感器，通过状态误差卡尔曼滤波(Error state Kalman Filter,ESKF)算法实现在地图坐标系中的位姿估计，并根据自身位姿和地图先验信息判定场景类型以决策巡逻方式。若判定为伪装巡逻场景，则执行主动伪装巡逻(具体参见中国专利CN113536934A)，否则执行常规巡逻方法，常规巡逻方法的运动控制通过差分纯跟踪控制(Differential Pure Pursuit Control,DPPC)方法实现。针对多元化大范围室外巡逻场景，上述方法可以更加安全高效地实现伪装巡逻任务。
[0005]第一方面，提供一种复杂场景下的智能机器人的主动伪装巡逻导航方法，具体包括以下步骤：
[0006]步骤S1、构建基于开放街道地图OSM的伪装巡逻语义地图；
[0007]步骤S2、基于状态误差卡尔曼滤波算法对巡逻机器人进行位姿估计；
[0008]步骤S3、巡逻路径生成及巡逻方式决策
[0009]结合步骤S2得到的巡逻机器人的估计位姿，获得当前巡逻机器人在伪装巡逻语义地图中的最近路径点N
i
；
[0010]将伪装巡逻语义地图中的所有路径点解析为一个无向图，采用Dijkstra算法规划得到一条巡逻路径；
[0011]对巡逻路径中各路径点获取其存储的道路语义信息，判断当前路径点的语义信息
是否为伪装隐藏道路，若是则执行主动伪装巡逻，若为其他语义信息则执行步骤S4，到达下一个路径点后重复步骤；
[0012]步骤S4、巡逻运动控制
[0013]结合步骤S2得到的巡逻机器人的估计位姿，得出巡逻机器人的运动角速度；根据实时更新的巡逻机器人的运动角速度实现巡逻路径跟随功能。
[0014]第二方面，提供一种机器人主动伪装巡逻导航系统，包括：
[0015]伪装巡逻语义地图构建模块，用于根据开放街道地图OSM构建巡逻语义地图；
[0016]巡逻机器人位姿估计模块，用于基于状态误差卡尔曼滤波算法对巡逻机器人进行位姿估计，并根据巡逻机器人的估计位姿获取在伪装巡逻语义地图中的最近路径点；
[0017]巡逻路径生成模块，用于根据最近路径点和巡逻目标点在伪装巡逻语义地图中获得巡逻路径；
[0018]巡逻方式决策模块，用于根据巡逻路径中各路径点存储的道路语义信息，决策巡逻方式；
[0019]巡逻运动控制模块，实现根据实时更新的巡逻机器人的运动角速度完成巡逻路径跟随功能。
[0020]第三方面，提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行所述的方法。
[0021]第四方面，提供一种机器人，其特征在于，包括处理器和存储器，所述存储器存储有能够被所述处理器执行的机器可执行指令，所述处理器执行所述机器可执行指令以实现所述的方法。
[0022]本专利技术具有以下有益效果：
[0023]1、本专利技术基于OSM构建了大范围伪装巡逻语义地图，可以为导航提供路径先验信息并基于语义信息决策伪装巡逻策略。
[0024]2、本专利技术采用ESKF融合了RTK和IMU传感器数据，该算法可以有效降低非线性系统线性化的误差。
[0025]3、本专利技术提出了DPPC路径跟踪控制方法，可以更好适用于差分模型机器人的运动控制。
[0026]4、本专利技术综合考虑了复杂场景下机器人巡逻策略，本专利技术指出的常规巡逻导航和伪装巡逻导航方法融合使用可以同时兼顾巡逻的高效性和机器人的安全性。
附图说明
[0027]图1为本专利技术方法的流程图；
[0028]图2为OSM地图可视化平面图；
[0029]图3为局部地图可视化平面图；
[0030]图4为路径筛选结果图；
[0031]图5(a)
‑
(b)分别为OSM地图和对应伪装巡逻语义地图；
[0032]图6为DPPC运动控制模型。
具体实施方式
[0033]以下结合附图对本专利技术作进一步的解释说明；
[0034]本专利技术实施采用搭载有Realsense D435深度相机、RTK和IMU的PowerBot机器人作为实验平台，启动计算机系统，运行相关节点，巡逻机器人开始执行伪装巡逻巡逻任务，一种复杂场景下的智能机器人的主动伪装巡逻导航方法，图1具体步骤如下：
[0035]步骤S1、构建基于开放街道地图OSM的伪装巡逻语义地图
[0036]OSM平台地图数据中路径点以经纬度进行位置描述，经纬度是基于球面坐标系的位置表示，而自主移动平台在进行自身位置描述时一般构基于面直角坐标系描述自身的位姿信息，因此需要先将OSM地图数据中的经纬度信息转换为平面坐标。
[0037]1‑
1由于开放街道地图OSM道路数据中路径点以经纬度进行位置描述，所述经纬度是基于球面坐标系的位置表示，故将开放街道地图OSM的经纬度信息转换为平面坐标；具体是：
[0038]计算开放街道地图OSM中采样点(lon2,lat2)与基准点(lon1,lat1)的平面距离：
[0039][0040]其中，R
earth
为地球半径；
[0041]依次计算全部采样点与基准点的距离，进而得到全部采样点相对于基准点的平面坐标位置，实现球面坐标系地图转换为平面坐标系地图；最终转换为平面坐标的可视化图片如图2所示。
[0042]1‑
2由于观察局部可视化(图3)可以看出，开放街道地图OSM道路数据中路径点分布不均匀，部分道路中路径点的分布过于稀疏，因此对本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种复杂场景下的智能机器人的主动伪装巡逻导航方法，其特征在于该方法包括以下步骤：步骤S1、构建基于开放街道地图OSM的伪装巡逻语义地图；步骤S2、基于状态误差卡尔曼滤波算法对巡逻机器人进行位姿估计，并获取最近路径点；2
‑
1估计状态量初始化假设{B}系、IMU坐标系和RTK坐标系三者重合，巡逻机器人的估计状态量为：X＝[P,V,R,ba,bω]
T
ꢀꢀꢀꢀ
(3)其中P∈R3[m]、V∈R3[m/s]和R∈R3[rad]分别表示巡逻机器人本体坐标系在全局坐标坐标系下的位置、速度和姿态；ba∈R3[m/s2]和bω∈R3[rad/s2]分别表示IMU加速度和角速度的偏置；上标T表示转置；误差状态量为：δX＝[δP,δV,δR,δba,δbω]
T
ꢀꢀꢀꢀ
(4)以巡逻机器人运动初始时刻的位置为全局坐标系的原点，故P初始化为(0,0,0)；初始时刻速度为零，故V初始化为(0,0,0)；R主要依据重力g在不同坐标系下量值的统一性获取，假设世界坐标系的z轴方向与重力方向平行，IMU加速度的测量值视为g在车体坐标系下的投影，故通过两组IMU加速度反推出初始时刻巡逻机器人的姿态R；如IMU不存在零偏，IMU静止时角速度的测量值为0，因此角速度的偏置取为：其中，ω
i
为i时刻IMU静止时的角速度测量值；n表示角速度测量值总帧数；加速度计的偏置为静止加速度测量值a
i
和重力投影的差值：其中，a
i
为i时刻IMU静止时的加速度测量值；至此，所有估计状态量初始化完成；2
‑
2基于IMU数据的状态预测；首先根据IMU数据计算加速度和角速度的中值：其中R
t
的更新通过旋转矩阵的相乘实现：R
t
＝R
t
‑1R{Δφ}
ꢀꢀꢀꢀ
(8)其中，R{}为通过罗德里格斯公式转换得到的旋转矩阵；Δφ为状态周期内的角增量，Δφ＝(ω
mid
‑
bω)Δt；Δt表示状态周期时间间隔；位置和速度更新为：
进而，误差状态更新为：其中，[]
×
为旋转量更新操作，V
noise
、R
noise
、ba
noise
、bw
noise
分别为随机高斯白噪声；将上式(10)进行整合为紧凑形式：其中,表示t时刻的先验误差状态量，为上一时刻的后验误差状态量，F
noise
为噪声转移矩阵，F
x
为误差状态转移矩阵，其值分别为其中，I表示单位矩阵；t时刻的先验预测协方差更新为：其中，为上一时刻的后验协方差，Q
n
为高斯白噪声矩阵，其值为：其中，σ
a
、σ
ω
分别表示加速度与角速度测量噪声的方差σ
ba
、σ
bω
分别表示分别为加速度偏置随机游走噪声方差与角速度偏置随机游走噪声方差；2
‑
3基于RTK数据的状态更新；为了消除长时间积分造成的误差，使用RTK(Real Time Kinematic)数据对先验估计信息进行观测更新；RTK数据的观测模型设为：Y＝HX
t
+v
ꢀꢀꢀꢀ
(15)其中，H为观测状态转移矩阵，X
t
为当前真实状态，v为传感器的观测噪声，其服从高斯分布v～Ν{0,V}；由于仅用RTK观测位置信息，故H为：
H＝[I 0 0 0 0]
ꢀꢀꢀꢀ
(16)进而求得卡尔曼增益：根据卡尔曼增益对后验状态误差进行更新：最后对后验状态协方差进行更新：根据状态误差对标称状态进行更新，最后得到巡逻机器人的估计位姿；2
‑
4获取巡逻机器人的估计位姿在伪装巡逻语义地图中的最近路径点：结合步骤S2得到的巡逻机器人的估计位姿，根据公式(20)获得当前巡逻机器人在伪装巡逻语义地图中的最近路径点N
i
：N
i
←
min(d)
ꢀꢀꢀꢀ
(20)其中，d＝{d
i
,i∈N*}，di为伪装巡逻语义地图中路径点与巡逻机器人当前姿态的距离；N*表示正整数；步骤S3、巡逻路径生成及巡逻方式决策3
‑
1生成巡逻路径将伪装巡逻语义地图中的所有路径点解析为一个无向图G＝(N,E)，其中N为路径点，E为相邻路径点间的距离；基于无向图G，采用Dijkstra算法规划得到一条巡逻路径way＝(node
s
,node
e
)，其中node
s
到node
e
表...

【专利技术属性】
技术研发人员：王树伟，张波涛，王若尧，吕强，
申请(专利权)人：杭州电子科技大学，
类型：发明
国别省市：