CN102436260B - 一种室内自主定位与定向的二维导航系统 - Google Patents

Abstract

本发明公开了一种室内自主定位与定向的二维导航系统，该二维导航系统由MEMS陀螺传感器、DSP数字信号处理器、数字舵机和超声波收发器等部件组成。本发明的二维导航系统安装在运动载体上，DSP数字信号处理器一方面处理MEMS陀螺传感器的测量数据，获得运动载体相对于其初始位置的偏转角度。另一方面通过调节数字舵机摇臂的偏转角度，对分布在载体两侧、固联在舵机摇臂上的超声波收发器的对准方位实施可控调节，实现扫描测距和定向测距两种测距模式，测得载体到障碍面的垂直距离；并利用收到回波时处理器内控制数字舵机偏转角度的变量换算出此时对应的测距指向角度；最后根据位置更新矩阵更新当前载体相对于其初始位置的位置坐标，完成对运动载体的一次局部自主定位与定向。

Description

一种室内自主定位与定向的二维导航系统

技术领域

本发明涉及一种对运动载体进行室内自主定位与定向的二维导航系统。

背景技术

对运动载体的定位包括室外环境的定位和室内环境的定位。目前主要针对室外环境进行定位的系统有采用蜂窝网络技术定位的系统和GPS全球定位系统。GPS全球定位系统目前能提供的室外定位精度约为3m～5m，蜂窝网络定位系统的精度远差于此。这些定位系统受到建筑物对信号的遮挡、反射和衍射等影响，在室内环境下使用往往存在更大的定位误差，不能满足用户对室内定位的精度需求。

室内定位系统与室外定位系统不同，用户更关心的是自己在室内平面的二维坐标和运动的航向信息。由于室内环境相对较小，用户对定位精度的要求也比室外环境更高。目前，室内定位技术在室内移动探测器、救灾机器人车、仓库搬运机器人车的跟踪和运动导航，以及汽车防撞检测、自动泊车等领域有广泛的应用需求。

根据导航定位使用的设备仪器不同，目前室内定位的方式主要有：电磁导航、无线电导航、无线网络导航、激光导航、视觉导航、超声波导航和多种传感器融合导航等。这些导航方式主要的优缺点对比如下：电磁导航是让低频电流流过埋设在路径下的电缆，然后用安装在机器人上的线圈检测电缆周围产生的磁场，并根据该信息计算位置，并控制方向，以实现沿规定路径的导航控制。主要优点是导引线隐蔽，不易污染和破损，导引原理简单，便于控制和通信，对声光无干扰；缺点是灵活性差，改变或扩充路径麻烦，对导引线附近的铁磁物质有干扰，电缆铺设工作量大，维护困难。

无线电导航是通过利用移动通信网络来实施定位导航的。具体有利用基站发射信号的到达时间差技术(TDOA)、信号到达角技术(AOA)和信号衰减技术来确定用户与基站之间的距离，利用球面三角算出移动终端的位置。主要优点是可以利用现有的移动通信网络的基站和用户手持设备完成定位服务，无需额外开发新的设备；缺点是信号受建筑物和周围环境的影响，定位精度不高，一般在50米以上，通常只用于粗定位。

无线网络导航技术，利用Wi-Fi、蓝牙(Bluetooth)等室内无线网络，通过用户设备接收到的室内无线信号的强弱(received signal strength，RSS)测定信号点与接收点的距离，进而进行定位计算的一种定位技术。无线网络目前已经开始广泛普及，尤其是在医院、学校、办公大楼等地区，使其用于室内定位成为可能，并且实施成本并不高。但是这种定位方式需要首先为使用环境建立一个精确的无线网络传播模型，当室内环境改变时，会影响无线信号强度的发布，需要重新修改模型；目前单独是使用无线网络实施室内定位的精度最高只能达到米级。

激光导航的一种方案是用一个指向标发出旋转扫描激光光束，再利用运动载体上的若干个传感器来检测，从而求出指向标的方向和到指向标的距离，用以确定运动载体的位置；另外还有和导向电缆方式类似的采用激光导引运动载体的方法。激光导航的优点是传感器发散小或没有发散，并对大多数物体无镜面反射现象；缺点是存在潜在的安全问题(首先是人眼安全问题)，且不适用于透明物质。

视觉导航成为近几年室内导航研究的热点，运动载体利用视觉传感器获取的信息进行定位，从而实现智能行驶。视觉导航中一般使用的方法如基于像素匹配的方法识别特征点、基于光流场分析的方法估计环境结构等等。视觉导航方法的优点是获取信息量大，灵敏度高，成本低，并且可根据需要灵活地改变或扩充路径，具有很好的柔性；缺点是对环境光线有一定要求，并且由于计算复杂对导航的实时性有一定影响。

超声波测距作为一种常用的非接触式测距方式，长期以来一直是室内导航定位研究的热点。常用的超声波定位方法有反射式和超声信标式。反射式将超声发射和接收做为一体，使用超声波收发器发出和接收信号，完成距离的测量。在目前的应用中，主要采用超声波收发器与载体固定连接的方法。这种方法最主要的缺点是只能获得超声波收发器到与之垂直的障碍面的距离，测量角度范围小(一般±5°)且难以控制，当载体姿态发生改变导致与之固联的超声波收发器的测量角度范围离开了障碍面时，将不能收到回波，无法完成测量。

