一种室内无人机精确定位与自主导航的系统及方法与流程
本发明涉及无人飞行器技术领域，更具体地，涉及一种基于uwb(ultrawideband，超宽带)与视觉传感器融合的室内无人机精确定位与自主导航的系统及方法。
背景技术：
室外无人机定位导航往往依赖差分gps技术，而室内gps信号较弱，无法满足无人机实时定位导航的需求。
目前，关于室内定位有多种解决方案，例如wifi定位、超声波定位、蓝牙定位、地磁定位、卫星定位、射频识别、红外线定位、zigbee定位等。但是上述解决方案由于技术局限以及成本太高的问题，尚未有像gnss(globalnavigationsatellitesystem，全球卫星导航系统)定位技术一样让不同用户满意的成熟产品。这些现有技术要么定位精度无法满足要求，要么成本太高，难以推广。
因此，室内定位技术已成为在人员管理、资产管理、公共安全、突发事件处理等方面的研究热点。
技术实现要素：
本发明要解决的技术问题是如何以较低的成本实现室内无人机的实时精确定位与自主导航。
为了解决上述技术问题，本发明的技术方案是提供一种室内无人机精确定位与自主导航的系统，其特征在于：包括
无人机，用于实现无人机本身的正常飞行；所述无人机上设有超宽带uwb定位标签和标记点；
双目相机模块，用于通过捕捉地面二维码标签获得所述无人机位置信息；
uwb模块，用于通过捕捉所述无人机上的所述uwb定位标签获得所述无人机位置信息；
外部全局高清相机模块，用于通过多个全局高清相机捕捉所述无人机上的所述标记点获得所述无人机位置信息；
扩展卡尔曼滤波模块，用于融合所述双目相机模块、所述uwb模块以及所述外部全局高清相机模块获得的所述无人机位置信息，预测所述无人机姿态和位置，对所述无人机姿态和位置进行实时更新；
即时定位模块，用于对所述无人机进行即时定位；
导航模块，用于提供所述无人机的自主导航功能；
计算存储模块，用于计算并存储所述uwb模块与所述外部全局高清相机模块提供的所述无人机位置信息；
人机交互模块，用于提供人机交互功能，设置所述无人机的飞行航线。
优选地，所述无人机包括微型电脑主机minipc模块，所述扩展卡尔曼滤波模块与所述minipc模块、所述计算存储模块相连，所述计算存储模块与所述uwb模块、所述外部全局高清相机模块相连；所述minipc模块与所述双目相机模块、即时定位模块、人机交互模块、导航模块相连。
优选地，所述双目相机模块设于所述无人机的底部，并且所述双目相机模块的摄像头的视场方向垂直朝下保持固定；所述双目相机模块的摄像头下方设有地面标记点的二维码标签，所述二维码标签携带自身的世界坐标信息。
优选地，所述外部全局高清相机模块由至少3个全局高清相机呈环状分布组成，所述全局高清相机固定在离地面设定高度且朝向环的中心点。
优选地，所述双目相机模块获得所述无人机的位置信息并送入所述minipc模块，所述uwb模块以及所述外部全局高清相机模块分别获得所述无人机的位置信息并均送入所述计算存储模块，所述扩展卡尔曼滤波模块融合所述双目相机模块、所述uwb模块以及所述外部全局高清相机模块的位置信息，并预测所述无人机姿态和位置，同时对所述无人机姿态和位置进行实时更新；
所述人机交互模块通过手动设置无人机的飞行路线，并将无人机所需经过的路径信息通过数据通信传至所述minipc模块，所述minipc模块根据所述路径信息设定目标位置，并根据所述目标位置和无人机的当前姿态及位置实时调整无人机的姿态，使无人机不断向所述目标位置靠近并最终到达所述目标位置，实现无人机的自主导航。
本发明还提供了一种室内无人机精确定位与自主导航的方法，其特征在于：采用上述的室内无人机精确定位与自主导航的系统，所述方法包括如下步骤：
步骤s1：双目相机模块捕捉预先布置在地面的二维码标签，根据所述双目相机模块获取得到的二维码标签的图像获取二维码标志物的位置信息、姿态信息以及编码信息，由所述位置信息、姿态信息以及编码信息得到无人机相对于所述二维码标志物的位置信息；
步骤s2：无人机上贴有uwb定位标签，uwb模块捕捉无人机上的所述uwb定位标签，通过计算所述uwb定位标签到每个uwb模块定位基站的距离，计算无人机的位置信息；
步骤s3：无人机上贴有多个标记点，外部全局高清相机模块通过捕捉无人机上的所述标记点，利用同一时间不同全局高清相机图片上的标记点，采用对极几何与三角测量计算无人机的位置；
步骤s4：由扩展卡尔曼滤波模块融合所述双目相机模块、所述uwb模块、所述外部全局高清相机模块获得的无人机的位置信息，对无人机的姿态和位置进行实时更新；
步骤s5：通过人机交互模块输入无人机的路径信息，minipc模块根据所述路径信息设定目标位置，根据所述目标位置和无人机的当前姿态及位置实时调整无人机的姿态，使无人机不断向所述目标位置靠近并最终到达所述目标位置。
优选地，所述步骤s1具体包括以下子步骤：
s101：将所述二维码标志物的图像转化为单通道灰度图；
s102：根据所述单通道灰度图设定一个固定阀值，将所述单通道灰度图转化为二值图；
s103：对所述二值图进行轮廓检测，遍历所述二值图中所有边个数为4的多边形，并剔除面积小于预设阀值的多边形，然后将剩余的边个数为4的多边形进行正交投影，得到标准的正方形图像；
s104：按照预设的编码信息规则提取所述正方形图像中的二进制编码信息和角点信息；
s105：根据提取的二进制编码信息和角点信息得到无人机相对于二维码标志物的位置信息。
优选地，所述步骤s2具体包括以下子步骤：
s201：uwb基站布置；
首先根据现场状况布放m个超宽带微雷达信号源作为无人机飞行定位参照点，后台计算机以m个超宽带微雷达信号源为原点分别建立空间坐标系；m为正整数；
s202：信号源组网；
各个超宽带微雷达信号源发出的uwb信号组成信号网，实现超宽带微雷达信号源与无人机之间的无缝通信，确保无人机定位的精确性；
s203：无人机上的uwb定位标签以uwb脉冲重复不间断发送数据帧；
s204：所述uwb定位标签发送的uwb脉冲串被m个uwb基站接收；
s205：每个uwb定位基站测量每个uwb定位标签的数据帧到达接收器天线的时间；
s206：定位引擎参考所述uwb定位标签发送过来的校准数据，确定所述uwb定位标签达到不同uwb基站之间的时间差，并利用三点定位技术来计算所述uwb定位标签位置。
优选地，所述步骤s4具体包括以下子步骤：
s401：设置扩展卡尔曼滤波模块的状态转移方程；
s402：所述扩展卡尔曼滤波模块针对双目视觉模块，通过前一时刻的状态值预测后一时刻的状态值；
s403：用uwb定位模块、外部全局高清相机模块的测量值与s402中预测的状态值估计出最优值，并更新扩展卡尔曼增益常数；
s404：重复步骤s402和s403。
优选地，所述步骤s5中，通过人机交互模块预设的任务航点信息生成航线跟踪轨迹，并对所述航线跟踪轨迹进行离散处理，得到n个期望航点作为目标位置，n为正整数。
相比现有技术，本发明提供的室内无人机精确定位与自主导航的系统及方法具有如下有益效果：
(1)精确度高：对多种传感器获取的数据信息进行融合，根据融合后的结果来进行定位和导航，相较于采用单一传感器的定位和导航，其精确度更高；
(2)通过对地面特定位置上布置的二维码标志物进行识别，利用二维码的编码技术获取地标信息，由于二维编码能够提供丰富的地标信息且具有加密能力，能够为无人机提供多元化信息指引，进而扩展飞行任务的多样性；
(3)通过多个uwb信号源组网，能够实现uwb信号源与无人机之间的无缝通信，从而实现精确定位，定位精度达到5-10cm。
(4)结构简单，性能可靠，成本较低。
附图说明
图1为本发明的一个实施例提供的室内无人机精确定位与自主导航的系统的结构框图；
图2为本发明的一个实施例提供的室内无人机精确定位与自主导航的系统的数据流图；
图3为双目相机获得无人机位置信息算法的流程图；
图4为uwb模块获得无人机位置信息算法的流程图；
图5为全局摄像机安装布置示意图；
图6为全局摄像机获得无人机位置信息算法地流程图；
图7为扩展卡尔曼滤波融合示意图；
图8为无人机室内精确定位输出结果效果图。
具体实施方式
本实施例致力于提供一种基于uwb(ultrawideband，超宽带)与视觉传感器融合的室内无人机精确定位与自主导航的系统。uwb与视觉传感器融合是利用计算机技术将各个传感器的多源信息和数据，在一定的准则下加以自动分析和综合，以完成所需要的决策和估计而进行的信息处理过程。
通过将uwb与视觉传感器进行多层次、多空间的信息互补和优化组合处理，最终产生对观测环境的一致性解释。在这个过程中，要充分地利用多源数据进行合理支配与使用，而信息融合的最终目标则是基于各传感器获得的分离观测信息，通过对信息多级别、多方面组合导出更多有用信息。这不仅是利用了多个传感器相互协同操作的优势，而且也综合处理了其它信息源的数据来提高整个传感器系统的智能化。
图1为本实施例提供的室内无人机精确定位与自主导航的系统的结构框图，所述的室内无人机精确定位与自主导航的系统包括
无人机，用于实现无人机本身的正常飞行；
双目相机模块，通过捕捉地面二维码获得无人机位置信息；
uwb模块，通过捕捉无人机上的uwb定位标签获得无人机位置信息；
外部全局高清相机模块，通过捕捉无人机上多个标记点获得无人机位置信息；
扩展卡尔曼滤波模块，融合双目相机模块、uwb模块以及外部全局高清相机获得的无人机位置信息，预测无人机姿态和位置，对无人机姿态和位置进行实时更新；
即时定位模块，用于无人机即时定位；
导航模块，用于提供无人机的自主导航功能；
计算存储模块，用于计算并存储uwb模块与外部全局高清相机模块提供的无人机位置信息；
人机交互模块，用于提供人机交互功能，设置无人机的飞行航线。
其中，无人机由minipc(微型电脑主机)模块、主控模块、无线模块、电源模块、电机及驱动模块等构成，minipc模块与主控模块、无线模块、电源模块、电机及驱动模块等相连。
扩展卡尔曼滤波模块与minipc模块、计算存储模块相连，计算存储模块与uwb模块、外部全局高清相机模块相连；minipc模块与无人机其他各模块、双目相机模块、即时定位模块、人机交互模块、导航模块相连。
如图2所示，双目相机模块2位于无人机1的底部，并且双目相机模块2的摄像头的视场方向垂直朝下保持固定。地面3d标记点的二维码标签3布置于地面，并且携带自身的世界坐标信息；外部全局高清相机模块4安置在摄像头支架上，或安置在墙壁上或者悬挂在建筑物上。
系统工作时的数据流按照图2中所示进行传输。双目相机模块2获得的无人机1的位置信息直接送入minipc模块，uwb模块5以及外部全局高清相机模块4分别获得的无人机位置信息均送入计算存储模块6，再由扩展卡尔曼滤波模块融合三者获得的信息，预测无人机姿态和位置，对无人机姿态和位置进行实时更新；人机交互模块通过手动设置无人机飞行路线，将无人机所需经过的路径通过数据通信，传至minipc模块，实现无人机的自主导航。
本实施例提供的室内无人机精确定位与自主导航的系统工作时的步骤具体如下：
步骤s1：双目相机模块2捕捉预先布置在地面的二维码标签3，根据双目相机模块2获取得到的二维码标签的图像获取二维码标志物的位置信息、姿态信息以及编码信息，由所述位置信息、姿态信息以及编码信息得到无人机相对于所述二维码标志物的位置信息；
步骤s2：无人机1贴有uwb定位标签，uwb模块捕捉无人机1上的uwb定位标签，通过计算uwb定位标签到每个uwb模块定位基站的距离，计算无人机1的位置信息；
步骤s3：外部全局高清相机模块4通过捕捉无人机1上的标记点，利用同一时间不同全局高清相机图片上的标记点，采用对极几何与三角测量计算无人机1的位置；
步骤s4：uwb模块5与外部全局高清相机模块4获得的无人机1的位置信息由计算存储模块6送到minipc模块，再由扩展卡尔曼滤波模块融合双目相机模块2、uwb模块5、外部全局高清相机模块4获得的无人机1的位置信息，对无人机1的姿态和位置进行实时更新；
步骤s5：通过人机交互模块输入无人机1的路径信息，minipc模块依次读取存储的目标位置，根据目标位置和无人机1的当前姿态及位置实时调整无人机1的姿态，使无人机1不断向目标位置靠近并最终到达目标位置。
设系统的状态量为
双目相机模块的状态量(作为核心状态量)，
uwb模块的状态量，
外部全局高清摄相机模块的状态量，
如图3所示，步骤s1(双目相机模块获得无人机位置信息)中具体包括以下子步骤：
s101：将所述二维码标志物的图像转化为单通道灰度图；
s102：根据单通道灰度图设定一个固定阀值，将所述单通道灰度图转化为二值图；
s103：对所述二值图进行轮廓检测，遍历所述二值图中所有边个数为4的多边形，并剔除面积小于预设阀值的多边形，然后将剩余的边个数为4的多边形进行正交投影，得到标准的正方形图像；
s104：按照预设的编码信息规则提取所述正方形图像中的二进制编码信息和角点信息；
s105：根据提取的二进制编码信息和角点信息得到无人机相对于二维码标志物的位置信息。
所述双目相机模块获得无人机位置信息的具体算法如下：
摄像头采集到局部视觉标记点后，通过已知的空间3d点与图像2d点对应的点对，利用pnp(perspective-n-point)算法，来计算相机(无人机)位姿。
从世界坐标系到相机坐标系的转换，需要矩阵[r|t]，其中r是旋转矩阵，t是位移向量。如果世界坐标系为p，相机坐标系对应坐标为x′，那么x′＝[r|t]*p。从相机坐标系到理想屏幕下像素坐标系的变换就需要内参数矩阵k。那么理想屏幕下像素坐标系l＝k*[r|t]*p。定位，也就求解相机在世界坐标系下旋转与平移了多少的问题，即求解[r|t]。
利用已知的标记点的世界坐标p，然后在摄像头捕获的帧里获得标记点对应点在理想屏幕下像素坐标l(已知)，通过求解线性方程组得到[r|t]的初值，再利用非线性最小二乘法迭代求得最优变换矩阵[r|t]。
考虑某个标记点p，它的齐次坐标为p＝(xyz1)′。在图像i1中，标记点p投影到特征点x1＝(u1v11)。此时相机的位姿r，t是未知的。定义增广矩阵[r|t]为一个3*4的矩阵，包含了旋转与平移信息。我们把它的展开形式列写如下：
利用机载双目摄像头，可以通过三角测量获得标记点的尺度信息s，标记点世界坐标(xyz1)已知，与其对应的归一化平面上的点(u1v11)已知。[r|t]为3*4矩阵，共有12维，因此通过六对匹配点，来实现矩阵[r|t]的线性求解。当匹配点大于六对时，使用svd的方法对超定方程求最小二乘解。
如图4所示，步骤s2(uwb模块获得无人机位置信息)中具体包括以下子步骤：
s201：uwb基站布置；首先根据现场状况在合适位置布放4个超宽带微雷达信号源作为无人机飞行定位参照点，后台计算机以4个超宽带微雷达信号源为原点分别建立空间坐标系；
s202：信号源组网；各个超宽带微雷达信号源发出的uwb信号组成信号网，实现超宽带微雷达信号源与无人机之间的无缝通信，确保无人机定位的精确性；
s203：无人机上的uwb定位标签以uwb脉冲重复不间断发送数据帧；
s204：uwb定位标签发送的uwb脉冲串被四个uwb基站接收；
s205：每个uwb定位基站测量每个uwb定位标签的数据帧到达接收器天线的时间；
s206：定位引擎参考uwb定位标签发送过来的校准数据，确定uwb定位标签达到不同uwb基站之间的时间差，并利用三点定位技术及优化算法来计算uwb定位标签位置；多基站定位采用tdoa(到达时间差)算法。
如图5所示，步骤s3(外部全局高清相机模块获得无人机位置信息)中，外部全局高清相机至少由3个摄像头组成，呈环状分布，摄像头固定在离地面一定高度，朝向环的中心点，用于将采集的无人机图像数据传输到计算存储模块中，以便精确定位可视区域内的标记点。所述可视区域为最少两个全局高清相机可观测到的共同区域，并在机身上设置至少三个容易被摄像头捕捉到的标记点；通过多目视觉坐标测量算法获取所述无人机上标记点的位置，并传输到计算存储模块中；所述计算存储模块通过四元数法解算出无人机的欧拉角和三维坐标，从而得到无人机的位置和姿态信息；
所述外部全局高清相机模块获得无人机位置信息的具体算法如下：
利用外部全局高清相机中任意两个摄像头，两两配对，采用两个摄像头三角测量的方式计算无人机的位姿，并进行优化。
三角测量在三角学与几何学上是一借由测量目标点与固定基准线的已知端点的角度，测量目标距离的方法。而不是直接测量特定位置的距离(三边量测法)。当已知一个边长及两个观测角度时，观测目标点可以被标定为一个三角形的第三个点。
从世界坐标系到相机坐标系的转换，需要经过[r|t]和相机内参k，即l＝k*[r|t]*p，p为世界坐标系下的坐标，l理想屏幕下像素坐标。
如图5所示，无人机被6个外部全局高清相机模块的摄像机捕捉到，本实施例中任选其中两个摄像机(设选择第i个和第j个摄像机，i、j为正整数)进行计算。结合图6，已知固定相机的位姿，即[r|t]，利用三角测量可以计算出无人机的尺度s，如式(2)：
sipi＝sjrpj+t(2)
其中pi为像素p在相机i下的像素坐标，pj为像素p在相机j下的像素坐标。si为pi在相机i下的尺度(即深度)，sj为pj在相机j下的尺度。
利用尺度s和相机的[r|t]，将无人机由图片上的坐标还原到世界坐标系中，从而实现对无人机的定位。
本实施例中，6个相机，两两配对，每对均对无人机进行定位，6个相机有15种配对方式，故有15组解。利用最小二乘法，求取最优的解，从而定位无人机。
如图7所示，使用扩展卡尔曼滤波模块，融合相应模块获取的数据信息计算，具体包括将双目视觉模块、uwb定位模块以及外部全局高清相机模块获取的无人机位置信息进行融合。以双目视觉模块测得的无人机位置信息为核心状态，利用uwb定位模块与外部全局高清相机模块获得的无人机位置信息对双目视觉模块获得的无人机位置信息进行更新，具体步骤为：
s401：设置扩展卡尔曼滤波模块的状态转移方程；
s402：扩展卡尔曼滤波模块对双目视觉模块，通过h-1时刻的状态预测h时刻的状态；
s403：用uwb定位模块、外部全局高清相机模块的测量值与s402中预测的状态值估计出最优值，并更新扩展卡尔曼增益常数；
s404：重复步骤s402和s403。
其中，更新扩展卡尔曼增益常数的公式如下：
其中，kk为扩展卡尔曼增益，
通过人机交互模块预设的任务航点信息生成航线跟踪轨迹，并对所述航线跟踪轨迹进行离散处理，得到n个期望航点，n为正整数；根据目标位置和无人机的当前姿态及位置实时调整无人机的姿态，使无人机不断向目标位置靠近，并最终到达目标位置。
本发明在多个大型室内场景得到应用，能够实现精确的无人机自主定位导航。如图8所示(a为测试数据，b为效果图)，测试时输入uwb以及视频数据，输出无人机的6自由度位姿，其中角度采用四元数表示，并采用rviz显示工具实时显示无人机运动路径。通过激光测距仪以及量角器的真实测量，无人机定位精度位置在1cm以内。目前最成熟的uwb单传感器定位精度误差在10-20cm之间，我们的定位精度在其10倍以上。可以实现无人机在室内的精确定位与自主导航。
以上所述，仅为本发明的较佳实施例，并非对本发明任何形式上和实质上的限制，应当指出，对于本技术领域的普通技术人员，在不脱离本发明方法的前提下，还将可以做出若干改进和补充，这些改进和补充也应视为本发明的保护范围。凡熟悉本专业的技术人员，在不脱离本发明的精神和范围的情况下，当可利用以上所揭示的技术内容而做出的些许更动、修饰与演变的等同变化，均为本发明的等效实施例；同时，凡依据本发明的实质技术对上述实施例所作的任何等同变化的更动、修饰与演变，均仍属于本发明的技术方案的范围内。
- 还没有人留言评论。精彩留言会获得点赞！