[发明专利]未知环境下移动机器人三维地图构建方法

摘要

一种未知环境下移动机器人三维地图构建方法。针对室内未知三维环境的地图构建问题，提出一种将信息增益引导局部探索策略和全局边界探索策略相结合的多探索策略方法。首先构建移动机器人自主探索和三维地图构建系统框架，包括地图构建模块和探索决策规划控制模块。其次，基于香农信息论建立信息增益引导决策模型，并设计了一种应用于多步探索动作评价的信息增益目标函数计算。最后，利用离线的局部运动轨迹构造局部探索策略，并结合信息增益引导全局边界探索策略实现了一种多策略探索方法，两种策略在探索过程中根据三维地图的实时构建情况动态切换。实验结果表明本发明在快速构建三维地图的同时保证了地图构建的完整性。

主权项

1.一种未知环境下移动机器人三维地图构建方法，其特征在于该方法包括：第1，自主探索和三维地图构建的系统框架系统主要包含基于八叉树数据结构的概率占据体元地图(Octomap)构建模块和基于多探索策略的决策规划控制模块；为了快速构建三维地图，将Octomap地图构建模块设计为独立线程，并与多探索策略决策规划控制模块以多线程的方式运行；为适应移动机器人实时运动控制过程中地图信息的动态更新变化，多探索策略决策规划控制模块采用滚动时域控制方法执行；在Octomap地图构建模块中，相机坐标系相对于机器人坐标系的坐标变换已知，机器人坐标系相对于世界坐标系的坐标变换由机器人的实时位姿x:(x,y,θ)得到；在实时运动控制过程中，相机坐标系下的每一帧原始RGBD点云数据通过预处理得到点云观测P_C，并通过坐标转换获得世界坐标系下的点云观测将P_W插入到全局地图中，并利用二值贝叶斯滤波得到概率更新后的地图状态m；第2，应用于多步探索动作评价的信息增益目标函数计算Octomap将三维环境表示为三维体元的集合体元ni的占据概率p(ni)∈[0,1]，根据体元间的独立性假设，的地图状态m服从概率分布p(m)＝Π_ip(n_i)；假设t时刻SLAM获得的历史轨迹x₁:_t已知，三维点云历史观测表示为z₁:_t，t+1时刻移动机器人在位姿x_t+1处对三维环境进行观测得到未来观测z_t+1，那么由zt+1产生的概率占据体元地图信息增益就是z_t+1对地图状态m进行概率更新之后地图状态不确定度的减少量；采用香农信息熵衡量地图状态m的不确定度，表示成如下形式：H(m|x1:t,z1:t)＝‑∑i[pocclog2pocc+(1‑pocc)log2(1‑pocc)]其中，pocc＝p(ni|x1:t,z1:t)表示体元ni的先验概率；经过未来观测zt+1概率更新，地图状态m产生的信息增益表示为如下形式：IG(m；zt+1|xt+1,x1:t,z1:t)＝H(m|x1:t,z1:t)‑H(m；zt+1|xt+1,x1:t,z1:t)其中，H(m|x_1:t,z_1:t)表示地图状态m的先验信息熵，H(m；z_t+1|x_t+1,x_1:t,z_1:t)表示地图状态m的后验信息熵；进一步地，定义探索动作为从t时刻开始的T个时间步范围内移动机器所历经的位姿序列位姿序列对应得到未来观测序列那么应用于T个时间步探索动作评价的信息增益引导决策模型表示为如下形式：其中为所有探索动作构成的候选探索动作集合；决策的目标是从候选探索动作集合中寻找信息增益最大的最优位姿序列为实现应用于多步探索动作评价的信息增益目标函数计算，将位姿x_t+1处的未来观测z_t+1表示成N束光束向量的集合其中光束向量由针孔相机模型得到；利用3D bresenham光线追踪算法将观测z_t+1转化为三维体元及其观测结果的集合具体而言，当光束向量表示的光束经过体元n_i时，体元的占据状态p(n_i)被用于判断光束穿越该体元所在的无障碍空间位置或者击中该体元所在的被障碍物占据的空间位置，分别用o_i＝0和o_i＝1来表示；进一步地，在T步时间范围内，利用光线追踪算法将未来观测序列每个时间步的观测都分别转化为三维体元及其观测结果的集合，得到由三维体元及其观测结果集合构成的集合序列为避免对三维环境所有体元的重复遍历计算，同时考虑到探索动作对应的观测序列z_τ中，不同时间步位姿所在的传感器观测视野范围存在视野重叠，即k,i,j∈[t+1,t+T]且i≠j，部分体元会被重复观测并更新占据概率，因此在地图推演的过程中动态地维护和更新一张“缓存地图”，使用M_c表示，缓存每个时间步观测到体元的最新占据概率；应用于多步探索动作评价的信息增益目标函数计算步骤如下：第2.1，输入当前t时刻的机器人位姿x_t和Octomap地图M_t，并初始化缓存地图多步探索动作信息增益值IG＝0；第2.2，对探索动作时间步k的位姿x_k及观测z_k，利用光线追踪算法得到集合对中的每个三维体元及其观测结果(n,o)，获取体元n最新的占据概率p(n|x_1:k‑1,z_1:k‑1)，并根据观测结果o进行log‑odd概率更新，得到后验概率p(n；z_k|x_k,x_1:k‑1,z_1:k‑1)，从而体元n产生的信息增益表达为如下形式：ig(n)＝H(p(n|x1:k‑1,z1:k‑1))‑H(p(n；zk|xk,x1:k‑1,z1:k‑1))对多步探索动作产生的概率占据体元地图总的信息增益执行累加运算IG+＝ig(n)；最后，体元n及经过log‑odd概率更新后的后验概率构成一对(n,p(n；zk|xk,x1:k‑1,z1:k‑1))并保存到缓存地图Mc中以便后续的查询和访问；第2.3，重复以上步骤直至时间步k＝T；第3，信息增益引导局部探索策略和全局边界探索策略相结合的多策略探索方法第3.1，信息增益引导局部探索策略在探索过程中，离线加载的局部运动轨迹根据机器人的实时位姿x:(x,y,θ)转化到世界坐标系下，并通过碰撞检测规划得到所有局部探索动作构成的候选局部探索动作集合离线运动轨迹通过已有的基于模型预测的数值优化方法生成；根据第2步的信息增益引导决策模型从候选探索动作集合中得到最优局部探索动作并交由底层控制器进行轨迹跟踪；底层控制器为现有的任意一种控制方法，且采用滚动时域控制策略，当达到重规划设定时间T_r时，进入到下一次决策循环中；第3.2，信息增益引导全局边界探索策略第3.2.1，候选全局边界区域观测视点生成边界体元定义为：位于RGBD传感器安装高度hc所在平面的二维体元地图上且八邻域内存在未知体元的自由状态体元；全局边界区域通过区域生长法得到，边界点数量小于阈值的区域视为无效边界区域被剔除，所有L个有效边界区域表示为{F1,...Fj,...FL}，对应的边界区域重心C为：考虑到三维空间下边界区域往往存在于机器人不可到达的空间位置，因此利用双采样法在边界区域的邻域范围内采样得到对边界区域可见观测视点，对边界区域可见性定义为：1)边界区域重心在观测视点的视野范围内；2)边界区域到观测视点的入射光束通畅；此外，移动机器人在采样观测视点处与障碍物无碰撞；满足如上可见性和碰撞检测要求的采样观测视点视为有效，所有L个边界区域{F1,...Fj,...FL}对应采样到L个有效观测视点{q1,...qj,...qL}；为确定L个边界区域各自的不确定度，利用信息增益指标评价观测视点观测各自边界区域所能产生的效益，计算公式如下：Ig(qj)＝H(m)‑H(m；zj|qj)按照信息增益大小对视点观测优先顺序进行降序排序，并筛选前l组信息增益最大的观测视点构成候选全局边界区域观测视点集合Q＝{q1,...ql}，其中l≤L；第3.2.2，最优全局探索动作选取对观测视点集合Q＝{q₁,...q_l}中的每个观测视点，利用已有的图搜索算法生成从移动机器人当前位姿(x,y,θ)到目标观测视点全局探索动作所有全局探索动作构成候选全局探索动作集合其中表示全局探索动作的位姿节点序列对应了未来观测序列在选择全局探索动作的位姿节点时，机器人每行进距离s或者当机器人方向角旋转超过δ时定义一个时间步；集合中不同全局探索动作的路径长度往往存在显著差异，导致其时间步范围也存在显著差异，因此在第2步信息增益目标函数设计的基础上，利用全局探索动作的每个位姿节点所能产生的平均信息增益作为评价函数，从而全局边界探索策略采用如下的决策模型：决策目标是从候选全局探索动作集合中选取平均信息增益最大的最优位姿序列第3.2.3，触发全局边界探索策略的条件如下：条件一：经过碰撞检测，候选局部探索动作集合为空集；条件二：在候选局部探索动作集合中，最优局部探索动作对概率占据体元地图产生的信息增益小于设定阈值Th₁。