专利名称：一种室内自主定位与定向的二维导航系统的制作方法
技术领域：
本发明涉及一种对运动载体进行室内自主定位与定向的二维导航系统。
背景技术：
对运动载体的定位包括室外环境的定位和室内环境的定位。目前主要针对室外环境进行定位的系统有采用蜂窝网络技术定位的系统和GPS全球定位系统。GPS全球定位系统目前能提供的室外定位精度约为:3m 5m，蜂窝网络定位系统的精度远差于此。这些定位系统受到建筑物对信号的遮挡、反射和衍射等影响，在室内环境下使用往往存在更大的定位误差，不能满足用户对室内定位的精度需求。室内定位系统与室外定位系统不同，用户更关心的是自己在室内平面的二维坐标和运动的航向信息。由于室内环境相对较小，用户对定位精度的要求也比室外环境更高。 目前，室内定位技术在室内移动探测器、救灾机器人车、仓库搬运机器人车的跟踪和运动导航，以及汽车防撞检测、自动泊车等领域有广泛的应用需求。根据导航定位使用的设备仪器不同，目前室内定位的方式主要有电磁导航、无线电导航、无线网络导航、激光导航、视觉导航、超声波导航和多种传感器融合导航等。这些导航方式主要的优缺点对比如下电磁导航是让低频电流流过埋设在路径下的电缆，然后用安装在机器人上的线圈检测电缆周围产生的磁场，并根据该信息计算位置，并控制方向，以实现沿规定路径的导航控制。主要优点是导引线隐蔽，不易污染和破损，导引原理简单，便于控制和通信，对声光无干扰；缺点是灵活性差，改变或扩充路径麻烦，对导引线附近的铁磁物质有干扰，电缆铺设工作量大，维护困难。无线电导航是通过利用移动通信网络来实施定位导航的。具体有利用基站发射信号的到达时间差技术(TDOA)、信号到达角技术(AOA)和信号衰减技术来确定用户与基站之间的距离，利用球面三角算出移动终端的位置。主要优点是可以利用现有的移动通信网络的基站和用户手持设备完成定位服务，无需额外开发新的设备；缺点是信号受建筑物和周围环境的影响，定位精度不高，一般在50米以上，通常只用于粗定位。无线网络导航技术，利用Wi-Fi、蓝牙(Bluetooth)等室内无线网络，通过用户设备接收到的室内无线信号的强弱(received signal strength, RSS)测定信号点与接收点的距离，进而进行定位计算的一种定位技术。无线网络目前已经开始广泛普及，尤其是在医院、学校、办公大楼等地区，使其用于室内定位成为可能，并且实施成本并不高。但是这种定位方式需要首先为使用环境建立一个精确的无线网络传播模型，当室内环境改变时，会影响无线信号强度的发布，需要重新修改模型；目前单独是使用无线网络实施室内定位的精度最高只能达到米级。激光导航的一种方案是用一个指向标发出旋转扫描激光光束，再利用运动载体上的若干个传感器来检测，从而求出指向标的方向和到指向标的距离，用以确定运动载体的位置；另外还有和导向电缆方式类似的采用激光导引运动载体的方法。激光导航的优点是传感器发散小或没有发散，并对大多数物体无镜面反射现象；缺点是存在潜在的安全问题(首先是人眼安全问题)，且不适用于透明物质。视觉导航成为近几年室内导航研究的热点，运动载体利用视觉传感器获取的信息进行定位，从而实现智能行驶。视觉导航中一般使用的方法如基于像素匹配的方法识别特征点、基于光流场分析的方法估计环境结构等等。视觉导航方法的优点是获取信息量大，灵敏度高，成本低，并且可根据需要灵活地改变或扩充路径，具有很好的柔性；缺点是对环境光线有一定要求，并且由于计算复杂对导航的实时性有一定影响。超声波测距作为一种常用的非接触式测距方式，长期以来一直是室内导航定位研究的热点。常用的超声波定位方法有反射式和超声信标式。反射式将超声发射和接收做为一体，使用超声波收发器发出和接收信号，完成距离的测量。在目前的应用中，主要采用超声波收发器与载体固定连接的方法。这种方法最主要的缺点是只能获得超声波收发器到与之垂直的障碍面的距离，测量角度范围小(一般士5° )且难以控制，当载体姿态发生改变导致与之固联的超声波收发器的测量角度范围离开了障碍面时，将不能收到回波，无法完成测量。超声信标式，如L. Kleeman使用主动超声作为信标，在运动载体身上装有超声波接收器，测得运动载体和信标的距离及方位从而估计运动载体的位姿。这种方法扩大了超声波定位的使用范围，但需在室内预先铺设很多的超声波信标，且超声波接收器很难区分不同信标发出的信号。综合上述两种方法，超声波导航的优点是硬件结构简单，价格低廉，容易操作；缺点是速度慢，传感器存在较大的波束角(角度分辨率低)，单一传感器的稳定性不理想等。 在实际应用中，往往采用其它传感器来补偿，或采用多传感器融合技术提高检测精度等。多传感器融合导航组合了多种导航方式。已有的室内多传感器融合导航系统往往融合了无线网络导航、激光导航、超声波导航等，如AT&T Lab Cambridge的Active badge 和Bat室内定位系统，MIT的Cricket系统。多传感器融合导航系统弥补了的单一传感器的许多固有缺点，提高了定位精度，但是系统复杂程度增加，运行和维护成本更高。
发明内容
本发明的目的是提供一种室内自主定位与定向的二维导航系统，该二维导航系统能够获取运动载体相对于其初始位置的偏转角度(定向)和位置坐标(定位)，满足在相对简洁的室内使用环境下对运动载体实施自主定位与定向的需求。在本发明中二维导航系统由MEMS陀螺传感器、DSP数字信号处理器、数字舵机和超声波收发器等部件组成。本发明的二维导航系统安装在运动载体上，一方面DSP数字信号处理器处理MEMS陀螺传感器的测量数据，获得运动载体相对于其初始位置的偏转角度 (即航向信息)；另一方面DSP数字信号处理器通过调节数字舵机摇臂的偏转角度(即扫描角度)，对载体两侧固联在舵机摇臂上的超声波收发器的对准方位实施可控调节，实现扫描测距和定向测距两种测距模式，测得载体到障碍面的垂直距离，并利用收到回波时处理器内控制数字舵机偏转角度的变量换算出此时对应的测距指向角度；最后根据位置更新矩阵更新当前载体相对于其初始位置的位置坐标，对运动载体实施室内自主定位与定向，具有实用意义。本发明的一种室内自主定位与定向的二维导航系统，该二维导航系统包括有DSP数字信号处理器(1)、MEMS陀螺传感器( 、转向舵机C3)、左超声波收发器(4)、右超声波收发器(7)、左扫描舵机( 和右扫描舵机(6)；其中左扫描舵机( 包括有左舵机本体 (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 陀螺传感器O)的三轴与运动载体(10)的坐标系0b-、yb、的三轴分别重合；MEMS陀螺传感器⑵用于测量运动载体(10)在运动状态下的三轴角速率信息F21(cobx，coby，cobz)，cobx 表示在坐标系ob-xbyb、下绕、轴转动的角速率分量，"by表示在坐标系ob-Xby#b下绕yb轴转动的角速率分量，"bz表示在坐标系obibyb、下绕、轴转动的角速率分量；所述的转向舵机C3)安装在运动载体10的前端，转向舵机摇臂31与运动载体 (10)的转向机构相连；转向舵机3用于操纵运动载体(10)实现转向；所述的左超声波收发器(4)安装在左舵机摇臂(5B)上，且保证左超声波收发器 (4)所发超声波波束的方向与左舵机摇臂(5B)的指向相同；左超声波收发器(4)根据收到的左超声波测距脉冲信号F14后发射出超声波，然后当左超声波收发器(4)收到回波后，则输出左距离脉冲信号F41给DSP数字信号处理器(1)；所述的右超声波收发器(7)安装在右舵机摇臂(6B)上，且保证右超声波收发器 (7)所发超声波波束的方向与右舵机摇臂(6B)的指向相同；右超声波收发器(7)根据收到的右超声波测距脉冲信号F17后发射出超声波，然后当右超声波收发器(7)收到回波后，则输出右距离脉冲信号F71给DSP数字信号处理器(1)。本发明的一种室内自主定位与定向的二维导航系统的优点在于①本发明中，超声波收发器的指向受到DSP数字信号处理器的可控调节，能够实现扫描测距和定向测距两种测距模式扫描测距模式在数字舵机摇臂旋转扫描过程中，超声波收发器检测是否收到回波，当收到回波时数字舵机摇臂停止旋转，进入定向测距模式。 使用这种方法可以保证不管载体如何运动，超声波收发器总能自动修改指向寻找障碍面， 始终保持与障碍面的垂直，始终能完成测距，有效地克服了传统的与载体固定连接的反射式测距方法的不足。②本发明中，组合使用了 MEMS惯性传感器确定载体运动方向。DSP数字信号处理器通过处理MEMS陀螺传感器的测量数据，能获得运动载体相对于其初始位置的偏转角度， 在辅助完成定位功能的同时还给用户提供了载体的航向信息。③本发明的一种室内自主定位与定向的二维导航系统已在遥控电动车自动倒车入库的应用中通过了室内试验验证。定位与定向精度高，方法正确可行，易于工程实现。。④本发明的二维导航系统能够应用于移动探测器、移动机器人车、救灾机器人车的室内运动导航，以及在汽车防撞检测、自动泊车等领域都具有广泛实用前景。
图1是本发明室内自主定位与定向的二图IA是本发明室内自主定位与定向的二图2是本发明室内自主定位与定向的二图3是本发明室内自主定位与定向的二图4是本发明室内自主定位与定向的二1.DSP数学信号处理器102.陀螺信息读取单元104.左超声波触发脉冲生成单元106.左超声波距离获取单元108.左扫描舵机控制脉冲生成单元110.超声波测距指向解析单元3.转向舵机4.5A.左舵机本体5B.7.右超声波收发器6.6B.右舵机摇臂6C.
维导航系统的硬件结构图。 二维导航系统的另一视角硬件结构图。 维导航系统中DSP处理器的功能框图维导航系统中定位解算的情景示意图维导航系统的工作流程框图。 101.当前位置坐标解算单元 103.转向舵机控制脉冲生成单元 105.右超声波触发脉冲生成单元 107.右超声波距离获取单元 109.右扫描舵机控制脉冲生成单元 2. MEMS陀螺传感器左超声波收发器左舵机摇臂右扫描舵机右舵机转轴
5.左扫描舵机 5C.左舵机转轴 6A.右舵机本体 10.运动载体
具体实施例方式下面将结合附图对本发明做进一步的详细说明。参见图1、图IA所示，本发明的一种室内自主定位与定向的二维导航系统，其组成包括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的坐标系记为ob_、yb、，也称为本体坐标系ob_、yb、；坐标原点Ob保持与运动载体10的几何中心重合，Ob-Xb7bZb坐标系的、轴沿运动载体的纵向轴向前，Ob-Xb7bZb坐标系的yb轴沿运动载体的横向轴向左，Ob-Xb7bZb坐标系的Ib轴垂直于运动载体向上，与、、yb构成右手坐标系。所述的DSP数字信号处理器1安装在运动载体10上。所述的MEMS陀螺传感器2安装在运动载体10上，且安装方位需要保证MEMS陀螺传感器2的三轴与运动载体10的坐标系ob_、yb、的三轴分别重合。MEMS陀螺传感器2用于测量运动载体10在运动状态下的三轴角速率信息F21(cobx，coby，cobz)，cobx表示在坐标系 Ob-XbYbZb下绕、轴转动的角速率分量，ω by表示在坐标系0b-、yb、下绕yb轴转动的角速率分量，cobz表示在坐标系ob-、yb、下绕、轴转动的角速率分量。所述的转向舵机3安装在运动载体10的前端，转向舵机摇臂31与运动载体10的转向机构相连；转向舵机3用于操纵运动载体10实现转向。所述的左超声波收发器4安装在左舵机摇臂5B上，且保证左超声波收发器4所发超声波波束的方向与左舵机摇臂5B的指向相同。左超声波收发器4根据收到的左超声波测距脉冲信号F14后发射出超声波，然后当左超声波收发器4收到回波后，则输出左距离脉冲信号I741给DSP数字信号处理器1。所述的右超声波收发器7安装在右舵机摇臂6B上，且保证右超声波收发器7所发超声波波束的方向与右舵机摇臂6B的指向相同。右超声波收发器7根据收到的右超声波测距脉冲信号F17后发射出超声波，然后当右超声波收发器7收到回波后，则输出右距离脉冲信号F71给DSP数字信号处理器1。所述的左舵机本体5A安装在运动载体10上，其安装方位需要保证在左舵机本体 5A复位时(上电初始状态)左舵机摇臂5B的指向沿着运动载体10的纵向轴(即ob-xbyb、 坐标系的、轴)向前，且以后左舵机摇臂5B能向左旋转不受阻碍。为了便于定位解算，左舵机本体5A安装时同时保证左舵机转轴5C垂直于运动载体10的左上顶点AA。所述的右舵机本体6A安装在运动载体10上，其安装方位需要保证在右舵机本体 6A复位时(上电初始状态)右舵机摇臂6B的指向沿着运动载体10的纵向轴(ob-Xbyb、坐标系的&轴)向前，且以后右舵机摇臂6B能向右旋转不受阻碍。为了便于定位解算，右舵机本体6A安装时同时保证右舵机转轴6C垂直于运动载体10的右上顶点BB。 在本发明中，左超声波收发器4和左扫描舵机5组成左侧扫描测距单元；右超声波收发器7和右扫描舵机6组成右侧扫描测距单元(参见图2所示)。本发明中，DSP数字信号处理器1选用AnalogDevices公司生产的ADSP-BF537处理器。MEMS陀螺传感器2选用hvenSense公司生产的ITG-3200陀螺仪。转向舵机3选用辉盛公司的Tower Pro 9805BB舵机，其舵机摇臂31选取原装的53. 3mm直型摇臂。左扫描舵机5选用ESKY公司的EK2-0508型数字舵机，其左舵机摇臂5B选取原装的T形摇臂。 右扫描舵机6选用ESKY公司的EK2-0508型数字舵机，其右舵机摇臂6B选取原装的T形摇臂。左超声波收发器4选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器， 其探测范围Icm 細，精度可达3mm。右超声波收发器7选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器，其探测范围Icm 細，精度可达3mm。参见图2所示，用于实现本发明室内自主定位与定向的二维导航系统的DSP数字信号处理器1，该DSP数字信号处理器1根据实现的功能包括有下列单元当前位置坐标解算单元101、陀螺信息读取单元102、转向舵机控制脉冲生成单元103、左超声波触发脉冲生成单元104、左超声波距离获取单元106、右超声波触发脉冲生成单元105、右超声波距离获取单元107、左扫描舵机控制脉冲生成单元108、右扫描舵机控制脉冲生成单元109、超声波测距指向解析单元110。下面将详细说明在DSP数字信号处理器1上各个单元实现的功能(1)陀螺信息读取单元102陀螺信息读取单元102第一方面用于读取MEMS陀螺传感器2测量得到的运动载体10在运动状态下的三轴角速率信息F21(G)bx, COby，CObz)；第二方面对三轴角速率信息 F21(cobx，coby，ω J中的绕、轴转动的角速率分量《bz进行初始时刻至当前时刻角速率分
量的积分处理0 = Γ ，t0表示初始时刻，t表示当前时刻，得到运动载体10相对于导航
nQ
坐标系0-x。y。z。的偏转角度θ (参见图3所示)。在本发明中，用于确定运动载体10位置的坐标系称为导航坐标系0-X。y。z。，导航坐标系0-X。y。z。的坐标原点0与初始位置时的运动载体10的几何中心重合，χ。沿初始位置时的运动载体10的纵向轴向前，y0沿初始位置时的运动载体10的横轴向左，Z0垂直运动载体10向上，与x。、y。构成右手坐标系。(2)转向舵机控制脉冲生成单元103转向舵机控制脉冲生成单元103第一方面用于产生控制转向舵机3的脉冲信号 F13 (简称为转向舵机脉冲信号F13)；第二方面以转向舵机脉冲宽度变量F31控制转向舵机脉
冲信号F13的脉冲宽度Wf13 (简称为转向舵机脉冲宽度WF13)，且
权利要求
1.一种室内自主定位与定向的二维导航系统，其特征在于该二维导航系统包括有 DSP数字信号处理器⑴、MEMS陀螺传感器(2)、转向舵机(3)、左超声波收发器⑷、右超声波收发器(7)、左扫描舵机( 和右扫描舵机(6)；其中左扫描舵机( 包括有左舵机本体 (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陀螺传感器O)的三轴与运动载体(10)的坐标系ob_、yb、的三轴分别重合；MEMS陀螺传感器 (2)用于测量运动载体(10)在运动状态下的三轴角速率信息F21(cobx，coby，cobz)，cobx表示在坐标系ob-xby#b下绕、轴转动的角速率分量，Ql3y表示在坐标系ob-Xby#b下绕yb轴转动的角速率分量，"bz表示在坐标系obibyb、下绕、轴转动的角速率分量；所述的转向舵机C3)安装在运动载体10的前端，转向舵机摇臂31与运动载体(10)的转向机构相连；转向舵机3用于操纵运动载体(10)实现转向；所述的左超声波收发器(4)安装在左舵机摇臂(5B)上，且保证左超声波收发器(4)所发超声波波束的方向与左舵机摇臂(5B)的指向相同；左超声波收发器(4)根据收到的左超声波测距脉冲信号F14后发射出超声波，然后当左超声波收发器(4)收到回波后，则输出左距离脉冲信号F41给DSP数字信号处理器(1)；所述的右超声波收发器(7)安装在右舵机摇臂(6B)上，且保证右超声波收发器(7)所发超声波波束的方向与右舵机摇臂(6B)的指向相同；右超声波收发器(7)根据收到的右超声波测距脉冲信号F17后发射出超声波，然后当右超声波收发器(7)收到回波后，则输出右距离脉冲信号F71给DSP数字信号处理器(1)。
2.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于左超声波收发器(4)和左扫描舵机( 组成左侧扫描测距单元；右超声波收发器(7)和右扫描舵机(6)组成右侧扫描测距单元。
3.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于DSP数字信号处理器(1)选用AnalogDevices公司生产的ADSP-BF537处理器；MEMS陀螺传感器(2) 选用hvenSense公司生产的ITG-3200陀螺仪；转向舵机(3)选用辉盛公司的Tower Pro 9805BB舵机，其舵机摇臂选取原装的53. 3mm直型摇臂；左扫描舵机(5)选用ESKY公司的 EK2-0508型数字舵机，其舵机摇臂选取原装的T形摇臂；右扫描舵机(6)选用ESKY公司的 EK2-0508型数字舵机，其舵机摇臂选取原装的T形摇臂；左超声波收发器(4)选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器，其探测范围Icm 細，精度可达3mm ； 右超声波收发器(7)选用上海芯源(中国)电子有限公司生产的SRF05超声波传感器，其探测范围Icm 鈿，精度可达3mm。
4.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于DSP数字信号处理器(1)根据实现的功能包括有当前位置坐标解算单元(101)、陀螺信息读取单元 (102)、转向舵机控制脉冲生成单元(103)、左超声波触发脉冲生成单元(104)、左超声波距离获取单元(106)、右超声波触发脉冲生成单元(105)、右超声波距离获取单元(107)、左扫描舵机控制脉冲生成单元(108)、右扫描舵机控制脉冲生成单元(109)、超声波测距指向解析单元(110)；陀螺信息读取单元(10 第一方面用于读取MEMS陀螺传感器( 测量得到的运动载体(10)在运动状态下的三轴角速率信息F21(cobx，coby，cobz)；第二方面对三轴角速率信息 F21(cobx，coby，ω J中的绕、轴转动的角速率分量《bz进行初始时刻至当前时刻角速率分量的积分处理
5.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于通过改变输出的左舵机控制脉冲信号Fni的脉冲宽度Wn6能够改变左偏转角度α ；通过改变输出的右舵机控制脉冲信号F19的脉冲宽度Wf19能够改变右偏转角度β ；利用改变左偏转角度α 对固联在左舵机摇臂5Β上的左超声波收发器(4)的指向进行实时可控调节；利用改变右偏转角度β对固联在右舵机摇臂(6Β)上的右超声波收发器(7)的指向进行实时可控调节。
6.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于超声波测距指向解析单元(110)连续地增加或减小左舵机脉冲宽度Wn6和右舵机脉冲宽度Wf19，左偏转角度α和右偏转角度β将连续改变，从而实现两个超声波收发器的扫描测距；当超声波测距指向解析单元(110)不改变左舵机脉冲宽度Wn6和右舵机脉冲宽度Wf19，则左偏转角度α和右偏转角度β将固定不变，从而实现两个超声波收发器的定向测距。
7.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于转向舵机控制脉冲信号F13是周期为19. 2ms的方波。
8.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于左舵机控制脉冲信号K6是周期为19. 2ms的方波；右舵机控制脉冲信号F19是周期为19. 2ms的方波。
9.根据权利要求1的一种室内自主定位与定向的二维导航系统，其特征在于左超声波测距脉冲信号F14是一个脉冲宽度为10 μ S的单束脉冲；右超声波测距脉冲信号F17是一个脉冲宽度为10 μ s的单束脉冲。
全文摘要
本发明公开了一种室内自主定位与定向的二维导航系统，该二维导航系统由MEMS陀螺传感器、DSP数字信号处理器、数字舵机和超声波收发器等部件组成。本发明的二维导航系统安装在运动载体上，DSP数字信号处理器一方面处理MEMS陀螺传感器的测量数据，获得运动载体相对于其初始位置的偏转角度。另一方面通过调节数字舵机摇臂的偏转角度，对分布在载体两侧、固联在舵机摇臂上的超声波收发器的对准方位实施可控调节，实现扫描测距和定向测距两种测距模式，测得载体到障碍面的垂直距离；并利用收到回波时处理器内控制数字舵机偏转角度的变量换算出此时对应的测距指向角度；最后根据位置更新矩阵更新当前载体相对于其初始位置的位置坐标，完成对运动载体的一次局部自主定位与定向。
文档编号G01C21/16GK102436260SQ20111025630
公开日2012年5月2日 申请日期2011年9月1日 优先权日2011年9月1日
发明者刘创, 舒磊正, 陈培, 韩潮 申请人:北京航空航天大学