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

摘要

本发明公开了一种室内无源导航定位系统和方法。本系统包括深度摄像机Kinect，惯性导航和标示符，Kinect和惯性导航连接到工控机SBC84823，标示符独立张贴在墙上；Kinect得到标示符的图像数据和深度数据，惯性导航测得自身的姿态角之后，通过USB接口将深度数据和图像数据传入到工控机SBC84823，由工控机SBC84823计算出机器人自身的坐标位置。本发明的方法是首先要根据数字识别技术识别出图像中标示符的世界坐标，计算出标示符相对于机器人（Kinect）的相对坐标；然后根据惯性导航测出机器人自身的姿态角和机器人的待求坐标组成变换矩阵得到变换方程，根据方程组求得机器人的自身的坐标。本发明的实施例主要用于图中机器人的坐标位置的检测和计算。

主权项

一种室内无源导航定位方法，该方法用于一种室内无源导航定位系统的定位，该室内无源导航定位系统包括：机器人(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)的匹配区，识别并定位出数字区的三个角的坐标，即右上定位点(XRU,YRU)、左下定位点(XLD,YLD)、右下定位点(XRD,YRD)，然后，由矩形的顶点位置关系可得标示符的左上定位点的坐标(XLU,YLU)，XLU＝XRU+XLD‑XRD，YLU＝YRU+YLD‑YRD；倘若在图像中未检测到标识的存在，调整机器人(5)的姿态和位置，重新进行步骤1；步骤3：对步骤2已找到的标示符(4)进行数字区的分割和提取；步骤4：工控机(2)利用数字识别技术对标示符(4)内的数字进行识别，从而获取标识相对世界坐标系的坐标步骤5：根据Kinect(1)获得的深度数据，和Kinect的特性计算出标示符(4)左上角定位点相对于Kinect坐标系的坐标具体计算按照如下公式进行：x′=-AXLUy′=-AYLUz′=AfA=Z0(1+Z0fbd)f]]>其中XLU和YLU是左上角定位点在图像中的坐标，Z0是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的姿态变换矩阵已知，根据机器人工学坐标系映射可知：可得到下式：即通过上式中l，m，n值就可以确定机器人(5)的坐标位置