超声信标式，如L.Kleeman使用主动超声作为信标，在运动载体身上装有超声波接收器，测得运动载体和信标的距离及方位从而估计运动载体的位姿。这种方法扩大了超声波定位的使用范围，但需在室内预先铺设很多的超声波信标，且超声波接收器很难区分不同信标发出的信号。

综合上述两种方法，超声波导航的优点是硬件结构简单，价格低廉，容易操作；缺点是速度慢，传感器存在较大的波束角(角度分辨率低)，单一传感器的稳定性不理想等。在实际应用中，往往采用其它传感器来补偿，或采用多传感器融合技术提高检测精度等。

多传感器融合导航组合了多种导航方式。已有的室内多传感器融合导航系统往往融合了无线网络导航、激光导航、超声波导航等，如AT&T Lab Cambridge的Activebadge和Bat室内定位系统，MIT的Cricket系统。多传感器融合导航系统弥补了的单一传感器的许多固有缺点，提高了定位精度，但是系统复杂程度增加，运行和维护成本更高。

发明内容

本发明的目的是提供一种室内自主定位与定向的二维导航系统，该二维导航系统能够获取运动载体相对于其初始位置的偏转角度(定向)和位置坐标(定位)，满足在相对简洁的室内使用环境下对运动载体实施自主定位与定向的需求。

在本发明中二维导航系统由MEMS陀螺传感器、DSP数字信号处理器、数字舵机和超声波收发器等部件组成。本发明的二维导航系统安装在运动载体上，一方面DSP数字信号处理器处理MEMS陀螺传感器的测量数据，获得运动载体相对于其初始位置的偏转角度(即航向信息)；另一方面DSP数字信号处理器通过调节数字舵机摇臂的偏转角度(即扫描角度)，对载体两侧固联在舵机摇臂上的超声波收发器的对准方位实施可控调节，实现扫描测距和定向测距两种测距模式，测得载体到障碍面的垂直距离，并利用收到回波时处理器内控制数字舵机偏转角度的变量换算出此时对应的测距指向角度；最后根据位置更新矩阵更新当前载体相对于其初始位置的位置坐标，对运动载体实施室内自主定位与定向，具有实用意义。

本发明的一种室内自主定位与定向的二维导航系统，该二维导航系统包括有DSP数字信号处理器(1)、MEMS陀螺传感器(2)、转向舵机(3)、左超声波收发器(4)、右超声波收发器(7)、左扫描舵机(5)和右扫描舵机(6)；其中：左扫描舵机(5)包括有左舵机本体(5A)、左舵机摇臂(5B)和左舵机转轴(5C)；左舵机摇臂(5B)为T形摇臂，左舵机摇臂(5B)的一端绕左舵机转轴(5C)旋转，左舵机摇臂(5B)安装在左舵机本体(5A)上；其中：右扫描舵机(6)包括有右舵机本体(6A)、右舵机摇臂(6B)和右舵机转轴(6C)；右舵机摇臂(6B)为T形摇臂，右舵机摇臂(6B)的一端绕右舵机转轴(6C)旋转，右舵机摇臂(6B)安装在右舵机本体(6A)上；

所述的DSP数字信号处理器(1)安装在运动载体(10)上；

所述的MEMS陀螺传感器(2)安装在运动载体(10)上，且安装方位需要保证MEMS陀螺传感器(2)的三轴与运动载体(10)的坐标系o_b-x_by_bz_b的三轴分别重合；MEMS陀螺传感器(2)用于测量运动载体(10)在运动状态下的三轴角速率信息F₂₁(ω_bx，ω_by，ω_bz)，ω_bx表示在坐标系o_b-x_by_bz_b下绕x_b轴转动的角速率分量，ω_by表示在坐标系o_b-x_by_bz_b下绕y_b轴转动的角速率分量，ω_bz表示在坐标系o_b-x_by_bz_b下绕z_b轴转动的角速率分量；

所述的转向舵机(3)安装在运动载体10的前端，转向舵机摇臂31与运动载体(10)的转向机构相连；转向舵机3用于操纵运动载体(10)实现转向；

所述的左超声波收发器(4)安装在左舵机摇臂(5B)上，且保证左超声波收发器(4)所发超声波波束的方向与左舵机摇臂(5B)的指向相同；左超声波收发器(4)根据收到的左超声波测距脉冲信号F₁₄后发射出超声波，然后当左超声波收发器(4)收到回波后，则输出左距离脉冲信号F₄₁给DSP数字信号处理器(1)；

所述的右超声波收发器(7)安装在右舵机摇臂(6B)上，且保证右超声波收发器(7)所发超声波波束的方向与右舵机摇臂(6B)的指向相同；右超声波收发器(7)根据收到的右超声波测距脉冲信号F₁₇后发射出超声波，然后当右超声波收发器(7)收到回波后，则输出右距离脉冲信号F₇₁给DSP数字信号处理器(1)。

本发明的一种室内自主定位与定向的二维导航系统的优点在于：

