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

权利要求书

1.一种未知环境下移动机器人三维地图构建方法，其特征在于该方法包括：

第1，自主探索和三维地图构建的系统框架

系统主要包含基于八叉树数据结构的概率占据体元地图构建模块和基于多探索策略的决策规划控制模块，使用基于八叉树数据结构的概率占据体元地图为Octomap；为了快速构建三维地图，将Octomap地图构建模块设计为独立线程，并与多探索策略决策规划控制模块以多线程的方式运行；为适应移动机器人实时运动控制过程中地图信息的动态更新变化，多探索策略决策规划控制模块采用滚动时域控制方法执行；

在Octomap地图构建模块中，相机坐标系F_C相对于机器人坐标系F_R的坐标变换已知，机器人坐标系F_R相对于世界坐标系F_W的坐标变换由机器人的实时位姿x:(x,y,θ)得到，其中(x，y)表示机器人在世界坐标系下的实时位置，θ表示机器人的朝向角；在实时运动控制过程中，相机坐标系F_C下的每一帧原始RGBD点云数据通过预处理得到点云观测P_C，并通过坐标转换获得世界坐标系F_W下的点云观测将P_W插入到全局地图中，并利用二值贝叶斯滤波得到概率更新后的地图状态m；

第2，应用于多步探索动作评价的信息增益目标函数计算

Octomap将三维环境表示为三维体元的集合M＝{n₁...,n_i...,n_N}，体元n_i的占据概率p(n_i)∈[0,1]，根据体元间的独立性假设，M的地图状态m服从概率分布p(m)＝Π_ip(n_i)；假设t时刻SLAM获得的历史轨迹x_1:t已知，三维点云历史观测表示为z_1:t，t+1时刻移动机器人在位姿x_t+1处对三维环境M进行观测得到未来观测z_t+1，那么由z_t+1产生的概率占据体元地图信息增益就是z_t+1对地图状态m进行概率更新之后地图状态不确定度的减少量；采用香农信息熵衡量地图状态m的不确定度，表示成如下形式：

H(m|x_1:t,z_1:t)＝-∑_i[p_occlog₂p_occ+(1-p_occ)log₂(1-p_occ)]

其中，p_occ＝p(n_i|x_1:t,z_1:t)表示体元n_i的先验概率；经过未来观测z_t+1概率更新，地图状态m产生的信息增益表示为如下形式：

IG(m；z_t+1|x_t+1,x_1:t,z_1:t)＝H(m|x_1:t,z_1:t)-H(m；z_t+1|x_t+1,x_1:t,z_1: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个时间步范围内移动机器所历经的位姿序列x_T＝[x_t+1,...,x_t+T]，位姿序列x_T对应得到未来观测序列z_T＝[z_t+1,...,z_t+T]，那么应用于T个时间步探索动作评价的信息增益引导决策模型表示为如下形式：

其中X为所有探索动作构成的候选探索动作集合；决策的目标是从候选探索动作集合中寻找信息增益最大的最优位姿序列

为实现应用于多步探索动作评价的信息增益目标函数计算，将位姿x_t+1处的未来观测z_t+1表示成N束光束向量的集合其中光束向量由针孔相机模型得到；利用3D bresenham光线追踪算法将观测z_t+1转化为三维体元及其观测结果的集合V_t+1＝{(n₁,o₁),...,(n_K,o_K)}；具体而言，当光束向量表示的光束经过体元n_i时，体元的占据状态p(n_i)被用于判断光束穿越该体元所在的无障碍空间位置或者击中该体元所在的被障碍物占据的空间位置，分别用o_i＝0和o_i＝1来表示；进一步地，在T步时间范围内，利用光线追踪算法将未来观测序列z_T＝[z_t+1,...,z_t+T]每个时间步的观测都分别转化为三维体元及其观测结果的集合，得到由三维体元及其观测结果集合构成的集合序列V_τ＝[V_t+1,...,V_t+T]；为避免对三维环境M所有体元的重复遍历计算，同时考虑到探索动作x_T对应的观测序列z_τ中，不同时间步位姿所在的传感器观测视野范围存在视野重叠，即两个集合的交集不为空集且i≠ii，部分体元会被重复观测并更新占据概率，因此在地图推演的过程中动态地维护和更新一张“缓存地图”，使用M_c表示，缓存每个时间步观测到体元的最新占据概率；

应用于多步探索动作评价的信息增益目标函数计算步骤如下：

第2.1，输入当前t时刻的机器人位姿x_t和Octomap地图M_t，并初始化缓存地图多步探索动作信息增益值IG＝0；

第2.2，对探索动作时间步k的位姿x_k及观测z_k，利用光线追踪算法得到集合V_k；对V_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|x_1:k-1,z_1:k-1))-H(p(n；z_k|x_k,x_1:k-1,z_1:k-1))

