[发明专利]室内无源导航定位方法

权利要求书

1.一种室内无源导航定位方法，该方法用于一种室内无源导航定位系统的定位，该室内无源导航定位系统包括：机器人(5)处于室内辐射环境，深度摄像机Kinect(1)集成于机器人(5)内部，与机器人(5)内部的工控机SBC84823(2)通过USB串口连接；工控机(2)对深度摄像机Kinect(1)传回的图像数据和深度数据进行接收和处理；该室内无源导航定位系统还有一个Xsens惯性导航(3)集成于机器人(5)内部，与工控机SBC84823(2)通过另一个USB串口连接；工控机SBC84823(2)对Xsens惯性导航(3)测量的自身姿态角进行接收和处理；另外，该室内无源导航定位系统在室内还安置一个标识符(4)，该标识符(4)包含有标识符的位置的数学图形，作为图像数据被Kinect(1)采集；该室内无源导航定位方法的特征在于，操作步骤如下：

步骤1：机器人(5)在室内运行，并利用Kinect(1)采集建筑内部的图像数据和深度数据，并将数据传输给工控机(2)；

步骤2：工控机(2)处理系统通过模板匹配方法，对图像数据进行模板匹配以找到定位标示符(4)的匹配区，识别并定位出数字区的三个角的坐标，即右上定位点(X_RU,Y_RU)、左下定位点(X_LD,Y_LD)、右下定位点(X_RD,Y_RD)，然后，由矩形的顶点位置关系可得标示符的左上定位点的坐标(X_LU,Y_LU)，X_LU＝X_RU+X_LD-X_RD，Y_LU＝Y_RU+Y_LD-Y_RD；倘若在图像中未检测到标识的存在，调整机器人(5)的姿态和位置，重新进行步骤1；

步骤3：对步骤2已找到的标示符(4)进行数字区的分割和提取；

步骤4：工控机(2)利用数字识别技术对标示符(4)内的数字进行识别，从而获取标识相对世界坐标系的坐标

步骤5：根据Kinect(1)获得的深度数据，和Kinect的特性计算出标示符(4)左上角定位点相对于Kinect坐标系的坐标具体计算按照如下公式进行：

$\begin{array}{cc}\left\{\begin{array}{c}{x}^{\′}=-{\mathrm{AX}}_{LU}\\ y^{\′}=-{\mathrm{AY}}_{LU}\\ z^{\′}=Af\end{array}\right.& A=\frac{Z_0}{(1+\frac{Z_0}{fb}d)f}\end{array}$

其中X_LU和Y_LU是左上角定位点在图像中的坐标，Z₀是Kinect特性参考平面的距离，d是Kinect(1)发射激光经过实际平面和参考平面映射在图像上的距离，b是Kinect(1)激光发射器和相机的距离，f是相机的焦距；

步骤6：根据惯性导航(3)和待求的未知的机器人(5)在世界坐标系的坐标推导出机器人(5)Kinect坐标系O′-X′Y′Z′相对于世界坐标系的变换矩阵，具体按照如下步骤进行：

①由固定在机器人上的惯性导航(3)测出机器人(5)自身的姿态角α,β,γ：其中γ是机器人(5)绕X轴旋转的角度——俯仰角，β是机器人(5)绕Y轴旋转的角度——滚转角，α是机器人(5)绕Z轴旋转的角度——偏航角；

利用姿态角α,β,γ计算出机器人坐标系O′-X′Y′Z′相对于世界坐标系O-XYZ的旋转矩阵：

其中是机器人(5)相对于世界坐标的坐标，即机器人(5)在房间中的位置；步骤7：为标识符在世界坐标系O-XYZ下的坐标；为标示符在Kinect坐标系O-X′Y′Z′下的坐标；Kinect坐标系O′-X′Y′Z′相对于世界坐标系O-XYZ的姿态变换矩阵已知，根据机器人工学坐标系映射可知：可得到下式：

$\left\{\begin{array}{c}l=x-x^{\′}c\mathrm{\α}c\mathrm{\β}-y^{\′}(c\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}-s\mathrm{\α}c\mathrm{\γ})-z^{\′}(c\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}+s\mathrm{\α}s\mathrm{\γ})\\ m=y-x^{\′}s\mathrm{\α}c\mathrm{\β}-y^{\′}(s\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}+c\mathrm{\α}c\mathrm{\γ})-z^{\′}(s\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}-c\mathrm{\α}s\mathrm{\γ})\\ n=z+x^{\′}s\mathrm{\β}-y^{\′}c\mathrm{\β}s\mathrm{\γ}-z^{\′}c\mathrm{\β}c\mathrm{\γ}\end{array}\right.$

即通过上式中l，m，n值就可以确定机器人(5)的坐标位置