①本发明中，超声波收发器的指向受到DSP数字信号处理器的可控调节，能够实现扫描测距和定向测距两种测距模式：扫描测距模式在数字舵机摇臂旋转扫描过程中，超声波收发器检测是否收到回波，当收到回波时数字舵机摇臂停止旋转，进入定向测距模式。使用这种方法可以保证不管载体如何运动，超声波收发器总能自动修改指向寻找障碍面，始终保持与障碍面的垂直，始终能完成测距，有效地克服了传统的与载体固定连接的反射式测距方法的不足。

②本发明中，组合使用了MEMS惯性传感器确定载体运动方向。DSP数字信号处理器通过处理MEMS陀螺传感器的测量数据，能获得运动载体相对于其初始位置的偏转角度，在辅助完成定位功能的同时还给用户提供了载体的航向信息。

③本发明的一种室内自主定位与定向的二维导航系统已在遥控电动车自动倒车入库的应用中通过了室内试验验证。定位与定向精度高，方法正确可行，易于工程实现。。

④本发明的二维导航系统能够应用于移动探测器、移动机器人车、救灾机器人车的室内运动导航，以及在汽车防撞检测、自动泊车等领域都具有广泛实用前景。

附图说明

图1是本发明室内自主定位与定向的二维导航系统的硬件结构图。

图1A是本发明室内自主定位与定向的二维导航系统的另一视角硬件结构图。

图2是本发明室内自主定位与定向的二维导航系统中DSP处理器的功能框图。

图3是本发明室内自主定位与定向的二维导航系统中定位解算的情景示意图。

图4是本发明室内自主定位与定向的二维导航系统的工作流程框图。

1.DSP数学信号处理器               101.当前位置坐标解算单元

102.陀螺信息读取单元              103.转向舵机控制脉冲生成单元

104.左超声波触发脉冲生成单元      105.右超声波触发脉冲生成单元

106.左超声波距离获取单元          107.右超声波距离获取单元

108.左扫描舵机控制脉冲生成单元    109.右扫描舵机控制脉冲生成单元

110.超声波测距指向解析单元        2.MEMS陀螺传感器

3.转向舵机                     4.左超声波收发器       5.左扫描舵机

5A.左舵机本体                  5B.左舵机摇臂          5C.左舵机转轴

7.右超声波收发器               6.右扫描舵机           6A.右舵机本体

6B.右舵机摇臂                  6C.右舵机转轴          10.运动载体

具体实施方式

下面将结合附图对本发明做进一步的详细说明。

参见图1、图1A所示，本发明的一种室内自主定位与定向的二维导航系统，其组成包括：DSP数字信号处理器1、MEMS陀螺传感器2、转向舵机3、左超声波收发器4、右超声波收发器7、左扫描舵机5和右扫描舵机6；其中：左扫描舵机5包括有左舵机本体5A、左舵机摇臂5B和左舵机转轴5C；左舵机摇臂5B为T形摇臂，左舵机摇臂5B的一端绕左舵机转轴5C旋转，左舵机摇臂5B安装在左舵机本体5A上。其中：右扫描舵机6包括有右舵机本体6A、右舵机摇臂6B和右舵机转轴6C；右舵机摇臂6B为T形摇臂，右舵机摇臂6B的一端绕右舵机转轴6C旋转，右舵机摇臂6B安装在右舵机本体6A上。

在本发明中，运动载体10的坐标系记为o_b-x_by_bz_b，也称为本体坐标系o_b-x_by_bz_b；坐标原点o_b保持与运动载体10的几何中心重合，o_b-x_by_bz_b坐标系的x_b轴沿运动载体的纵向轴向前，o_b-x_by_bz_b坐标系的y_b轴沿运动载体的横向轴向左，o_b-x_by_bz_b坐标系的z_b轴垂直于运动载体向上，与x_b、y_b构成右手坐标系。

所述的DSP数字信号处理器1安装在运动载体10上。

所述的MEMS陀螺传感器2安装在运动载体10上，且安装方位需要保证MEMS陀螺传感器2的三轴与运动载体10的坐标系o_b-x_by_bz_b的三轴分别重合。MEMS陀螺传感器2用于测量运动载体10在运动状态下的三轴角速率信息F₂₁(ω_bx，ω_by，ω_bz)，ω_bx表示在坐标系o_b-x_by_bz_b下绕x_b轴转动的角速率分量，ω_by表示在坐标系o_b-x_by_bz_b下绕y_b轴转动的角速率分量，ω_bz表示在坐标系o_b-x_by_bz_b下绕z_b轴转动的角速率分量。

所述的转向舵机3安装在运动载体10的前端，转向舵机摇臂31与运动载体10的转向机构相连；转向舵机3用于操纵运动载体10实现转向。

所述的左超声波收发器4安装在左舵机摇臂5B上，且保证左超声波收发器4所发超声波波束的方向与左舵机摇臂5B的指向相同。左超声波收发器4根据收到的左超声波测距脉冲信号F₁₄后发射出超声波，然后当左超声波收发器4收到回波后，则输出左距离脉冲信号F₄₁给DSP数字信号处理器1。

所述的右超声波收发器7安装在右舵机摇臂6B上，且保证右超声波收发器7所发超声波波束的方向与右舵机摇臂6B的指向相同。右超声波收发器7根据收到的右超声波测距脉冲信号F₁₇后发射出超声波，然后当右超声波收发器7收到回波后，则输出右距离脉冲信号F₇₁给DSP数字信号处理器1。

