·664· 智能系统学报 第14卷 4)标记识别：在这一点上，有必要确定获得 p0S:= ylocal -R'.T 的标记候选哪些属于字典，哪些只是环境的一部 (3) 分。提取标记的候选码之后，就可获得4个不同 根据式(2)可以计算出摄像头当前欧拉角， 的标识符。为了加快这个过程，字典元素使用平 式(3)可以计算出摄像头当前相对某个二维码的 衡二叉树排序。为此，标记由通过连接其所有位 局部位置，然后根据该二维码的全局坐标计算出 获得的整数值表示。 摄像头当前的全局坐标位置。进一步文中基于扩 1.2二维码阵列的设计 展卡尔曼滤波器对二维码位置、光流传感器速度测 对一幅图像中多个二维码进行识别可以计算 量值和机载MU数据进行融合得到最优位姿估计。 出摄像头相对每个二维码的位置和姿态，未被识 别的标志不参与位置解算。同时摄像头能在任意 2改进D*路径规划算法 方向识别出唯一码号，通过在室内合理布置二维 无人机室内全局路径规划是无人机研究领域 码，综合多个标志解算的信息能够提供较大范围 的一个重要课题。路径规划的含义为已知无人机 内的无人机全局位置信息。地标阵列除了使用规 运动的起点和终点，通过机载传感器对周围环境 则排列外，可以通过任意的形式和距离组合，只需 进行描述，得到自由区域和障碍区域，从而规划 要通过人工一次性标定后制表存入机载导航系统 出一条从起点到终点并且避开障碍物的最短路 存储器中即可。文中以最简单的矩阵规则排列形 径。本文在对各种主流路径规划算法比较之后， 式为例，所建立的地面定位阵列（后文简称二维码 基于室内地面二维码阵列标志，选用D*算法2， 阵列)，当确定某二维码为全局坐标系原点后即可 并对D*算法做了改进，实现无人机室内自主飞行。 以通过几何关系计算出其他二维码在全局坐标中 2.1D*路径规划 的位置，进而将摄像头解算出相对某二维码的位 为了在室内环境中寻找最短路径，首先需要 置转换到全局坐标系下。本文根据AUco库，生成 建立环境模型。D*算法采用栅格表示地图，将地 ID编号为1~100的二维码集，设计如图3所示的 图信息记录在每一个栅格上，定义该栅格的属性 二维码地面阵列。其中，单个二维码边长为18cm, 为自由或者障碍。无人机的飞行轨迹被栅格离散 码与码的间距为18cm,ID编号为1的码的中心 化之后，每个栅格上的运动信息表征了无人机在 为原点，行方向为x方向，列方向为y方向。例如 这个栅格上的飞行方向。理论上，无人机在每个 编号为55的二维码位置为(18×8cm,18×10cm)0 栅格上的运动有无数种，但考虑到模型建立的复 杂性，一般在实际运用中只定义8个方向的运动， 分别为：前、后、左、右、右前、右后、左前、左后， 如图4所示。 图3二维码地面阵列 Fig.3 QR code ground array 1.3摄像头位姿估计和全局坐标获取 摄像头经过标定之后，可以解算出二维码相 对于摄像头光学中心的旋转向量r和平移向量 图4栅格地图及无人机可能运动方向 T以及旋转矩阵R四，解算过程如下所示： Fig.4 Raster map and possible direction of movement of 0=norm(r) the drone r=r/0 地图上的栅格采用矩阵方式储存1，单个栅 R=cos(0)/+(1-cos0)rr+ (1) 格的信息记录在矩阵中的第i行和第j列，记为 0 -I R11 R12 R13 G(i,)。采用这种矩阵存储栅格地图的方法，可建 sin 0 -rx R21 R22 R23 立起环境的整幅地图。 -Ty 0 R31 R32 R33 在室内无人机飞行空间中，只考虑水平方向， arctan( 23 可将飞行空间二维化，其中无人机高度控制由超 R33 euler; arcsin(Ri3) (2) 声波传感器实现。栅格地图模型建立后，需要对 每个栅格设置编码信息。编码设置如下：1为含 arctan( R 有障碍的栅格，0为自由栅格（无障碍），start为起4) 标记识别：在这一点上，有必要确定获得 的标记候选哪些属于字典，哪些只是环境的一部 分。提取标记的候选码之后，就可获得 4 个不同 的标识符。为了加快这个过程，字典元素使用平 衡二叉树排序。为此，标记由通过连接其所有位 获得的整数值表示。 1.2 二维码阵列的设计 对一幅图像中多个二维码进行识别可以计算 出摄像头相对每个二维码的位置和姿态，未被识 别的标志不参与位置解算。同时摄像头能在任意 方向识别出唯一码号，通过在室内合理布置二维 码，综合多个标志解算的信息能够提供较大范围 内的无人机全局位置信息。地标阵列除了使用规 则排列外，可以通过任意的形式和距离组合，只需 要通过人工一次性标定后制表存入机载导航系统 存储器中即可。文中以最简单的矩阵规则排列形 式为例，所建立的地面定位阵列 (后文简称二维码 阵列)，当确定某二维码为全局坐标系原点后即可 以通过几何关系计算出其他二维码在全局坐标中 的位置，进而将摄像头解算出相对某二维码的位 置转换到全局坐标系下。本文根据 ArUco 库，生成 ID 编号为 1~100 的二维码集，设计如图 3 所示的 二维码地面阵列。其中，单个二维码边长为 18 cm， 码与码的间距为 18 cm，ID 编号为 1 的码的中心 为原点，行方向为 x 方向，列方向为 y 方向。例如 编号为 55 的二维码位置为 (18×8 cm,18×10 cm)。 图 3 二维码地面阵列 Fig. 3 QR code ground array 1.3 摄像头位姿估计和全局坐标获取 摄像头经过标定之后，可以解算出二维码相 对于摄像头光学中心的旋转向量 r 和平移向量 T 以及旋转矩阵 R [19] ，解算过程如下所示： θ = norm(r) r = r/θ R = cos(θ)I +(1−cos θ)rrT+ sinθ   0 −rz ry rz 0 −rx −ry rx 0   =   R11 R12 R13 R21 R22 R23 R31 R32 R33   (1) euleri =   γlocal θlocal ψlocal   =   arctan( R23 R33 ) −arcsin(R13) arctan( R12 R11 )   (2) posi = [ xlocal ylocal zlocal ] = −R ′ ·T (3) 根据式 (2) 可以计算出摄像头当前欧拉角， 式 (3) 可以计算出摄像头当前相对某个二维码的 局部位置，然后根据该二维码的全局坐标计算出 摄像头当前的全局坐标位置。进一步文中基于扩 展卡尔曼滤波器对二维码位置、光流传感器速度测 量值和机载 IMU 数据进行融合得到最优位姿估计。 2 改进 D*路径规划算法 无人机室内全局路径规划是无人机研究领域 的一个重要课题。路径规划的含义为已知无人机 运动的起点和终点，通过机载传感器对周围环境 进行描述，得到自由区域和障碍区域，从而规划 出一条从起点到终点并且避开障碍物的最短路 径。本文在对各种主流路径规划算法比较之后， 基于室内地面二维码阵列标志，选用 D*算法[20] ， 并对 D*算法做了改进，实现无人机室内自主飞行。 2.1 D*路径规划 为了在室内环境中寻找最短路径，首先需要 建立环境模型。D*算法采用栅格表示地图，将地 图信息记录在每一个栅格上，定义该栅格的属性 为自由或者障碍。无人机的飞行轨迹被栅格离散 化之后，每个栅格上的运动信息表征了无人机在 这个栅格上的飞行方向。理论上，无人机在每个 栅格上的运动有无数种，但考虑到模型建立的复 杂性，一般在实际运用中只定义 8 个方向的运动， 分别为：前、后、左、右、右前、右后、左前、左后， 如图 4 所示。 图 4 栅格地图及无人机可能运动方向 Fig. 4 Raster map and possible direction of movement of the drone 地图上的栅格采用矩阵方式储存[21] ，单个栅 格的信息记录在矩阵中的第 i 行和第 j 列，记为 G(i, j)。采用这种矩阵存储栅格地图的方法，可建 立起环境的整幅地图。 在室内无人机飞行空间中，只考虑水平方向， 可将飞行空间二维化，其中无人机高度控制由超 声波传感器实现。栅格地图模型建立后，需要对 每个栅格设置编码信息。编码设置如下：1 为含 有障碍的栅格，0 为自由栅格 (无障碍)，start 为起 ·664· 智 能 系 统 学 报 第 14 卷