CN108844543A - 基于uwb定位及航位推算的室内agv导航控制方法 - Google Patents
基于uwb定位及航位推算的室内agv导航控制方法 Download PDFInfo
- Publication number
- CN108844543A CN108844543A CN201810585592.6A CN201810585592A CN108844543A CN 108844543 A CN108844543 A CN 108844543A CN 201810585592 A CN201810585592 A CN 201810585592A CN 108844543 A CN108844543 A CN 108844543A
- Authority
- CN
- China
- Prior art keywords
- node
- positioning
- agv
- dead reckoning
- uwb
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000005259 measurement Methods 0.000 claims abstract description 29
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 18
- 238000004891 communication Methods 0.000 claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims abstract description 10
- 230000007246 mechanism Effects 0.000 claims abstract description 9
- 230000004044 response Effects 0.000 claims description 12
- 230000008859 change Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 6
- 230000005540 biological transmission Effects 0.000 claims description 5
- JEIPFZHSYJVQDO-UHFFFAOYSA-N ferric oxide Chemical compound O=[Fe]O[Fe]=O JEIPFZHSYJVQDO-UHFFFAOYSA-N 0.000 claims description 5
- 230000004807 localization Effects 0.000 claims description 5
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 230000000694 effects Effects 0.000 claims description 4
- 230000006870 function Effects 0.000 claims description 4
- 238000004458 analytical method Methods 0.000 claims description 3
- 230000008054 signal transmission Effects 0.000 claims description 3
- 230000009471 action Effects 0.000 claims description 2
- 238000001914 filtration Methods 0.000 claims description 2
- 238000012937 correction Methods 0.000 claims 1
- 238000009790 rate-determining step (RDS) Methods 0.000 abstract 1
- 230000008569 process Effects 0.000 description 12
- 238000005516 engineering process Methods 0.000 description 11
- 230000033001 locomotion Effects 0.000 description 7
- 238000013461 design Methods 0.000 description 4
- 230000007812 deficiency Effects 0.000 description 3
- 230000004888 barrier function Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 2
- 230000002146 bilateral effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 241000208340 Araliaceae Species 0.000 description 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 1
- 235000003140 Panax quinquefolius Nutrition 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 235000008434 ginseng Nutrition 0.000 description 1
- 238000003306 harvesting Methods 0.000 description 1
- 238000009738 saturating Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000001228 spectrum Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
Abstract
本发明公开了一种基于UWB定位及航位推算的室内AGV导航控制方法，单个待定位节点与单个参考节点之间的距离测量；单个待定位节点与多个参考节点之间的距离测量；待定位节点的定位；航向角的测量；AGV行驶距离的测量；航位推算的定位导航以及组合定位导航几个控制步骤，其中待定位节点的定位利用内四边形质心法定位，通过事先设定好的通讯机制，测量单个待定位节点与多个参考节点之间的信号飞行时间，从而计算距离；最后，通过内四边形质心算法来实现定位计算，分别以各个参考节点为圆心，相应的测量距离为半径画圆，利用四个相近的圆交点，组成四边形，再通过定位修正因子来计算得到内四边形的四个顶点，计算得到待定位节点的估计坐标。
Description
技术领域
本发明属于室内机器人技术领域，具体涉及一种基于UWB定位及航位推算的室内AGV导航控制方法。
背景技术
随着社会经济与科技水平的进一步发展，工业自动化的水平得到了进一步的提高，AGV的服务已经深入到社会的每一个角落。
目前，室内AGV的路径控制主要有轨道引导和视觉引导两种方式。轨道引导需要在作业现场铺设轨道来引导小车前进，这种方式的不足是：(1)轨道的铺设使得工业作业现场AGV小车路径灵活性不足，加工工艺的改变，需要用户改变铺设的轨道；(2)对于多移动机器人的复杂路径或交叉路径，铺设轨道的方式，是无法引导AGV小车的。视觉引导方式是使用基于图像背景的定位原理，存在的不足：(1)位置精度误差大；(2)计算量大；(3)作业环境的改变，需要重新布置图像背景。
针对上述AGV小车路径控制方式和定位原理，无线定位方案明显弥补了其定位的不足。目前无线定位技术有很多，包括Wifi、RFID、超声波和蓝牙等，各种定位技术的定位精度都比较差，正是这一问题的存在，限制了无线定位技术在室内环境中的应用。
UWB技术是一种利用亚纳秒级超窄脉冲的无载波通信技术。相对于其他定位技术，UWB技术具有以下优势：
1)频谱利用率高，传输速度快；2)抗多径效应能力强；3)抗干扰能力：4)强大的穿透能力；5)功耗低，成本低；6)高定位精度。
UWB室内定位技术在理论上可以将误差控制在10cm，实际应用中能控制误差在20cm内。相对于其他定位技术，使用UWB信号进行室内定位可以获得更高的定位精度和稳定性。
发明内容
发明目的：本发明的目的是为了解决现有技术中的不足，提供一种基于UWB定位及航位推算的室内AGV导航控制方法，该控制方法通过AGV的航位推算自主导航对室内AGV的无线定位导航控制，加以配合UWB的室内高精度定位，很好的纠正室内AGV运动过程中的路径误差，从而实现室内AGV定位导航精度高，稳定性好。
技术方案：本发明所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，包括：
单个待定位节点与单个参考节点之间的距离测量：通过设置在待定位节点与参考节点上的用于UWB无线信号传输的模组来获取UWB信号在待定位节点与参考节点上两节间发送与接收信号的时间，从而获取信号在两节点间的传播时间，再与传播速度相乘就得到所测的单个待定位节点与单个参考节点之间的距离；
单个待定位节点与多个参考节点之间的距离测量：待定位节点使用广播方式，向各个参考节点传输信号，分配各参考节点的响应时间；参考节点在接收待定位节点的广播帧后，重新进入接收状态，等待待定位节点的请求信号；待定位节点发送请求帧后，进入接收状态，等待参考节点响应信号；各参考节点依据通信前广播帧分配的响应时间，依序向带定位节点作出回应，发送成功后进入接收状态，等待待定位节点终止帧信号；待定位节点接收完各参考节点响应帧后，将各时间节点打包形成终止帧，以广播方式发送给相应参考节点；各参考节点在接收待定位节点的终止帧后，解析数据帧中的有效数据，计算与待定位节点之间距离，隔一段时间，将距离值返送至待定位节点；待定位节点在获取与各参考节点之间的距离值后，使用内四边形质心定位算法软件计算出自身坐标信息；
待定位节点的定位：利用内四边形质心法定位，通过事先设定好的通讯机制，测量单个待定位节点与多个参考节点之间的信号飞行时间，从而计算距离；最后，通过内四边形质心算法来实现定位计算，分别以各个参考节点为圆心，相应的测量距离为半径画圆，利用四个相近的圆交点，组成四边形，再通过定位修正因子来计算得到内四边形的四个顶点，计算得到待定位节点的估计坐标；
航向角的测量：利用MEMS惯性传感器，测量空间方向上的原始数据，对原始数据通过动态卡尔曼滤波与数据融合算法分离出每一个传感器在受干扰状况下所测量的精确数据，通过精确数据计算得到航向角数据；
AGV行驶距离的测量：利用AGV自身的编码器，在已知车轮直径以及行驶一周输出脉冲数，计算得到输出一个脉冲，AGV相应的行驶距离，从而根据一段时间内捕获的脉冲数计算得到相应的行驶距离；
航位推算的定位导航：在得到高精度的航向角与行驶距离后，通过相应的方法计算得到平面内x、y轴的变化量，再根据前一时刻位置坐标就可以求出当前位置坐标；
组合定位导航：UWB定位数据每隔一定时间获取一次，而航位推算数据都是每隔一定时间获取一次，将UWB获取的定位坐标与同时获得的航位推算坐标之间的距离为路径误差d与ε值相比较，ε为设定路径误差；当d＞ε时，直接将UWB定位坐标赋值给航位推算所得AGV当前的坐标，作为新的起点，对下一次航位进行推算；否则，以航位推算出来的AGV当前坐标为导航结果继续推算下去。
进一步的，待定位节点的定位步骤中，当待定位节点的定位发生误差，多个定位圆不能相较于一点时，在这种情况下，利用定位算法从中选择多个交点，具体的若四个圆都是两两相交的，则选择距离较近的四个组成四边形，进而根据内四边形算法求得坐标，否则，舍弃本次测量结果，再次测量，最终达到定位坐标的计算。
进一步的，所述待定位节点的单片机在计算得到自身的定位坐标之后，一方面要通过串口将信息传送到AGV控制器，并存储到SD存储卡中；另一方面，需要通过无线通讯模块，将坐标信息传送上位机，供上位机分析所用。
进一步的，所述MEMS惯性传感器中包括了三轴加速度计、三轴速度计与三轴磁力计。
进一步的，用户可以通过上位机来观察得到AGV的各种运行状态参数；AGV能够在当前环境中自动有效的进行避障和防碰撞处理，并且得到到达目标点的较优路径。
进一步的，所述的航位推算的定位导航中，AGV中的控制器单片机要具有准确快速捕获每一个脉冲的功能，以便准确计算AGV输出的脉冲数，计算行驶距离。
进一步的，在航位推算的定位导航中，使用计算量较小且无万向节死锁的四元数方法来求解得到航向角的大小。
进一步的，在组合定位导航中，先进行航位推算的自主定位导航，在相隔相同的时间内，获得UWB的高精度定位数据，并计算其与同一时间得到的航位推算的数据的误差，从而达到纠正自主导航的作用。
进一步的，在航位推算的定位导航中，在得到高精度的航向角与行驶距离后，推算出平面内x、y两个方向各自的位移变化，以此计算AGV实时位置坐标。
有益效果：本发明通过AGV的航位推算自主导航对室内AGV的无线定位导航控制，加以配合UWB的室内高精度定位，很好的纠正室内AGV运动过程中的路径误差，从而实现室内AGV定位导航精度高，稳定性好。
附图说明
图1为本发明一个实施例的系统运行预期效果图；
图2为本发明一个实施例的基于UWB的AGV定位导航系统框图；
图3为本发明一个实施例的四边形定位模型图；
图4为本发明一个实施例的三角定位模型图；
图5为本发明一个实施例的内四边形质心定位模型图；
图6为本发明一个实施例的非对称双边双向测距机制；
图7为本发明一个实施例的多参考节点通讯机制；
图8为本发明一个实施例的内四边形质心定位算法流程图；
图9为本发明一个实施例的航位推算定位模型；
图10为本发明一个实施例的航位推算算法流程图；
图11为本发明一个实施例的组合定位导航算法流程图。
具体实施方式
下面结合具体实施例对本发明的技术方案作进一步详细说明。
AGV应用可以概括为两大部分：定位与导航。定位是指AGV依据4个参考节点的测量数据，通过UWB无线信号传输，实时计算得到的自身的平面坐标信息(x1，y1)或者是根据航向角测量结果和编码器测量距离来推算的自身在平面内的坐标信息(x2，y2)；导航是指AGV根据自身当前位置信息，用户设定的目标位置信息，能自动进行路径规划并前行，同时，能够实现在当前环境中进行自动避障，并选择较优的路径。
图1为“基于UWB定位的航位推算的室内AGV的导航系统”的预期运行效果图，模拟在4个加工工位，5辆AGV协同工作的场景。
在车间室内顶部4个角落偏里的地方放置4个带有基于UWB无线传输的DWM1000模组，(1号参考节点、2号参考节点、3号参考节点、4号参考节点)，在每一辆AGV上安装有同样的模组的待定位节点。待定位节点根据事先约定的通讯机制分别于4个参考节点进行通讯，可以将测量得到的距离信息传输给待定位节点上的单片机，然后该单片机根据约定的定位算法来计算当前自身所在的平面坐标信息(x，y)。待定位节点的单片机在计算得到自身的定位坐标之后，一方面要通过串口将信息传送到AGV控制器，并存储到SD存储卡中，另一方面，需要通过无线通讯模块，将坐标信息传送上位机，供上位机分析所用。
车间工位之间的AGV在航位推算自主导航方法的作用下运动，然后，在根据UWB定位的结果对其纠正，使得整个系统精度高，稳定性好，能够让AGV准确运动到需要的目标位置，进行生产加工。
如图1所示，图中工位之间的带箭头的虚线是最优的运行路径，但是工位1与工位2、工位3与工位4之间存在障碍物，AGV具有红外检测及避障功能，能自动完成避障。
图2是基于UWB的AGV定位导航系统框图。该基于UWB的AGV定位导航系统包括微控制器(单片机)，所述微控制器分别连接有超声波测距模块、红外线模块、SD卡存储模块、测速模块、JY-901航向角测量模块、待定位节点模块、电机驱动模块、舵机控制驱动模块、无线通讯模块，其中JY-901航向角测量模块中包括了MEMS传感器和MCU，MEMS传感器中包括了三轴加速度计、三轴速度计与三轴磁力计。电机驱动模块连接有电机马达，无线通讯模块无线连接有PC机，用户可以通过PC机来观察得到AGV的各种运行状态参数。
AGV在运动过程中，需要自主定位导航。在上述基于UWB的AGV定位导航系统中，选择先进的MEMS传感器并设计相关的电路来获取各个方向上相关的数据，然后设计相应的软件算法来准确测量航向角的大小。此外，设计脉冲捕获软件来计算AGV测速模块输出的脉冲个数。最后，设计基于航位推算的算法来推导出当前时刻AGV的实时位置信息。
在UWB定位模块设计方面，首先设计基于UWB技术的待定位节点和参考节点的模块，并将它们之间的通信调试好，使其能够正常收发信号，进一步就需要设计基于TOF(飞行时间)的测距原理及其软件、定位算法等获取定位坐标信息。
基于UWB的AGV定位导航系统，还设计了SD卡存储模块来存放获得的坐标信息，还设计了与上位机的无线通信，将数据上传至上位机，便于上位机的数据处理。
下面结合一个实施例具体阐述上述的一种基于UWB定位的航位推算的室内AGV导航控制系统的相关控制方法：
本发明中UWB定位采用TOF定位方法：待定位节点与参考节点之间通过UWB信号，按照设定好的通讯机制进行通信测距；待定位节点与4个参考节点进行通信测距，测得4个距离值；以个参考节点为圆心，相应的距离为半径画圆，利用相近的4个交点组成四边形，如图3所示。
UWB定位使用的是内四边形质心定位算法模型：对于具有4个参考点的定位算法模型，先在图3中的四个交点A，B，C，D中选取任意三个交点作为顶点形成一个三角形。然后，不妨假设其中某一顶点为待定位点的实际坐标，根据式(1)计算出其余两个顶点的定位修正因子K。再依次对选中的其余顶点作类似的处理，就可以获得这3个顶点的等效位置。最后，利用得到的三个点的等效点作为三角形的顶点求取其质心的坐标，即待定位点的坐标。例如从图3中选取A、B、D三点作为顶点形成三角形，如图4所示，求得三个等效点a，b，d。先不妨假设顶点A就是待定位点的实际位置，LAB和LAD分别表示点B、D与点A之间的距离，则通过式(1)求取定位修正因子如式(2)所示。
根据加权平均的计算方法可以求出等效点a的具体坐标，如式(3)所示。
通过同样的方法，可以得到b、d两等效点的坐标，则△abd的质心坐标可以通过式(4)来求解，并将其作为内四边形的一个顶点。
然后依次从图3的公共区域的四个顶点中选出(A,B,C)、(A,C,D)、(B,C,D)三种组合，应用上述步骤求取一个内四边形顶点的方法依次求取出其他三个顶点等效点，组成内四边形，如图5所示。
最后，根据加权平均计算方法求取待定位节点坐标，如式(5)所示。
TOF测距原理：本系统中采用的是异步测距，即非对称双边双向测距方法。此方法在两个节点侧分别完成一次单向测距过程。其测距过程如图6所示，具体测距步骤如下：
(1)待定位节点先向参考节点发送请求帧，同时启动测距过程；
(2)参考节点接收到待定位节点发出的请求帧，解析数据，获得规定的时延值大小，经过相应的时延后，向待定位节点发送回应帧；
(3)待定位节点在接收到参考节点的回应帧数据之后，经过事先规定好的时延值，同时将tround1和treply1时间值装入到最终帧中，并发送给参考节点；
(4)参考节点接收到最终帧数据，结束测距过程。
参考节点解析出有效数据，根据式(6)来计算出信号在待定位节点与参考节点之间传输时间的大小，然后根据式(7)求取距离。
d＝△t×Vc (7)
上面的测距还是实现了单个待定位节点与单个参考节点之间的通讯，如图7是单个待定位节点与4个参考节点的通讯机制，具体包括如下步骤：
(1)待定位节点先通过广播的形式向所有参考节点发送一帧信息，该信息规定了各参考节点的响应时延等参数，发送完毕，待定位节点又设置为发送状态，且各参考节点在接收信息后还维持接收状态不变；
(2)待定位节点以广播形式向所有节点发送请求帧信息，同时，启动测距过程，参考节点1、2、3、4按顺序接收信息，并按事先规定好的时延，依次向待定位节点发送响应帧信息；
(3)待定位点在接收到所有参考节点的响应信号之后，把对应的时延计算出来并放到最终帧，以广播形式发送给各参考节点，结束测距过程。
单个待定位点与单个参考节点之间的测距过程需要三次通信才能完成，以此类推，与4个参考节点测距需要12次通信才能完成。但是上述机制只需要7次就可以完成，大大缩短了测距时间，提高了效率，UWB定位的方法整体流程如图8所示。
本发明中航位推算的具体实现：实施步骤是首先利用JY-901模块测量三维空间中的各个方向上的原始数据，再利用四元数法计算得到航向角ψ的大小。
在测量出航向角的基础上，航位推算应用AGV的测速模块测量其左右轮各自的运行距离，进而推算出实时坐标位置。不妨假设左轮测量的运行距离为L1，右轮测量运行距离为L2，通过式(8)可以计算出AGV实际运行距离为L3。
得到AGV较为准确的运行距离之后，结合上面得到的航向角推算出平面内x、y两个方向各自的位移变化，以此计算AGV实时位置坐标。其具体的推算模型如图9所示。
根据图9可知，AGV从M点运动至N点时，其中在M点位置时AGV的航向角为θ(k)，在N点位置时AGV的航向角为θ(k+1)。将MN这段圆弧长看做为从M点到N点的线段的长度。其与x轴的夹角的大小为θ。θ的值可以根据式(9)来计算得到。
然后，将当前时刻AGV所在位置的航向角θ(k+1)赋值给前一时刻AGV所在位置航向角θ(k)，重新测量新的航向角并赋值给θ(k+1)，以便用于下一次导航推算的计算。
如图9所示，从M点位置运动到N点位置，x轴与y轴位移变化可以通过式(10)来计算得到。
其中LMN表示线段MN的长度，其大小可以根据式(8)来计算，并将其大小近似地看作为MN之间的弧长。
在计算得到了x轴与y轴的坐标变化量后，由于M点的坐标是已知的，结合式(11)可以推算得到N点的具体坐标。
综上所述，可以得到航位推算，具体流程如图10所示。
本发明中的组合导航方法的流程如图11所示。UWB定位数据每隔1s获取一次，而航位推算数据都是每隔0.5s获取一次，将UWB获取的定位坐标与同时获得的航位推算坐标之间的距离为路径误差d，用d与ε值(ε为设定路径误差)相比较，当d＞ε时，直接将UWB定位坐标赋值给航位推算所得AGV当前的坐标，作为新的起点，对下一次航位进行推算；否则，以航位推算出来的AGV当前坐标为导航结果继续推算下去。
本发明通过AGV的航位推算自主导航对室内AGV的无线定位导航控制，加以配合UWB的室内高精度定位，很好的纠正室内AGV运动过程中的路径误差，从而实现室内AGV定位导航精度高，稳定性好。
以上所述，仅是本发明的较佳实例，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭示如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。
Claims (9)
1.一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：包括：
单个待定位节点与单个参考节点之间的距离测量：通过设置在待定位节点与参考节点上的用于UWB无线信号传输的模组来获取UWB信号在待定位节点与参考节点上两节间发送与接收信号的时间，从而获取信号在两节点间的传播时间，再与传播速度相乘就得到所测的单个待定位节点与单个参考节点之间的距离；
单个待定位节点与多个参考节点之间的距离测量：待定位节点使用广播方式，向各个参考节点传输信号，分配各参考节点的响应时间；参考节点在接收待定位节点的广播帧后，重新进入接收状态，等待待定位节点的请求信号；待定位节点发送请求帧后，进入接收状态，等待参考节点响应信号；各参考节点依据通信前广播帧分配的响应时间，依序向带定位节点作出回应，发送成功后进入接收状态，等待待定位节点终止帧信号；待定位节点接收完各参考节点响应帧后，将各时间节点打包形成终止帧，以广播方式发送给相应参考节点；各参考节点在接收待定位节点的终止帧后，解析数据帧中的有效数据，计算与待定位节点之间距离，隔一段时间，将距离值返送至待定位节点；待定位节点在获取与各参考节点之间的距离值后，使用内四边形质心定位算法软件计算出自身坐标信息；
待定位节点的定位：利用内四边形质心法定位，通过事先设定好的通讯机制，测量单个待定位节点与多个参考节点之间的信号飞行时间，从而计算距离；最后，通过内四边形质心算法来实现定位计算，分别以各个参考节点为圆心，相应的测量距离为半径画圆，利用四个相近的圆交点，组成四边形，再通过定位修正因子来计算得到内四边形的四个顶点，计算得到待定位节点的估计坐标；
航向角的测量：利用MEMS惯性传感器，测量空间方向上的原始数据，对原始数据通过动态卡尔曼滤波与数据融合算法分离出每一个传感器在受干扰状况下所测量的精确数据，通过精确数据计算得到航向角数据；
AGV行驶距离的测量：利用AGV自身的编码器，在已知车轮直径以及行驶一周输出脉冲数，计算得到输出一个脉冲，AGV相应的行驶距离，从而根据一段时间内捕获的脉冲数计算得到相应的行驶距离；
航位推算的定位导航：在得到高精度的航向角与行驶距离后，通过相应的方法计算得到平面内x、y轴的变化量，再根据前一时刻位置坐标就可以求出当前位置坐标；
组合定位导航：UWB定位数据每隔一定时间获取一次，而航位推算数据都是每隔一定时间获取一次，将UWB获取的定位坐标与同时获得的航位推算坐标之间的距离为路径误差d与ε值相比较，ε为设定路径误差；当d＞ε时，直接将UWB定位坐标赋值给航位推算所得AGV当前的坐标，作为新的起点，对下一次航位进行推算；否则，以航位推算出来的AGV当前坐标为导航结果继续推算下去。
2.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：待定位节点的定位步骤中，当待定位节点的定位发生误差，多个定位圆不能相较于一点时，在这种情况下，利用定位算法从中选择多个交点，具体的若四个圆都是两两相交的，则选择距离较近的四个组成四边形，进而根据内四边形算法求得坐标，否则，舍弃本次测量结果，再次测量，最终达到定位坐标的计算。
3.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：所述待定位节点的单片机在计算得到自身的定位坐标之后，一方面要通过串口将信息传送到AGV控制器，并存储到SD存储卡中；另一方面，需要通过无线通讯模块，将坐标信息传送上位机，供上位机分析所用。
4.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：所述MEMS惯性传感器中包括了三轴加速度计、三轴速度计与三轴磁力计。
5.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：用户可以通过上位机来观察得到AGV的各种运行状态参数；AGV能够在当前环境中自动有效的进行避障和防碰撞处理，并且得到到达目标点的较优路径。
6.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：所述的航位推算的定位导航中，AGV中的控制器单片机要具有准确快速捕获每一个脉冲的功能，以便准确计算AGV输出的脉冲数，计算行驶距离。
7.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：在航位推算的定位导航中，使用计算量较小且无万向节死锁的四元数方法来求解得到航向角的大小。
8.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：在组合定位导航中，先进行航位推算的自主定位导航，在相隔相同的时间内，获得UWB的高精度定位数据，并计算其与同一时间得到的航位推算的数据的误差，从而达到纠正自主导航的作用。
9.根据权利要求1所述的一种基于UWB定位及航位推算的室内AGV导航控制方法，其特征在于：在航位推算的定位导航中，在得到高精度的航向角与行驶距离后，推算出平面内x、y两个方向各自的位移变化，以此计算AGV实时位置坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810585592.6A CN108844543A (zh) | 2018-06-08 | 2018-06-08 | 基于uwb定位及航位推算的室内agv导航控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810585592.6A CN108844543A (zh) | 2018-06-08 | 2018-06-08 | 基于uwb定位及航位推算的室内agv导航控制方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN108844543A true CN108844543A (zh) | 2018-11-20 |
Family
ID=64210420
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810585592.6A Pending CN108844543A (zh) | 2018-06-08 | 2018-06-08 | 基于uwb定位及航位推算的室内agv导航控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108844543A (zh) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109631908A (zh) * | 2019-01-31 | 2019-04-16 | 北京永安信通科技股份有限公司 | 基于建筑物结构数据的对象定位方法、装置和电子设备 |
| CN109668556A (zh) * | 2019-01-23 | 2019-04-23 | 广州中石科技有限公司 | 防碰撞系统及方法 |
| CN109938974A (zh) * | 2019-04-01 | 2019-06-28 | 杨天歌 | 一种视障人士用便携式智能导盲设备及导盲方法 |
| CN110012537A (zh) * | 2019-04-08 | 2019-07-12 | 重庆邮电大学 | 一种基于ap四边形组网的质心约束室内定位方法 |
| CN110297502A (zh) * | 2019-06-06 | 2019-10-01 | 浙江智莱科技有限公司 | 基于uwb定位的线控底盘轨迹跟踪控制系统及方法 |
| CN111294731A (zh) * | 2020-01-16 | 2020-06-16 | 珠海格力电器股份有限公司 | Uwb室内定位方法、装置、电子设备及存储介质 |
| CN112073909A (zh) * | 2020-08-20 | 2020-12-11 | 哈尔滨工程大学 | 基于uwb/mems组合的uwb基站位置误差补偿方法 |
| CN112244737A (zh) * | 2020-10-19 | 2021-01-22 | 重庆金山医疗器械有限公司 | 一种胶囊定位方法、装置及系统 |
| CN113720343A (zh) * | 2021-08-16 | 2021-11-30 | 中国科学院上海微系统与信息技术研究所 | 基于动态数据实时适应的航向预测方法 |
| US20220121211A1 (en) * | 2019-01-31 | 2022-04-21 | Lg Electronics Inc. | Mobile robot and control method therefor |
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN116954235A (zh) * | 2023-09-21 | 2023-10-27 | 深圳大工人科技有限公司 | Agv小车导航控制方法及系统 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101464510A (zh) * | 2009-01-08 | 2009-06-24 | 浙江大学 | 一种无线传感器网络多点准确定位与跟踪方法 |
| CN103344942A (zh) * | 2013-06-17 | 2013-10-09 | 清华大学 | 控制节点、异步定位方法与系统 |
| US20130282278A1 (en) * | 2009-09-14 | 2013-10-24 | Research In Motion Limited | Methods, device and systems for determining route metrics using stored route information |
| CN104697517A (zh) * | 2015-03-26 | 2015-06-10 | 江南大学 | 一种室内停车场多目标跟踪定位系统 |
| CN105657668A (zh) * | 2016-03-16 | 2016-06-08 | 南通大学 | 一种基于uwb的室内移动机器人定位与导航控制方法 |
-
2018
- 2018-06-08 CN CN201810585592.6A patent/CN108844543A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101464510A (zh) * | 2009-01-08 | 2009-06-24 | 浙江大学 | 一种无线传感器网络多点准确定位与跟踪方法 |
| US20130282278A1 (en) * | 2009-09-14 | 2013-10-24 | Research In Motion Limited | Methods, device and systems for determining route metrics using stored route information |
| CN103344942A (zh) * | 2013-06-17 | 2013-10-09 | 清华大学 | 控制节点、异步定位方法与系统 |
| CN104697517A (zh) * | 2015-03-26 | 2015-06-10 | 江南大学 | 一种室内停车场多目标跟踪定位系统 |
| CN105657668A (zh) * | 2016-03-16 | 2016-06-08 | 南通大学 | 一种基于uwb的室内移动机器人定位与导航控制方法 |
Non-Patent Citations (4)
| Title |
|---|
| 杨杰等: "《高精度激光陀螺惯导系统非线性模型参数评估方法研究》", 30 June 2016, 国防工业出版社 * |
| 王洪元等: "一种改进的加权质心自定位算法", 《计算机与应用化学》 * |
| 陈业海等: "基于RSSI测距修正的无线传感器网络四边形加权质心定位", 《计算机测量与控制》 * |
| 鲍硕等: "红外传感器地面定位能力研究", 《信息通信》 * |
Cited By (17)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109668556A (zh) * | 2019-01-23 | 2019-04-23 | 广州中石科技有限公司 | 防碰撞系统及方法 |
| US20220121211A1 (en) * | 2019-01-31 | 2022-04-21 | Lg Electronics Inc. | Mobile robot and control method therefor |
| EP3919237A4 (en) * | 2019-01-31 | 2022-11-09 | LG Electronics Inc. | MOBILE ROBOT AND CONTROL METHOD THEREOF |
| CN109631908A (zh) * | 2019-01-31 | 2019-04-16 | 北京永安信通科技股份有限公司 | 基于建筑物结构数据的对象定位方法、装置和电子设备 |
| CN109938974A (zh) * | 2019-04-01 | 2019-06-28 | 杨天歌 | 一种视障人士用便携式智能导盲设备及导盲方法 |
| CN110012537A (zh) * | 2019-04-08 | 2019-07-12 | 重庆邮电大学 | 一种基于ap四边形组网的质心约束室内定位方法 |
| CN110297502B (zh) * | 2019-06-06 | 2022-07-01 | 浙江智莱科技有限公司 | 基于uwb定位的线控底盘轨迹跟踪控制系统及方法 |
| CN110297502A (zh) * | 2019-06-06 | 2019-10-01 | 浙江智莱科技有限公司 | 基于uwb定位的线控底盘轨迹跟踪控制系统及方法 |
| CN111294731A (zh) * | 2020-01-16 | 2020-06-16 | 珠海格力电器股份有限公司 | Uwb室内定位方法、装置、电子设备及存储介质 |
| CN112073909A (zh) * | 2020-08-20 | 2020-12-11 | 哈尔滨工程大学 | 基于uwb/mems组合的uwb基站位置误差补偿方法 |
| CN112073909B (zh) * | 2020-08-20 | 2022-05-24 | 哈尔滨工程大学 | 基于uwb/mems组合的uwb基站位置误差补偿方法 |
| CN112244737A (zh) * | 2020-10-19 | 2021-01-22 | 重庆金山医疗器械有限公司 | 一种胶囊定位方法、装置及系统 |
| CN113720343A (zh) * | 2021-08-16 | 2021-11-30 | 中国科学院上海微系统与信息技术研究所 | 基于动态数据实时适应的航向预测方法 |
| CN114545944A (zh) * | 2022-02-24 | 2022-05-27 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN114545944B (zh) * | 2022-02-24 | 2024-04-16 | 合肥工业大学 | 一种基于磁钉磁场强度纠正的agv航向定位导航方法 |
| CN116954235A (zh) * | 2023-09-21 | 2023-10-27 | 深圳大工人科技有限公司 | Agv小车导航控制方法及系统 |
| CN116954235B (zh) * | 2023-09-21 | 2023-11-24 | 深圳大工人科技有限公司 | Agv小车导航控制方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108844543A (zh) | 基于uwb定位及航位推算的室内agv导航控制方法 | |
| CN108012326B (zh) | 基于栅格地图的机器人监视宠物的方法及芯片 | |
| CN105547305B (zh) | 一种基于无线定位和激光地图匹配的位姿解算方法 | |
| CN109282811B (zh) | Uwb与智能手机互联的室内外定位系统与方法 | |
| CN105157697B (zh) | 基于光电扫描的室内移动机器人位姿测量系统及测量方法 | |
| CN103412565B (zh) | 一种具有全局位置快速估计能力的机器人定位方法 | |
| CN109541535A (zh) | 一种基于uwb和视觉slam的agv室内定位及导航的方法 | |
| CN107861513A (zh) | 基于uwb技术的智能机器人自主导航系统 | |
| CN106338991A (zh) | 一种基于惯性导航和二维码的机器人及定位导航方法 | |
| CN106291517A (zh) | 基于位置与视觉信息优化的室内云机器人角度定位方法 | |
| WO2020151663A1 (zh) | 车辆定位装置、系统、方法以及车辆 | |
| CN108801269A (zh) | 一种室内云机器人导航系统及方法 | |
| CN105044754A (zh) | 一种基于多传感器融合的移动平台室外定位方法 | |
| CN108627864A (zh) | 基于汽车钥匙的定位方法及系统、无人驾驶汽车系统 | |
| CN110148308A (zh) | 室内停车场内的车辆定位系统 | |
| CN109917790A (zh) | 一种自主导引车辆及其行驶控制方法和控制装置 | |
| CN107302754A (zh) | 一种基于WiFi与PDR的室内定位简易方法 | |
| CN111465908B (zh) | 一种分段式自主充电对接方法及移动设备、充电站 | |
| CN105116886A (zh) | 一种机器人自主行走的方法 | |
| CN110177337A (zh) | 室内定位方法及室内定位购物车系统 | |
| CN107065858A (zh) | 基于超宽带的巡检机器人导航方法 | |
| CN103278132A (zh) | 自行走式地下掘进机器人姿态测量装置及其测量方法 | |
| CN109029423A (zh) | 变电站室内移动机器人导航定位系统及其导航定位方法 | |
| CN113607166B (zh) | 基于多传感融合的自主移动机器人室内外定位方法及装置 | |
| CN108919297A (zh) | 一种无人机室内定位系统及方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181120 |
|
| RJ01 | Rejection of invention patent application after publication |