所述的左舵机本体5A安装在运动载体10上，其安装方位需要保证在左舵机本体5A复位时(上电初始状态)左舵机摇臂5B的指向沿着运动载体10的纵向轴(即o_b-x_by_bz_b坐标系的x_b轴)向前，且以后左舵机摇臂5B能向左旋转不受阻碍。为了便于定位解算，左舵机本体5A安装时同时保证左舵机转轴5C垂直于运动载体10的左上顶点AA。

所述的右舵机本体6A安装在运动载体10上，其安装方位需要保证在右舵机本体6A复位时(上电初始状态)右舵机摇臂6B的指向沿着运动载体10的纵向轴(o_b-x_by_bz_b坐标系的x_b轴)向前，且以后右舵机摇臂6B能向右旋转不受阻碍。为了便于定位解算，右舵机本体6A安装时同时保证右舵机转轴6C垂直于运动载体10的右上顶点BB。

在本发明中，左超声波收发器4和左扫描舵机5组成左侧扫描测距单元；右超声波收发器7和右扫描舵机6组成右侧扫描测距单元(参见图2所示)。

本发明中，DSP数字信号处理器1选用AnalogDevices公司生产的ADSP-BF537处理器。MEMS陀螺传感器2选用InvenSense公司生产的ITG-3200陀螺仪。转向舵机3选用辉盛公司的Tower Pro 9805BB舵机，其舵机摇臂31选取原装的53.3mm直型摇臂。左扫描舵机5选用ESKY公司的EK2-0508型数字舵机，其左舵机摇臂5B选取原装的T形摇臂。右扫描舵机6选用ESKY公司的EK2-0508型数字舵机，其右舵机摇臂6B选取原装的T形摇臂。左超声波收发器4选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器，其探测范围1cm～4m，精度可达3mm。右超声波收发器7选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器，其探测范围1cm～4m，精度可达3mm。

参见图2所示，用于实现本发明室内自主定位与定向的二维导航系统的DSP数字信号处理器1，该DSP数字信号处理器1根据实现的功能包括有下列单元：当前位置坐标解算单元101、陀螺信息读取单元102、转向舵机控制脉冲生成单元103、左超声波触发脉冲生成单元104、左超声波距离获取单元106、右超声波触发脉冲生成单元105、右超声波距离获取单元107、左扫描舵机控制脉冲生成单元108、右扫描舵机控制脉冲生成单元109、超声波测距指向解析单元110。

下面将详细说明在DSP数字信号处理器1上各个单元实现的功能：

(1)陀螺信息读取单元102

陀螺信息读取单元102第一方面用于读取MEMS陀螺传感器2测量得到的运动载体10在运动状态下的三轴角速率信息F₂₁(ω_bx，ω_by，ω_bz)；第二方面对三轴角速率信息F₂₁(ω_bx，ω_by，ω_bz)中的绕z_b轴转动的角速率分量ω_bz进行初始时刻至当前时刻角速率分量的积分处理

t₀表示初始时刻，t表示当前时刻，得到运动载体10相对于导航坐标系O-x_oy_oz_o的偏转角度θ(参见图3所示)。

在本发明中，用于确定运动载体10位置的坐标系称为导航坐标系O-x_oy_oz_o，导航坐标系O-x_oy_oz_o的坐标原点O与初始位置时的运动载体10的几何中心重合，x_o沿初始位置时的运动载体10的纵向轴向前，y_o沿初始位置时的运动载体10的横轴向左，z_o垂直运动载体10向上，与x_o、y_o构成右手坐标系。

(2)转向舵机控制脉冲生成单元103

转向舵机控制脉冲生成单元103第一方面用于产生控制转向舵机3的脉冲信号F₁₃(简称为转向舵机脉冲信号F₁₃)；第二方面以转向舵机脉冲宽度变量F₃₁控制转向舵机脉冲信号F₁₃的脉冲宽度W_F13(简称为转向舵机脉冲宽度W_F13)，且

f_CLK表示DSP数字信号处理器1片上定时器工作的时钟频率；转向舵机依据接收到的具有不同脉冲宽度W_F13的转向舵机脉冲信号F₁₃改变转向舵机摇臂31的偏转角度，进而操纵运动载体10转向。

在本发明中，转向舵机控制脉冲信号F₁₃是周期为19.2ms的方波。

(3)左超声波触发脉冲生成单元104

左超声波触发脉冲生成单元104用于产生左超声波测距脉冲信号F₁₄，该左超声波测距脉冲信号F₁₄是一个脉冲宽度为10μs的单束脉冲。

在本发明中，左超声波收发器4收到左超声波测距脉冲信号F₁₄后发射出超声波，当左超声波收发器4收到回波时，则向左超声波距离获取单元106输出左距离脉冲信号F₄₁。

(4)左超声波距离获取单元106

左超声波距离获取单元106第一方面接收左距离脉冲信号F₄₁；第二方面对左距离脉冲信号F₄₁进行脉冲宽度测量得到左距离脉冲宽度W_F41；第三方面依据脉冲宽度与到障碍面距离的关系