对多步探索动作产生的概率占据体元地图总的信息增益执行累加运算IG+＝ig(n)；最后，体元n及经过log-odd概率更新后的后验概率构成一对(n,p(n；z_k|x_k,x_1:k-1,z_1:k-1))并保存到缓存地图M_c中以便后续的查询和访问；

第2.3，重复以上步骤直至时间步k＝T；

第3，信息增益引导局部探索策略和全局边界探索策略相结合的多策略探索方法

第3.1，信息增益引导局部探索策略

在探索过程中，离线加载的局部运动轨迹根据机器人的实时位姿x:(x,y,θ)转化到世界坐标系F_W下，并通过碰撞检测规划得到所有局部探索动作构成的候选局部探索动作集合X_local；

离线运动轨迹通过已有的基于模型预测的数值优化方法生成；根据第2步的信息增益引导决策模型从候选探索动作集合X_local中得到最优局部探索动作并交由底层控制器进行轨迹跟踪；底层控制器采用滚动时域控制策略，当达到重规划设定时间T_r时，进入到下一次决策循环中；

第3.2，信息增益引导全局边界探索策略

第3.2.1，候选全局边界区域观测视点生成

边界体元定义为：位于RGBD传感器安装高度h_c所在平面的二维体元地图上且八邻域内存在未知体元的自由状态体元；全局边界区域通过区域生长法得到，边界点数量小于阈值的区域视为无效边界区域被剔除，所有L个有效边界区域表示为{F₁,...F_j,...F_L}，对应的边界区域重心C为：

利用双采样法在边界区域的邻域范围内采样得到对边界区域可见观测视点，对边界区域可见性定义为：1)边界区域重心在观测视点的视野范围内；2)边界区域到观测视点的入射光束通畅；此外，移动机器人在采样观测视点处与障碍物无碰撞；满足如上可见性和碰撞检测要求的采样观测视点视为有效，所有L个边界区域{F₁,...F_j,...F_L}对应采样到L个有效观测视点{q₁,...q_j,...q_L}；为确定L个边界区域各自的不确定度，利用信息增益指标评价观测视点观测各自边界区域所能产生的效益，计算公式如下：

Ig(q_j)＝H(m)-H(m；z_j|q_j)

按照信息增益大小对视点观测优先顺序进行降序排序，并筛选前l组信息增益最大的观测视点构成候选全局边界区域观测视点集合Q＝{q₁,...q_l}，其中l≤L；

第3.2.2，最优全局探索动作选取

对观测视点集合Q＝{q₁,...q_l}中的每个观测视点，利用已有的图搜索算法生成从移动机器人当前位姿(x,y,θ)到目标观测视点全局探索动作a_T，所有全局探索动作a_T构成候选全局探索动作集合X_global，其中a_T表示全局探索动作的位姿节点序列a_T＝[x_t+1,....,x_t+T]，对应了未来观测序列z_T＝[z_t+1,....,z_t+T]；在选择全局探索动作的位姿节点时，机器人每行进距离s或者当机器人方向角旋转超过δ时定义一个时间步；在第2步信息增益目标函数设计的基础上，利用全局探索动作的每个位姿节点所能产生的平均信息增益作为评价函数，从而全局边界探索策略采用如下的决策模型：

决策目标是从候选全局探索动作集合X_global中选取平均信息增益最大的最优位姿序列

第3.2.3，触发全局边界探索策略的条件如下：

条件一：经过碰撞检测，候选局部探索动作集合X_local为空集；

条件二：在候选局部探索动作集合X_local中，最优局部探索动作对概率占据体元地图产生的信息增益小于设定阈值Th₁。

2.根据权利要求1所述的未知环境下移动机器人三维地图构建方法，其特征在于，所述第2.2步，在工程实现时采用无序关联容器数据结构存储时间步k的观测z_k的光线追踪结果V_k，V_k中的每个三维体元及其观测结果(n,o)用键-值对(key,value)来表示；同样地，缓存地图M_c也使用无序关联容器存储，M_c中的每个三维体元及其占据概率(n,p(n))用键-值对(key,value)来表示。