CN102436260A - 一种室内自主定位与定向的二维导航*** - Google Patents
一种室内自主定位与定向的二维导航*** Download PDFInfo
- Publication number
- CN102436260A CN102436260A CN2011102563055A CN201110256305A CN102436260A CN 102436260 A CN102436260 A CN 102436260A CN 2011102563055 A CN2011102563055 A CN 2011102563055A CN 201110256305 A CN201110256305 A CN 201110256305A CN 102436260 A CN102436260 A CN 102436260A
- Authority
- CN
- China
- Prior art keywords
- rudder machine
- pulse width
- rocking arm
- steering wheel
- ultrasonic
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
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)的坐标系ob-xbybzb的三轴分别重合;MEMS陀螺传感器(2)用于测量运动载体(10)在运动状态下的三轴角速率信息F21(ωbx,ωby,ωbz),ωbx表示在坐标系ob-xbybzb下绕xb轴转动的角速率分量,ωby表示在坐标系ob-xbybzb下绕yb轴转动的角速率分量,ωbz表示在坐标系ob-xbybzb下绕zb轴转动的角速率分量;
所述的转向舵机(3)安装在运动载体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是本发明室内自主定位与定向的二维导航***的硬件结构图。
图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的坐标系记为ob-xbybzb,也称为本体坐标系ob-xbybzb;坐标原点ob保持与运动载体10的几何中心重合,ob-xbybzb坐标系的xb轴沿运动载体的纵向轴向前,ob-xbybzb坐标系的yb轴沿运动载体的横向轴向左,ob-xbybzb坐标系的zb轴垂直于运动载体向上,与xb、yb构成右手坐标系。
所述的DSP数字信号处理器1安装在运动载体10上。
所述的MEMS陀螺传感器2安装在运动载体10上,且安装方位需要保证MEMS陀螺传感器2的三轴与运动载体10的坐标系ob-xbybzb的三轴分别重合。MEMS陀螺传感器2用于测量运动载体10在运动状态下的三轴角速率信息F21(ωbx,ωby,ωbz),ωbx表示在坐标系ob-xbybzb下绕xb轴转动的角速率分量,ωby表示在坐标系ob-xbybzb下绕yb轴转动的角速率分量,ωbz表示在坐标系ob-xbybzb下绕zb轴转动的角速率分量。
所述的转向舵机3安装在运动载体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。
所述的左舵机本体5A安装在运动载体10上,其安装方位需要保证在左舵机本体5A复位时(上电初始状态)左舵机摇臂5B的指向沿着运动载体10的纵向轴(即ob-xbybzb坐标系的xb轴)向前,且以后左舵机摇臂5B能向左旋转不受阻碍。为了便于定位解算,左舵机本体5A安装时同时保证左舵机转轴5C垂直于运动载体10的左上顶点AA。
所述的右舵机本体6A安装在运动载体10上,其安装方位需要保证在右舵机本体6A复位时(上电初始状态)右舵机摇臂6B的指向沿着运动载体10的纵向轴(ob-xbybzb坐标系的xb轴)向前,且以后右舵机摇臂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在运动状态下的三轴角速率信息F21(ωbx,ωby,ωbz);第二方面对三轴角速率信息F21(ωbx,ωby,ωbz)中的绕zb轴转动的角速率分量ωbz进行初始时刻至当前时刻角速率分量的积分处理t0表示初始时刻,t表示当前时刻,得到运动载体10相对于导航坐标系O-xoyozo的偏转角度θ(参见图3所示)。
在本发明中,用于确定运动载体10位置的坐标系称为导航坐标系O-xoyozo,导航坐标系O-xoyozo的坐标原点O与初始位置时的运动载体10的几何中心重合,xo沿初始位置时的运动载体10的纵向轴向前,yo沿初始位置时的运动载体10的横轴向左,zo垂直运动载体10向上,与xo、yo构成右手坐标系。
(2)转向舵机控制脉冲生成单元103
转向舵机控制脉冲生成单元103第一方面用于产生控制转向舵机3的脉冲信号F13(简称为转向舵机脉冲信号F13);第二方面以转向舵机脉冲宽度变量F31控制转向舵机脉冲信号F13的脉冲宽度WF13(简称为转向舵机脉冲宽度WF13),且fCLK表示DSP数字信号处理器1片上定时器工作的时钟频率;转向舵机依据接收到的具有不同脉冲宽度WF13的转向舵机脉冲信号F13改变转向舵机摇臂31的偏转角度,进而操纵运动载体10转向。
在本发明中,转向舵机控制脉冲信号F13是周期为19.2ms的方波。
(3)左超声波触发脉冲生成单元104
左超声波触发脉冲生成单元104用于产生左超声波测距脉冲信号F14,该左超声波测距脉冲信号F14是一个脉冲宽度为10μs的单束脉冲。
在本发明中,左超声波收发器4收到左超声波测距脉冲信号F14后发射出超声波,当左超声波收发器4收到回波时,则向左超声波距离获取单元106输出左距离脉冲信号F41。
(4)左超声波距离获取单元106
左超声波距离获取单元106第一方面接收左距离脉冲信号F41;第二方面对左距离脉冲信号F41进行脉冲宽度测量得到左距离脉冲宽度WF41;第三方面依据脉冲宽度与到障碍面距离的关系得到左距离值m;Vvoice表示声速;L左表示左舵机摇臂5B左舵机摇臂5B的长度。
(5)右超声波触发脉冲生成单元105
右超声波触发脉冲生成单元105用于产生右超声波测距脉冲信号F17,该右超声波测距脉冲信号F17是一个脉冲宽度为10μs的单束脉冲。
在本发明中,右超声波收发器7收到右超声波测距脉冲信号F17后发射出超声波,当右超声波收发器7收到回波时,则向右超声波距离获取单元107输出右距离脉冲信号F71。
(6)右超声波距离获取单元107
右超声波距离获取单元107第一方面接收右距离脉冲信号F71;第二方面对右距离脉冲信号F71进行脉冲宽度测量得到右距离脉冲宽度WF71;第三方面依据脉冲宽度与到障碍面距离的关系得到右距离值n;Vvoice表示声速;L右表示右舵机摇臂6B的长度。
(7)超声波测距指向解析单元110
超声波测距指向解析单元110第一方面用于产生左扫描舵机脉冲宽度变量F61,该左扫描舵机脉冲宽度变量F61用于调节左舵机控制脉冲信号F16的脉冲宽度WF16(简称为左舵机脉冲宽度WF16),该左舵机脉冲宽度WF16与左扫描舵机脉冲宽度变量F61之间满足关系,fCLK表示DSP数字信号处理器1片上定时器工作的时钟频率;
超声波测距指向解析单元110第三方面用于产生右扫描舵机脉冲宽度变量F91,该右扫描舵机脉冲宽度变量F91用于控制右舵机脉冲信号F19的脉冲宽度WF19(简称为右舵机脉冲宽度WF19);该右舵机脉冲宽度WF19与右扫描舵机脉冲宽度变量F91之间满足 关系。
在本发明中,左、右舵机控制脉冲信号F16、F19的脉冲宽度WF16、WF19的单位为秒;左、右偏转角度α、β的单位为度。
在本发明中,通过改变输出的左舵机控制脉冲信号F16的脉冲宽度WF16能够改变左舵机摇臂5B的偏转角度α;通过改变输出的右舵机控制脉冲信号F19的脉冲宽度WF19能够改变右舵机摇臂6B的偏转角度β;利用改变偏转角度α对固联在左舵机摇臂5B上的左超声波收发器4的指向进行实时可控调节;利用改变偏转角度β对固联在右舵机摇臂6B上的右超声波收发器7的指向进行实时可控调节。
在本发明中,超声波测距指向解析单元110连续地增加或减小左舵机脉冲宽度WF16和右舵机脉冲宽度WF19,左偏转角度α和右偏转角度β将连续改变,从而实现两个超声波收发器的扫描测距。当超声波测距指向解析单元110不改变左舵机脉冲宽度WF16和右舵机脉冲宽度WF19,则左偏转角度α和右偏转角度β将固定不变,从而实现两个超声波收发器的定向测距。
(8)左扫描舵机控制脉冲生成单元108
左扫描舵机控制脉冲生成单元108用于生成控制左扫描舵机5转动的脉冲信号F16(简称为左舵机控制脉冲信号F16)。
左舵机控制脉冲信号F16是周期为19.2ms的方波。
(9)右扫描舵机控制脉冲生成单元109
右扫描舵机控制脉冲生成单元109用于生成控制右扫描舵机6转动的脉冲信号F19(简称为右舵机控制脉冲信号F19)。
右舵机控制脉冲信号F19是周期为19.2ms的方波。
(10)当前位置坐标解算单元101(参见图3所示)
在运动载体10的坐标系ob-xbybzb下,当运动载体10完成姿态初始化后xb轴与左侧障碍面平齐,定义此时的位置为初始位置,并以运动载体10的几何中心点建立了导航坐标系O-xoyozo。
在初始位置时,左偏转角度记为α0,右偏转角度记为β0,左距离值记为m0,右距离值记为n0;
当运动载体10运动一个航向角θ后,依据运动载体在导航坐标系O-xoyozo下的位置更新矩阵
得到运动载体10的当前位置坐标 其中,CA表示运动载体10的横向的几何尺寸,CB表示运动载体10的纵向的几何尺寸。
在本发明中,在已知运动载体相对于初始位置的偏转角度θ、左右超声波测距指向[α β]T和到障碍面的左右距离[m n]T后,通过上述运动载体10在导航坐标系O-xoyozo下的位置更新矩阵便能够计算出运动载体10的当前位置坐标,从而完成对运动载体10的定位。
参见图4所示,采用本发明室内自主定位与定向的二维导航***完成一次室内定位与定向的具体工作流程如下:
a)当运动载体10进入室内使用环境后,首先完成***初始化,所有舵机(转向舵机3、左扫描舵机5、右扫描舵机6)复位;
b)通过超声波测距指向解析单元110连续改变左扫描舵机脉冲宽度变量F61,并由左扫描舵机控制脉冲生成单元108产生具有不同左舵机脉冲宽度WF16的左舵机控制脉冲信号F16,该左舵机控制脉冲信号F16用于驱动左扫描舵机5扫描;同时左超声波触发脉冲生成单元104产生左超声波测距脉冲信号F14,左侧扫描测距单元通过扫描测距搜索左侧障碍面;当左超声波收发器4收到回波时,超声波测距指向解析单元110不再左扫描舵机脉冲宽度变量F61,并计算此时的左偏转角度α;当左偏转角度α不等于90度时,启动转向舵机控制脉冲生成单元103调整转向舵机3的偏转角度,从而控制运动载体10偏转;
c)重复b)步骤,直至夹角α=90°结束循环。此时完成了运动载体10的姿态初始化,其轴与左侧障碍面的平齐;定义此时的位置为初始位置,并以此时运动载体10的几何中心点建立了导航坐标系O-xoyozo,同时初始化运动载体10的航向角变量和位置坐标变量,即[θXY]T=[0 0 0]T;
d)当运动载体10运动后,MEMS陀螺数据读取单元102读取MEMS陀螺传感器绕轴转动的角速率分量ωbz;然后积分载体相对于其初始位置(导航坐标系)的偏转角度,即求得航向角θ;
e)超声波测距指向解析单元110连续改变左扫描舵机脉冲宽度变量F61,并由左扫描舵机控制脉冲生成单元108产生产生具有不同左舵机脉冲宽度WF16的左舵机控制脉冲信号F16,该左舵机控制脉冲信号F16用于驱动左扫描舵机5扫描;同时左超声波触发脉冲生成单元104产生左超声波测距脉冲信号F14,左侧扫描测距单元通过扫描测距搜索左侧障碍面;当左超声波收发器4收到回波时,超声波测距指向解析单元110不再左扫描舵机脉冲宽度变量F61,并计算此时的左偏转角度α;
超声波测距指向解析单元110连续改变右扫描舵机脉冲宽度变量F91,并由右扫描舵机控制脉冲生成单元109产生产生具有不同右舵机脉冲宽度WF19的右舵机控制脉冲信号F19,该右舵机控制脉冲信号F19用于驱动右扫描舵机6扫描;同时右超声波触发脉冲生成单元105产生右超声波测距脉冲信号F17,右侧扫描测距单元通过扫描测距搜索右上侧障碍面;当右超声波收发器7收到回波时,超声波测距指向解析单元110不再右扫描舵机脉冲宽度变量F91,并计算此时的右偏转角度β;
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)的坐标系ob-xbybzb的三轴分别重合;MEMS陀螺传感器(2)用于测量运动载体(10)在运动状态下的三轴角速率信息F21(ωbx,ωby,ωbz),ωbx表示在坐标系ob-xbybzb下绕xb轴转动的角速率分量,ωby表示在坐标系ob-xbybzb下绕yb轴转动的角速率分量,ωbz表示在坐标系ob-xbybzb下绕zb轴转动的角速率分量;
所述的转向舵机(3)安装在运动载体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)和左扫描舵机(5)组成左侧扫描测距单元;右超声波收发器(7)和右扫描舵机(6)组成右侧扫描测距单元。
3.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:DSP数字信号处理器(1)选用AnalogDevices公司生产的ADSP-BF537处理器;MEMS陀螺传感器(2)选用InvenSense公司生产的ITG-3200陀螺仪;转向舵机(3)选用辉盛公司的Tower Pro 9805BB舵机,其舵机摇臂选取原装的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)在运动状态下的三轴角速率信息F21(ωbx,ωby,ωbz);第二方面对三轴角速率信息F21(ωbx,ωby,ωbz)中的绕zb轴转动的角速率分量ωbz进行初始时刻至当前时刻角速率分量的积分处理t0表示初始时刻,t表示当前时刻,得到运动载体(10)相对于导航坐标系O-xoyozo的偏转角度θ;
转向舵机控制脉冲生成单元(103)第一方面用于产生控制转向舵机脉冲信号F13;第二方面以转向舵机脉冲宽度变量F31控制转向舵机脉冲宽度WF13,且fCLK表示DSP数字信号处理器(1)片上定时器工作的时钟频率;转向舵机(3)依据接收到的具有不同脉冲宽度WF13的转向舵机脉冲信号F13改变转向舵机摇臂(31)的偏转角度,进而操纵运动载体(10)转向。左超声波触发脉冲生成单元(104)用于产生左超声波测距脉冲信号F14;
左超声波距离获取单元(106)第一方面接收左距离脉冲信号F41;第二方面对左距离脉冲信号F41进行脉冲宽度测量得到左距离脉冲宽度WF41;第三方面依据脉冲宽度与到障碍面距离的关系得到左距离值m;Vvoice表示声速;L左表示左舵机摇臂(5B)的长度;
右超声波触发脉冲生成单元(105)用于产生右超声波测距脉冲信号F17;
右超声波距离获取单元(107)第一方面接收右距离脉冲信号F71;第二方面对右距离脉冲信号F71进行脉冲宽度测量得到右距离脉冲宽度WF71;第三方面依据脉冲宽度与到障碍面距离的关系得到右距离值n;Vvoice表示声速;L右表示右舵机摇臂(6B)的长度;
超声波测距指向解析单元(110)第一方面用于产生左扫描舵机脉冲宽度变量F61,该左扫描舵机脉冲宽度变量F61用于调节左舵机脉冲宽度WF16,该左舵机脉冲宽度WF16与左扫描舵机脉冲宽度变量F61之间满足关系,fCLK表示DSP数字信号处理器1片上定时器工作的时钟频率;
超声波测距指向解析单元(110)第三方面用于产生右扫描舵机脉冲宽度变量F91,该右扫描舵机脉冲宽度变量F91用于控制右舵机脉冲宽度WF19;该右舵机脉冲宽度WF19与右扫描舵机脉冲宽度变量F91之间满足关系;
左扫描舵机控制脉冲生成单元(108)用于生成左舵机控制脉冲信号F16;
右扫描舵机控制脉冲生成单元(109)用于生成右舵机控制脉冲信号F19;
在运动载体(10)的坐标系ob-xbybzb下,当运动载体(10)完成姿态初始化后xb轴与左侧障碍面平齐,定义此时的位置为初始位置,并以运动载体(10)的几何中心点建立了导航坐标系O-xoyozo;
在初始位置时,左偏转角度记为α0,右偏转角度记为β0,左距离值记为m0,右距离值记为n0;
当运动载体(10)运动一个航向角θ后,依据运动载体在导航坐标系O-xoyozo下的位置更新矩阵
得到运动载体(10)的当前位置坐标 其中,CA表示运动载体(10)的横向的几何尺寸,CB表示运动载体(10)的纵向的几何尺寸。
5.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:通过改变输出的左舵机控制脉冲信号F16的脉冲宽度WF16能够改变左偏转角度α;通过改变输出的右舵机控制脉冲信号F19的脉冲宽度WF19能够改变右偏转角度β;利用改变左偏转角度α对固联在左舵机摇臂5B上的左超声波收发器(4)的指向进行实时可控调节;利用改变右偏转角度β对固联在右舵机摇臂(6B)上的右超声波收发器(7)的指向进行实时可控调节。
6.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:超声波测距指向解析单元(110)连续地增加或减小左舵机脉冲宽度WF16和右舵机脉冲宽度WF19,左偏转角度α和右偏转角度β将连续改变,从而实现两个超声波收发器的扫描测距;当超声波测距指向解析单元(110)不改变左舵机脉冲宽度WF16和右舵机脉冲宽度WF19,则左偏转角度α和右偏转角度β将固定不变,从而实现两个超声波收发器的定向测距。
7.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:转向舵机控制脉冲信号F13是周期为19.2ms的方波。
8.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:左舵机控制脉冲信号F16是周期为19.2ms的方波;右舵机控制脉冲信号F19是周期为19.2ms的方波。
9.根据权利要求1的一种室内自主定位与定向的二维导航***,其特征在于:左超声波测距脉冲信号F14是一个脉冲宽度为10μs的单束脉冲;右超声波测距脉冲信号F17是一个脉冲宽度为10μs的单束脉冲。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110256305 CN102436260B (zh) | 2011-09-01 | 2011-09-01 | 一种室内自主定位与定向的二维导航*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN 201110256305 CN102436260B (zh) | 2011-09-01 | 2011-09-01 | 一种室内自主定位与定向的二维导航*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN102436260A true CN102436260A (zh) | 2012-05-02 |
CN102436260B CN102436260B (zh) | 2013-07-24 |
Family
ID=45984362
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN 201110256305 Expired - Fee Related CN102436260B (zh) | 2011-09-01 | 2011-09-01 | 一种室内自主定位与定向的二维导航*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN102436260B (zh) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102789233A (zh) * | 2012-06-12 | 2012-11-21 | 湖北三江航天红峰控制有限公司 | 基于视觉的组合导航机器人及导航方法 |
CN103791898A (zh) * | 2012-10-30 | 2014-05-14 | 联想(北京)有限公司 | 一种建筑物内部导航方法及电子设备 |
CN103914067A (zh) * | 2013-01-05 | 2014-07-09 | 联想(北京)有限公司 | 一种控制方法及电子设备 |
CN105606101A (zh) * | 2015-12-21 | 2016-05-25 | 北京航天科工世纪卫星科技有限公司 | 一种基于超声波测量的机器人室内导航方法 |
WO2017000515A1 (zh) * | 2015-06-30 | 2017-01-05 | 上海慧流云计算科技有限公司 | 一种基于路径集与坐标系的室内定位***及方法 |
CN111257887A (zh) * | 2020-02-17 | 2020-06-09 | 自然资源部第一海洋研究所 | 水下仪器姿态检测***及其检测方法 |
CN113660724A (zh) * | 2021-10-20 | 2021-11-16 | 北京卓建智菡科技有限公司 | 一种运动轨迹确定方法、装置、计算机设备及存储介质 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
WO2005119290A1 (en) * | 2004-05-17 | 2005-12-15 | Csi Wireless Inc. | Satellite position and heading sensor for vehicle steering control |
CN2767963Y (zh) * | 2004-08-16 | 2006-03-29 | 华南理工大学 | 基于微电子机械***器件的微型六自由度捷联惯导*** |
CN101881619A (zh) * | 2010-06-25 | 2010-11-10 | 哈尔滨工程大学 | 基于姿态测量的船用捷联惯导与天文定位方法 |
-
2011
- 2011-09-01 CN CN 201110256305 patent/CN102436260B/zh not_active Expired - Fee Related
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6459990B1 (en) * | 1999-09-23 | 2002-10-01 | American Gnc Corporation | Self-contained positioning method and system thereof for water and land vehicles |
WO2005119290A1 (en) * | 2004-05-17 | 2005-12-15 | Csi Wireless Inc. | Satellite position and heading sensor for vehicle steering control |
CN2767963Y (zh) * | 2004-08-16 | 2006-03-29 | 华南理工大学 | 基于微电子机械***器件的微型六自由度捷联惯导*** |
CN101881619A (zh) * | 2010-06-25 | 2010-11-10 | 哈尔滨工程大学 | 基于姿态测量的船用捷联惯导与天文定位方法 |
Non-Patent Citations (1)
Title |
---|
陈培等: "基于星间测量的卫星星座自主导航算法", 《北京航空航天大学学报》 * |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102789233A (zh) * | 2012-06-12 | 2012-11-21 | 湖北三江航天红峰控制有限公司 | 基于视觉的组合导航机器人及导航方法 |
CN102789233B (zh) * | 2012-06-12 | 2016-03-09 | 湖北三江航天红峰控制有限公司 | 基于视觉的组合导航机器人及导航方法 |
CN103791898A (zh) * | 2012-10-30 | 2014-05-14 | 联想(北京)有限公司 | 一种建筑物内部导航方法及电子设备 |
CN103914067A (zh) * | 2013-01-05 | 2014-07-09 | 联想(北京)有限公司 | 一种控制方法及电子设备 |
WO2017000515A1 (zh) * | 2015-06-30 | 2017-01-05 | 上海慧流云计算科技有限公司 | 一种基于路径集与坐标系的室内定位***及方法 |
CN105606101A (zh) * | 2015-12-21 | 2016-05-25 | 北京航天科工世纪卫星科技有限公司 | 一种基于超声波测量的机器人室内导航方法 |
CN105606101B (zh) * | 2015-12-21 | 2018-07-17 | 北京航天科工世纪卫星科技有限公司 | 一种基于超声波测量的机器人室内导航方法 |
CN111257887A (zh) * | 2020-02-17 | 2020-06-09 | 自然资源部第一海洋研究所 | 水下仪器姿态检测***及其检测方法 |
CN111257887B (zh) * | 2020-02-17 | 2021-06-15 | 自然资源部第一海洋研究所 | 水下仪器姿态检测***及其检测方法 |
CN113660724A (zh) * | 2021-10-20 | 2021-11-16 | 北京卓建智菡科技有限公司 | 一种运动轨迹确定方法、装置、计算机设备及存储介质 |
CN113660724B (zh) * | 2021-10-20 | 2021-12-31 | 北京卓建智菡科技有限公司 | 一种运动轨迹确定方法、装置、计算机设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN102436260B (zh) | 2013-07-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102436260B (zh) | 一种室内自主定位与定向的二维导航*** | |
CN105157697B (zh) | 基于光电扫描的室内移动机器人位姿测量***及测量方法 | |
US10520223B2 (en) | Robotic heliostat calibration system and method | |
CN108919825A (zh) | 具备避障功能的无人机室内定位***及方法 | |
CN103809174A (zh) | 自动跟随方法 | |
CN104102222A (zh) | 一种agv精确定位的方法 | |
CN107505597A (zh) | 一种基于深度信息的超短基线定位***及方法 | |
CN110026993B (zh) | 一种基于uwb及热释电红外传感器的人体跟随机器人 | |
CN101109630A (zh) | 测量距离的信标、使用其的定位***和测量距离的方法 | |
CN205656497U (zh) | 一种rfid定位机器人 | |
CN111465908B (zh) | 一种分段式自主充电对接方法及移动设备、充电站 | |
CN105547290A (zh) | 一种基于超短基线定位***的从潜器导航方法 | |
CN108776344A (zh) | 一种低成本可斜射激光雷达 | |
CN110741272B (zh) | 无线电信标*** | |
KR101155500B1 (ko) | 복수 개의 이동체를 제어하는 장치 및 방법 | |
CN114035150B (zh) | 一种基于无人机升空平台的无线电射频源测向装置及定位方法 | |
CN105652238A (zh) | 一种基于rfid的机器人位置姿态确定方法 | |
CN111510624A (zh) | 目标跟踪***及目标跟踪方法 | |
CN104898146A (zh) | 一种车载定位装置 | |
CN107037400B (zh) | 一种应用天线阵列的高精度agv定位方法 | |
AU2011336375A1 (en) | Robotic heliostat and calibration system and method | |
CN109828569A (zh) | 一种基于2d-slam导航的智能agv叉车 | |
KR100962674B1 (ko) | 이동 로봇의 위치 추정 방법 및 이를 위한 이동 로봇 | |
CN112462322B (zh) | 一种水下蛙人定位方法及定位*** | |
CN104457757A (zh) | 一种基于前项反馈修正的井下动目标定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20130724 Termination date: 20140901 |
|
EXPY | Termination of patent right or utility model |