得到左距离值m；V_voice表示声速；L_左表示左舵机摇臂5B左舵机摇臂5B的长度。

(5)右超声波触发脉冲生成单元105

右超声波触发脉冲生成单元105用于产生右超声波测距脉冲信号F₁₇，该右超声波测距脉冲信号F₁₇是一个脉冲宽度为10μs的单束脉冲。

在本发明中，右超声波收发器7收到右超声波测距脉冲信号F₁₇后发射出超声波，当右超声波收发器7收到回波时，则向右超声波距离获取单元107输出右距离脉冲信号F₇₁。

(6)右超声波距离获取单元107

右超声波距离获取单元107第一方面接收右距离脉冲信号F₇₁；第二方面对右距离脉冲信号F₇₁进行脉冲宽度测量得到右距离脉冲宽度W_F71；第三方面依据脉冲宽度与到障碍面距离的关系

得到右距离值n；V_voice表示声速；L_右表示右舵机摇臂6B的长度。

(7)超声波测距指向解析单元110

超声波测距指向解析单元110第一方面用于产生左扫描舵机脉冲宽度变量F₆₁，该左扫描舵机脉冲宽度变量F₆₁用于调节左舵机控制脉冲信号F₁₆的脉冲宽度W_F16(简称为左舵机脉冲宽度W_F16)，该左舵机脉冲宽度W_F16与左扫描舵机脉冲宽度变量F₆₁之间满足

关系，f_CLK表示DSP数字信号处理器1片上定时器工作的时钟频率；

超声波测距指向解析单元110第二方面依据左舵机脉冲宽度W_F16与左舵机摇臂5B的偏转角度α之间的关系

得到左舵机摇臂5B的偏转角度α(简称为左偏转角度α)；

超声波测距指向解析单元110第三方面用于产生右扫描舵机脉冲宽度变量F₉₁，该右扫描舵机脉冲宽度变量F₉₁用于控制右舵机脉冲信号F₁₉的脉冲宽度W_F19(简称为右舵机脉冲宽度W_F19)；该右舵机脉冲宽度W_F19与右扫描舵机脉冲宽度变量F₉₁之间满足 $W_{F19}=\frac{F_{91}}{f_{\mathrm{CLK}}}$ 关系。

超声波测距指向解析单元110第四方面依据右舵机脉冲宽度W_F19与右舵机摇臂6B的偏转角度β之间的关系

得到右舵机摇臂6B的偏转角度β(简称为右偏转角度β)；

在本发明中，左、右舵机控制脉冲信号F₁₆、F₁₉的脉冲宽度W_F16、W_F19的单位为秒；左、右偏转角度α、β的单位为度。

在本发明中，通过改变输出的左舵机控制脉冲信号F₁₆的脉冲宽度W_F16能够改变左舵机摇臂5B的偏转角度α；通过改变输出的右舵机控制脉冲信号F₁₉的脉冲宽度W_F19能够改变右舵机摇臂6B的偏转角度β；利用改变偏转角度α对固联在左舵机摇臂5B上的左超声波收发器4的指向进行实时可控调节；利用改变偏转角度β对固联在右舵机摇臂6B上的右超声波收发器7的指向进行实时可控调节。

在本发明中，超声波测距指向解析单元110连续地增加或减小左舵机脉冲宽度W_F16和右舵机脉冲宽度W_F19，左偏转角度α和右偏转角度β将连续改变，从而实现两个超声波收发器的扫描测距。当超声波测距指向解析单元110不改变左舵机脉冲宽度W_F16和右舵机脉冲宽度W_F19，则左偏转角度α和右偏转角度β将固定不变，从而实现两个超声波收发器的定向测距。

(8)左扫描舵机控制脉冲生成单元108

左扫描舵机控制脉冲生成单元108用于生成控制左扫描舵机5转动的脉冲信号F₁₆(简称为左舵机控制脉冲信号F₁₆)。

左舵机控制脉冲信号F₁₆是周期为19.2ms的方波。

(9)右扫描舵机控制脉冲生成单元109

右扫描舵机控制脉冲生成单元109用于生成控制右扫描舵机6转动的脉冲信号F₁₉(简称为右舵机控制脉冲信号F₁₉)。

右舵机控制脉冲信号F₁₉是周期为19.2ms的方波。

(10)当前位置坐标解算单元101(参见图3所示)

在运动载体10的坐标系o_b-x_by_bz_b下，当运动载体10完成姿态初始化后x_b轴与左侧障碍面平齐，定义此时的位置为初始位置，并以运动载体10的几何中心点建立了导航坐标系O-x_oy_oz_o。

在初始位置时，左偏转角度记为α₀，右偏转角度记为β₀，左距离值记为m₀，右距离值记为n₀；

当运动载体10运动一个航向角θ后，依据运动载体在导航坐标系O-x_oy_oz_o下的位置更新矩阵

$\left[\begin{array}{c}X\\ Y\end{array}\right]=\left[\begin{array}{c}\frac{[n_0-n-(\frac{\mathrm{CA}}{2}\sin \mathrm{\β}+\frac{\mathrm{CB}}{2}\cos \mathrm{\β})+\frac{\mathrm{CA}}{2}\sin {\mathrm{\β}}_0]}{\cos {\mathrm{\β}}_0}+[(m_0+\frac{\mathrm{CA}}{2})-m-(\frac{\mathrm{CB}}{2}\sin \mathrm{\θ}+\frac{\mathrm{CA}}{2}\cos \mathrm{\θ})]\·\tan {\mathrm{\β}}_0+\frac{\mathrm{CB}}{2}\\ (m_0+\frac{\mathrm{CA}}{2})-m-(\frac{\mathrm{CB}}{2}\sin \mathrm{\θ}+\frac{\mathrm{CA}}{2}\cos \mathrm{\θ})\end{array}\right]$

得到运动载体10的当前位置坐标 $\left[\begin{array}{c}X\\ Y\end{array}\right];$ 其中，CA表示运动载体10的横向的几何尺寸，CB表示运动载体10的纵向的几何尺寸。

在本发明中，在已知运动载体相对于初始位置的偏转角度θ、左右超声波测距指向[α β]^T和到障碍面的左右距离[m n]^T后，通过上述运动载体10在导航坐标系O-x_oy_oz_o下的位置更新矩阵便能够计算出运动载体10的当前位置坐标，从而完成对运动载体10的定位。

参见图4所示，采用本发明室内自主定位与定向的二维导航系统完成一次室内定位与定向的具体工作流程如下：

a)当运动载体10进入室内使用环境后，首先完成系统初始化，所有舵机(转向舵机3、左扫描舵机5、右扫描舵机6)复位；

b)通过超声波测距指向解析单元110连续改变左扫描舵机脉冲宽度变量F₆₁，并由左扫描舵机控制脉冲生成单元108产生具有不同左舵机脉冲宽度W_F16的左舵机控制脉冲信号F₁₆，该左舵机控制脉冲信号F₁₆用于驱动左扫描舵机5扫描；同时左超声波触发脉冲生成单元104产生左超声波测距脉冲信号F₁₄，左侧扫描测距单元通过扫描测距搜索左侧障碍面；当左超声波收发器4收到回波时，超声波测距指向解析单元110不再左扫描舵机脉冲宽度变量F₆₁，并计算此时的左偏转角度α；当左偏转角度α不等于90度时，启动转向舵机控制脉冲生成单元103调整转向舵机3的偏转角度，从而控制运动载体10偏转；

c)重复b)步骤，直至夹角α＝90°结束循环。此时完成了运动载体10的姿态初始化，其

轴与左侧障碍面的平齐；定义此时的位置为初始位置，并以此时运动载体10的几何中心点建立了导航坐标系O-x_oy_oz_o，同时初始化运动载体10的航向角变量和位置坐标变量，即[θXY]^T＝[0 0 0]^T；

d)当运动载体10运动后，MEMS陀螺数据读取单元102读取MEMS陀螺传感器绕

轴转动的角速率分量ω_bz；然后积分载体相对于其初始位置(导航坐标系)的偏转角度，即求得航向角θ；

e)超声波测距指向解析单元110连续改变左扫描舵机脉冲宽度变量F₆₁，并由左扫描舵机控制脉冲生成单元108产生产生具有不同左舵机脉冲宽度W_F16的左舵机控制脉冲信号F₁₆，该左舵机控制脉冲信号F₁₆用于驱动左扫描舵机5扫描；同时左超声波触发脉冲生成单元104产生左超声波测距脉冲信号F₁₄，左侧扫描测距单元通过扫描测距搜索左侧障碍面；当左超声波收发器4收到回波时，超声波测距指向解析单元110不再左扫描舵机脉冲宽度变量F₆₁，并计算此时的左偏转角度α；

超声波测距指向解析单元110连续改变右扫描舵机脉冲宽度变量F₉₁，并由右扫描舵机控制脉冲生成单元109产生产生具有不同右舵机脉冲宽度W_F19的右舵机控制脉冲信号F₁₉，该右舵机控制脉冲信号F₁₉用于驱动右扫描舵机6扫描；同时右超声波触发脉冲生成单元105产生右超声波测距脉冲信号F₁₇，右侧扫描测距单元通过扫描测距搜索右上侧障碍面；当右超声波收发器7收到回波时，超声波测距指向解析单元110不再右扫描舵机脉冲宽度变量F₉₁，并计算此时的右偏转角度β；

f)左超声波距离获取单元106依据脉冲宽度与到障碍面距离的关系

得到左距离值m；右超声波距离获取单元107依据脉冲宽度与到障碍面距离的关系得到右距离值n；

g)当前位置坐标解算单元101在获得载体相对于初始位置的偏转角度θ、左右超声波测距指向[α β]^T和到障碍面的左右距离[m n]^T后，利用位置更新矩阵计算并更新载体在导航坐标系下的位置坐标[X Y]^T，从而完成一次自主定位和定向导航。

Claims (9)

1.一种室内自主定位与定向的二维导航系统，其特征在于：该二维导航系统包括有DSP数字信号处理器（1）、MEMS陀螺传感器（2）、转向舵机（3）、左超声波收发器（4）、右超声波收发器（7）、左扫描舵机（5）和右扫描舵机（6）；其中：左扫描舵机（5）包括有左舵机本体（5A）、左舵机摇臂（5B）和左舵机转轴（5C）；左舵机摇臂（5B）为T形摇臂，左舵机摇臂（5B）的一端绕左舵机转轴（5C）旋转，左舵机摇臂（5B）安装在左舵机本体（5A）上；其中：右扫描舵机（6）包括有右舵机本体（6A）、右舵机摇臂（6B）和右舵机转轴（6C）；右舵机摇臂（6B）为T形摇臂，右舵机摇臂（6B）的一端绕右舵机转轴（6C）旋转，右舵机摇臂（6B）安装在右舵机本体（6A）上；

所述的DSP数字信号处理器（1）安装在运动载体（10）上；

所述的MEMS陀螺传感器（2）安装在运动载体（10）上，且安装方位需要保证MEMS陀螺传感器（2）的三轴与运动载体（10）的坐标系o_b-x_by_bz_b的三轴分别重合；MEMS陀螺传感器（2）用于测量运动载体（10）在运动状态下的三轴角速率信息F₂₁(ω_bx,ω_by,ω_bz)，ω_bx表示在坐标系o_b-x_by_bz_b下绕x_b轴转动的角速率分量，ω_by表示在坐标系o_b-x_by_bz_b下绕y_b轴转动的角速率分量，ω_bz表示在坐标系o_b-x_by_bz_b下绕z_b轴转动的角速率分量；

所述的转向舵机（3）安装在运动载体（10）的前端，转向舵机摇臂（31）与运动载体（10）的转向机构相连；转向舵机（3）用于操纵运动载体（10）实现转向；

所述的左超声波收发器（4）安装在左舵机摇臂（5B）上，且保证左超声波收发器（4）所发超声波波束的方向与左舵机摇臂（5B）的指向相同；左超声波收发器（4）根据收到的左超声波测距脉冲信号F₁₄后发射出超声波，然后当左超声波收发器（4）收到回波后，则输出左距离脉冲信号F₄₁给DSP数字信号处理器（1）；

所述的右超声波收发器（7）安装在右舵机摇臂（6B）上，且保证右超声波收发器（7）所发超声波波束的方向与右舵机摇臂（6B）的指向相同；右超声波收发器（7）根据收到的右超声波测距脉冲信号F₁₇后发射出超声波，然后当右超声波收发器（7）收到回波后，则输出右距离脉冲信号F₇₁给DSP数字信号处理器（1）。

2.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：左超声波收发器（4）和左扫描舵机（5）组成左侧扫描测距单元；右超声波收发器（7）和右扫描舵机（6）组成右侧扫描测距单元。

3.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：DSP数字信号处理器（1）选用AnalogDevices公司生产的ADSP-BF537处理器；MEMS陀螺传感器（2）选用InvenSense公司生产的ITG-3200陀螺仪；转向舵机（3）选用辉盛公司的Tower Pro9805BB舵机，其舵机摇臂选取原装的53.3mm直型摇臂；左扫描舵机（5）选用ESKY公司的EK2—0508型数字舵机，其舵机摇臂选取原装的T形摇臂；右扫描舵机（6）选用ESKY公司的EK2—0508型数字舵机，其舵机摇臂选取原装的T形摇臂；左超声波收发器（4）选用上海芯源（中国）电子有限公司生产的SRF05超声波传感器，其探测范围1cm～4m，精度可达3mm；右超声波收发器（7）选用上海芯源（中国）电子有限公司生产的SRF05超声波传感器，其探测范围1cm～4m，精度可达3mm。

4.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：DSP数字信号处理器（1）根据实现的功能包括有当前位置坐标解算单元（101）、陀螺信息读取单元（102）、转向舵机控制脉冲生成单元（103）、左超声波触发脉冲生成单元（104）、左超声波距离获取单元（106）、右超声波触发脉冲生成单元（105）、右超声波距离获取单元（107）、左扫描舵机控制脉冲生成单元（108）、右扫描舵机控制脉冲生成单元（109）、超声波测距指向解析单元（110）；

陀螺信息读取单元（102）第一方面用于读取MEMS陀螺传感器（2）测量得到的运动载体（10）在运动状态下的三轴角速率信息F₂₁(ω_bx,_by,ω_bz)；第二方面对三轴角速率信息F₂₁(ω_bx,ω_by,ω_bz)中的绕z_b轴转动的角速率分量ω_bz进行初始时刻至当前时刻角速率分量的积分处理t₀表示初始时刻，t表示当前时刻，得到运动载体（10）相对于导航坐标系O-x_oy_oz_o的偏转角度θ；

转向舵机控制脉冲生成单元（103）第一方面用于产生控制转向舵机脉冲信号F₁₃；第二方面以转向舵机脉冲宽度变量F₃₁控制转向舵机脉冲宽度W_F13，且

f_CLK表示DSP数字信号处理器（1）片上定时器工作的时钟频率；转向舵机（3）依据接收到的具有不同脉冲宽度W_F13的转向舵机脉冲信号F₁₃改变转向舵机摇臂（31）的偏转角度，进而操纵运动载体（10）转向；左超声波触发脉冲生成单元（104）用于产生左超声波测距脉冲信号F₁₄；

左超声波距离获取单元（106）第一方面接收左距离脉冲信号F₄₁；第二方面对左距离脉冲信号F₄₁进行脉冲宽度测量得到左距离脉冲宽度W_F41；第三方面依据脉冲宽度与到障碍面距离的关系

得到左距离值m；V_voice表示声速；L_左表示左舵机摇臂（5B）的长度；

右超声波触发脉冲生成单元（105）用于产生右超声波测距脉冲信号F₁₇；

右超声波距离获取单元（107）第一方面接收右距离脉冲信号F₇₁；第二方面对右距离脉冲信号F₇₁进行脉冲宽度测量得到右距离脉冲宽度W_F71；第三方面依据脉冲宽度与到障碍面距离的关系得到右距离值n；V_voice表示声速；L_右表示右舵机摇臂（6B）的长度；

超声波测距指向解析单元（110）第一方面用于产生左扫描舵机脉冲宽度变量F₆₁，该左扫描舵机脉冲宽度变量F₆₁用于调节左舵机脉冲宽度W_F16，该左舵机脉冲宽度W_F16与左扫描舵机脉冲宽度变量F₆₁之间满足关系，f_CLK表示DSP数字信号处理器（1）片上定时器工作的时钟频率；

超声波测距指向解析单元（110）第二方面依据左舵机脉冲宽度E_F16与左舵机摇臂（5B）的偏转角度α之间的关系

得到左偏转角度α；

超声波测距指向解析单元（110）第三方面用于产生右扫描舵机脉冲宽度变量F₉₁，该右扫描舵机脉冲宽度变量F₉₁用于控制右舵机脉冲宽度W_F19；该右舵机脉冲宽度W_F19与右扫描舵机脉冲宽度变量F₉₁之间满足关系；

超声波测距指向解析单元（110）第四方面依据右舵机脉冲宽度W_F19与右舵机摇臂（6B）的偏转角度β之间的关系

得到右偏转角度β；

左扫描舵机控制脉冲生成单元（108）用于生成左舵机控制脉冲信号F₁₆；

右扫描舵机控制脉冲生成单元（109）用于生成右舵机控制脉冲信号F₁₉；

在运动载体（10）的坐标系o_b-x_by_bz_b下，当运动载体（10）完成姿态初始化后x_b轴与左侧障碍面平齐，定义此时的位置为初始位置，并以运动载体（10）的几何中心点建立了导航坐标系O-x_oy_oz_o；

在初始位置时，左偏转角度记为α₀，右偏转角度记为β₀，左距离值记为m₀，右距离值记为n₀；

当运动载体（10）运动一个航向角θ后，依据运动载体在导航坐标系O-x_oy_oz_o下的位置更新矩阵

$\left[\begin{array}{c}X\\ Y\end{array}\right]=\left[\begin{array}{c}\frac{[n_0-n-(\frac{\mathrm{CA}}{2}\sin \mathrm{\β}+\frac{\mathrm{CB}}{2}\cos \mathrm{\β})+\frac{\mathrm{CA}}{2}\sin {\mathrm{\β}}_0]}{\cos {\mathrm{\β}}_0}+[(m_0+\frac{\mathrm{CA}}{2})-m-(\frac{\mathrm{CB}}{2}\sin \mathrm{\θ}+\frac{\mathrm{CA}}{2}\cos \mathrm{\θ})]\·\tan {\mathrm{\β}}_0+\frac{\mathrm{CB}}{2}\\ (m_0+\frac{\mathrm{CA}}{2})-m-(\frac{\mathrm{CB}}{2}\sin \mathrm{\θ}+\frac{\mathrm{CA}}{2}\cos \mathrm{\θ})\end{array}\right]$

得到运动载体（10）的当前位置坐标

其中，CA表示运动载体（10）的横向的几何尺寸，CB表示运动载体（10）的纵向的几何尺寸。

5.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：通过改变输出的左舵机控制脉冲信号F₁₆的脉冲宽度W_F16能够改变左偏转角度α；通过改变输出的右舵机控制脉冲信号F₁₉的脉冲宽度W_F19能够改变右偏转角度β；利用改变左偏转角度α对固联在左舵机摇臂5B上的左超声波收发器（4）的指向进行实时可控调节；利用改变右偏转角度β对固联在右舵机摇臂（6B）上的右超声波收发器（7）的指向进行实时可控调节。

6.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：超声波测距指向解析单元（110）连续地增加或减小左舵机脉冲宽度W_F16和右舵机脉冲宽度W_F19，左偏转角度α和右偏转角度β将连续改变，从而实现两个超声波收发器的扫描测距；当超声波测距指向解析单元（110）不改变左舵机脉冲宽度W_F16和右舵机脉冲宽度W_F19，则左偏转角度α和右偏转角度β将固定不变，从而实现两个超声波收发器的定向测距。

7.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：转向舵机控制脉冲信号F₁₃是周期为19.2ms的方波。

8.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：左舵机控制脉冲信号F₁₆是周期为19.2ms的方波；右舵机控制脉冲信号F₁₉是周期为19.2ms的方波。

9.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于：左超声波测距脉冲信号F₁₄是一个脉冲宽度为10μs的单束脉冲；右超声波测距脉冲信号F₁₇是一个脉冲宽度为10μs的单束脉冲。

Images