CN113984073A - 一种基于方位的移动机器人协同校正算法 - Google Patents
一种基于方位的移动机器人协同校正算法 Download PDFInfo
- Publication number
- CN113984073A CN113984073A CN202111167717.1A CN202111167717A CN113984073A CN 113984073 A CN113984073 A CN 113984073A CN 202111167717 A CN202111167717 A CN 202111167717A CN 113984073 A CN113984073 A CN 113984073A
- Authority
- CN
- China
- Prior art keywords
- positioning
- mobile robot
- coordinate system
- orientation
- observation
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 26
- 238000012937 correction Methods 0.000 title claims abstract description 20
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 238000007781 pre-processing Methods 0.000 claims abstract description 11
- 239000013598 vector Substances 0.000 claims description 24
- 238000013519 translation Methods 0.000 claims description 17
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000011478 gradient descent method Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000000034 method Methods 0.000 abstract description 24
- 230000008569 process Effects 0.000 abstract description 11
- 238000012545 processing Methods 0.000 abstract description 7
- 238000004891 communication Methods 0.000 abstract description 4
- 230000007547 defect Effects 0.000 abstract description 4
- 239000011159 matrix material Substances 0.000 description 9
- 230000004807 localization Effects 0.000 description 7
- 238000004088 simulation Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 239000003795 chemical substances by application Substances 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012804 iterative process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- 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
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/36—Input/output arrangements for on-board computers
- G01C21/3626—Details of the output of route guidance instructions
- G01C21/3629—Guidance using speech or audio output, e.g. text-to-speech
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Health & Medical Sciences (AREA)
- Audiology, Speech & Language Pathology (AREA)
- General Health & Medical Sciences (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于方位的移动机器人协同校正算法，包括以下步骤：定位体记录观测信息，所述观测信息包括目标体发送的播报信息以及定位体自身的坐标信息；定位体接收观测信息并对观测信息进行预处理；根据预处理后得到的观测值通过迭代计算不断校正，从而实现定位体的精准定位，采用上述技术方案，无需依赖固定的参考位置和连续通讯，有利于实际运用，扩展了文献中对移动节点处理不足的缺陷，并且通过预处理过程中的基于最小二乘法的拟合算法和适用于动态的基于扩展卡尔曼滤波的噪声处理算法，进行精准的校正，大大减小了定位偏差，实现定位体的精准定位。
Description
技术领域
本发明涉及移动机器人定位技术领域，及一种同时依赖位置信息和角度信息的移动节点定位方法，具体指一种基于方位的移动机器人协同校正算法。
背景技术
定位任务广泛存在于军事和民用的多个领域，例如AGV导航、无人机驾驶和机器人SLAM等。这些先进技术的关键之一，就是如何利用有限的时空信息进行准确的定位。定位的目标体从静态单个物体发展到网络节点的协同定位，再发展到单个或多个移动节点的定位。定位技术将真实的物理空间和虚拟的信息空间联系起来，是开发信息物理系统(CPS)的必要手段。
较早的关于协同定位技术的研究大多关注的是基于距离的定位方法(Aspnes J,Whiteley W,and Yang Y R.A theory of network localization[J].IEEE Transactionson Mobile Computing,2006,5(12):1663–1678；Yingfei Diao,Zhiyun Lin,and MinyueFu.A Barycentric Coordinate Based Distributed Localization Algorithm forSensor Networks.IEEE Transactions on Signal Processing,62(18):4760–4771,September 2014；U.A.Khan,S.Kar,and J.M.F.Moura.Distributed Sensor Localizationin Random Environments Using Minimal Number of Anchor Nodes.IEEE Transactionson Signal Processing,57(5):2000–2016,May 2009)。这种方法几何意义直观明了，便于处理成数学规划问题。但这些数学规划问题通常是非线性和非凸的，优化问题很容易陷入局部平衡点(A.Saha and B.Sau.Network localization with noisy distances by non-convex optimization.2016International Conference on Wireless Communications,Signal Processing and Networking(WiSPNET),2016)。随着通信技术和视觉技术的发展，目标体方位信息的获取变得更加简单，例如采用光学相机很容易锁定目标的方位(N.Moshtagh,N.Michael,A.Jadbabaie,and K.Daniilidis.Vision-Based,DistributedControl Laws for Motion Coordination of Nonholonomic Robots.IEEE Transactionson Robotics,25(4):851–860,August 2009；R.Tron,J.Thomas,G.Loianno,K.Daniilidis,and V.Kumar.A Distributed Optimization Framework for Localization andFormation Control:Applications to VisionBased Measurements.IEEE ControlSystems Magazine,36(4):22–44,August 2016)。因此，近些年基于方位的定位理论和算法得到长足的发展(F.Arrigoni and A.Fusiello.Bearing-Based NetworkLocalizability:A Unifying View.IEEE Transactions on Pattern Analysis andMachine Intelligence,vol.41,no.9,pp.2049-2069,1Sept.2019)。Cornejo等提出了只需要角度信息的编队控制算法(Alejandro Cornejo,Andrew J.Lynch,Elizabeth Fudge,Siegfried Bilstein,Majid Khabbazian,and James McLurkin.Scale-free coordinatesfor multi-robot systems with bearing-only sensors.International Journal ofRobotics Research,32(12):1459–1474,October 2013)，Schoof解决了任意通信底图的编队控制问题，但仅限于二维空间(E.Schoof,A.Chapman,and M.Mesbahi.Bearing-compassformation control:A human-swarm interaction perspective.In 2014AmericanControl Conference,pages 3881–3886,June2014)，赵用角度刚性理论对基于方位的定位算法进行了总结概括，并将定位算法从低维空间推广到任意维度空间(Shiyu Zhao andDaniel Zelazo.Bearing Rigidity Theory and Its Applications for Control andEstimation of Network Systems:Life Beyond Distance Rigidity.IEEE ControlSystems,39(2):66–83,April 2019)。
然而，上述研究考虑的都是固定节点的定位问题，目前对移动节点的定位研究还不足。相关的文献(Jason N.Greenberg and Xiaobo Tan.Dynamic Optical Localizationof a Mobile Robot Using Kalman Filtering-Based Position Prediction.IEEE/ASMETransactions on Mechatronics,25(5):2483–2492,October 2020；J.Son andH.Ahn.Formation Coordination for the Propagation of a Group of Mobile Agentsvia Self-Mobile Localization.IEEE Systems Journal,9(4):1285–1298,December2015)仅仅采用两个固定的智能体测量移动智能体的角度的办法，该方法需要固定节点的参与，不能由移动节点自我实现。
发明内容
本发明根据现有技术的不足，提出一种基于方位的移动机器人协同校正算法，两个移动机器人的协同定位，一个机器人作为目标体具有自身的全局坐标信息并间歇性地向外广播自己的坐标信息，另一个机器人只有局部惯性定位系统，需要通过接受目标体广播的信息并测量其方位角来矫正自己的惯性坐标系，从而完成在全局坐标系中的自身定位。
为了解决上述技术问题，本发明的技术方案为：
一种基于方位的移动机器人协同校正算法，包括以下步骤：
A、定位体记录观测信息，所述观测信息包括目标体发送的播报信息以及定位体自身的坐标信息；
B、定位体接收观测信息并对观测信息进行预处理；
C、根据预处理后得到的观测值通过迭代计算不断校正更新，从而实现移动机器人的精确定位，
所述步骤A中限定目标体和定位体在二维平面内任意运行，且所述目标体间歇性发送坐标信息，
所述目标体发送的播报信息包括在全局坐标系∑t的坐标pt，所述定位体记录的坐标信息包括自身在局部惯性坐标系∑a中的位置pa、根据目标体发送的坐标信息测量得到目标体在其局部坐标系∑a中的方位角α，所述方位角α为观测到目标体的方向与局部坐标系x轴的夹角。
作为优选，所述步骤B将步骤A中n次观测的pt、pa、α形成几何关系，并求解坐标系∑a的原点到坐标系∑t的原点的平移向量F和两个坐标系x轴之间的旋转角θ。
作为优选，所述观测次数不少于4次。
作为优选，所述步骤B中包括如下步骤：
B1)将pt通过平移向量F和旋转角θ转换到∑a内的坐标
B2)经过n次观测后
形成了一个多边形，要调整参数F和θ使得多边形的各个顶点依次落在对应的由Pa＝[pa1，…，pan]T和ρ＝[α1，…，αn]T形成的射线上。
B3)由
和Pa＝[pa1，…，pan]T对应的各个点形成的向量的角度与ρ＝[α1，…，αn]T定义误差；
B4)当n≥4时，采用梯度下降法使得误差在有限观测条件下误差最小，从而得到接近真实值的平移向量
和旋转角
作为优选，所述步骤C中将步骤B4中得到的接近真实值的
和
进行扩展卡尔曼滤波从而得到定位体在全局坐标系中的坐标的估计值。
作为优选，所述步骤C中通过更多的观测信息消除噪声带来的误差。
作为优选，所述步骤C中还包括通过获得的平移向量F和旋转角θ的估计值更新定位体在全局坐标系中的估计值。
本发明具有以下的特点和有益效果：
采用上述技术方案，无需依赖固定的参考位置和连续通讯，有利于实际运用；扩展了文献中对移动节点处理不足的缺陷，并且通过预处理过程的基于最小二乘法的拟合算法和适用于动态的基于扩展卡尔曼滤波的噪声处理算法，进行精准的校正，大大减小了定位偏差，实现定位体的精准定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例中基于方位的校正算法流程图。
图2是本发明实施例中坐标系关系示意图。
图3是本发明实施例中三次观测几何约束示意图。
图4是本发明实施例中带有观测噪声的最小二乘算法仿真图。
图5是本发明实施例中带有观测噪声的扩展卡尔曼滤波算法仿真图。
图6是本发明实施例中有无预处理过程的仿真比较图。
具体实施方式
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。
本发明提供了一种基于方位的移动机器人协同校正算法，如图1所示，包括以下步骤：
A、定位体记录观测信息，所述观测信息包括目标体发送的播报信息以及定位体自动的坐标信息；
B、定位体接收观测信息并对观测信息进行预处理；
C、根据预处理后得到的观测值通过迭代计算不断校正更新，从而实现移动机器人的精确定位，
所述步骤A中限定目标体和定位体在二维平面内任意运行，且所述目标体间歇性发送坐标信息，
所述目标体发送的播报信息包括在全局坐标系Σt的坐标pt，所述定位体记录的坐标信息包括自身在局部惯性坐标系Σa中的位置pa、根据目标体发送的坐标信息测量得到目标体在其局部坐标系∑a中的方位角α，所述方位角α为观测到目标体的方向与局部坐标系x轴的夹角。
其中，所述步骤B将步骤A中n次观测的pt、pa、α形成几何关系，并求解坐标系∑a的原点到坐标系∑t的原点的平移向量F和两个坐标系x轴之间的旋转角θ。
可以理解的，本技术方案限定在二维空间中的定位，因此所有的坐标和向量都是二维的列向量，并且所有的角度都在[0，2π)的区间内。假定局部坐标系和全局坐标系的偏差可以用平移向量F和旋转角θ表示。在时刻f记录下的信息分别为{pti，pai，αi}。n次观测后能够得到数据Pt＝[pt1，…，ptn]T、Pa＝[pa1，…，pan]T和ρ＝[α1，…，αn]T。
上述技术方案中，经n次观测后能够得到数据Pt＝[pt1，…，ptn]T、Pa＝[pa1，…，Pan]T和ρ＝[α1，…，αn]T。局部坐标系和全局坐标系的偏差可以用平移向量F和旋转角θ趋向于真实位置，整个定位过程经过不断校正，实现精准定位。并且在整个定位过程中，无需依赖固定的参考位置和连续通讯，有利于实际运用，扩展了文献中对移动节点处理不足的缺陷，并且通过预处理过程中的基于最小二乘法的拟合算法和适用于动态的基于扩展卡尔曼滤波的噪声处理算法，进行精准的校正，大大减小了定位偏差，实现定位体的精度定位。
本发明的进一步设置，所述观测次数不少于4次。
可以理解的，若观测次数小于4次，由于数据不足，从而导致校正出现的误差和偏差较大，因此观测次数不少于4次，进而确保有足够的数据支撑最终的校正结果，减小误差。
本发明的进一步设置，所述步骤B中包括如下步骤：
B1)将pt通过平移向量F和旋转角θ转换到∑a内的坐标
可以理解的，由于记录的坐标pt是在全局坐标系中，不和pa在同一坐标系中，无法构建几何关系，因此，由图2的坐标系关系示意图，可将全局坐标系中的坐标pt经过如下转换至局部坐标系中：
其中，
为旋转矩阵。
B2)如图3所示，经过n次观测后
形成了一个多边形，要调整参数F和θ使得多边形的各个顶点依次落在对应的由Pa＝[pa1，…，pan]T和ρ＝[α1，…，αn]T形成的射线上。
B3)由
和Pa＝[pa1，…，pan]T对应的各个点形成的向量的角度与ρ＝[α1，…，αn]T定义误差；
可以理解的，经过n次观测后就可建立了几何约束。图中
为一个刚体，其形状保持不变，随着参数F和θ的改变在∑a中移动，进而找到唯一的一组解(F，θ)使得刚体的顶点分别落到每次观测形成的射线上。由此出发，可定义第i次观测在静态情况下的误差
ei＝Rpti+F-pai-κi·gi
其中κi为一个大于0的常数，gi＝tan-1αi。在预处理过程中，根据最小二乘法，定义最小二乘误差为
B4)进一步的，采用梯度下降法使得误差在有限观测条件下误差最小，从而得到接近真实值的
和
其中，
表示经过最小二乘法得到的平移向量的估计值，
表示经过最小二乘法得到的旋转角的估计值。
具体的，令κ＝[κ1，…，κn]T，定义未知量
为真实值的估计，则根据梯度下降法可知
具体的，设定观测次数n＝5，
由此可得，五次观测时移动机器人的坐标为
观测到的五次目标体播报的坐标为
五次期望的不带噪声的移动智能体对目标体观测角度为
真实的平移向量F*＝[34，58]T和旋转角θ*＝4.5。令被观测噪声v污染后的观测角为α＝α*+v＝[0.9882，1.4583，1.2136，0.7577，1.5724]。取初始值
得到的仿真图，如图4所示。其中(a)图展示了∑t中
的拟合轨迹，并且从结果可以看到在有噪声干扰的情况下拟合结果接近真实值。(b)图展示了最小二乘误差的变化趋势，随着仿真时间的增加，曲线趋向于水平，即偏差值减小。曲线可以看到由于观测噪声的存在，最小二乘误差并不能被完全消除，因此它只作为一个预处理过程，后续用扩展卡尔曼滤波算法来处理噪声。
本发明的进一步设置，所述步骤C中将步骤B4中得到的接近真实值的平移向量
和旋转角
进行扩展卡尔曼滤波从而得到定位体在全局坐标系中的坐标的估计值，所述步骤C中通过更多的观测信息消除噪声带来的误差，所述步骤C中还包括通过获得的平移向量F和旋转角θ的估计值更新定位体在全局坐标系中的估计值。
可以理解的，在动态情况下，因为不断有观测数据，此时采用滤波的方式，定义动态观测下的误差
在动态过程中采用扩展卡尔曼滤波的方法。现给出扩展卡尔曼滤波的基本框架如下
其中下标k代表迭代次数，Xk＝[FT，θ]T代表k时刻的状态向量，Zk＝αk代表估计的观测角，A是系统的状态转移矩阵，这里取A＝I3为三阶单位矩阵，α＝fk(Xk)＝tan-1gk为非线性观测方程，我们通过它来更新我们的模型。ωk和vk分别是系统的噪声向量和观测噪声向量，他们是正定的、对称的并且是不相关的零均值高斯噪声，其表达式为
E(ω)＝0，cov(ω)＝E(ωωT)＝M，
E(v)＝0，cov(v)＝E(vvT)＝N，E(ωvT)＝0.
为了处理非线性过程，扩展卡尔曼滤波用了非线性函数fk(Xk)的雅可比矩阵当作观测矩阵Ck。定义
和
其中下标x和y分别代表局部坐标系∑a的两个维度，则Ck可以表示为
这是经过了线性化之后的观测矩阵。用
表示由
得到的先验状态估计，则扩展卡尔曼滤波的迭代过程可以表示为
其中M是一个3阶的ωk的协方差矩阵，N是一个一阶的vk的协方差矩阵，他们分别代表着系统和观测的可靠性。Pk是卡尔曼估计误差协方差矩阵，Pk′是卡尔曼预测误差协方差矩阵，Gk是卡尔曼增益，I是单位矩阵。将B4)中得到的最小二乘解
和
作为扩展卡尔曼滤波的初值，即
就可以通过扩展卡尔曼滤波得到不断更新估计值达到消除误差的目的。
最后，局部坐标系∑a内的坐标可以通过补偿公式
转换到全局坐标系∑t中。
进一步的，经扩展卡尔曼滤波处理的仿真图，如图5所示，假定观测角α被一个0均值标准差为0.02rad的噪声v污染了，并且目标体在∑t中的(-50，50)的矩形区域内随机生成。令
M＝10-10×I3，N＝5×10-4，X0＝[50，50，3]T，P0＝A＝I3，θ*＝1，F*＝[45，20]T和pak＝[20cos(k/10)，20sin(k/10)]T+[k/20，k/20]T，仿真结果如图5所示。(a)图展示了目标体播报位置时的真实坐标和拟合坐标的区别。(b)和(c)分别展示了迭代过程中平移变量和旋转角的误差变化趋势，可以有较好的处理效果，能够快速的实现精准定位。
特别的，我们展示了预处理过程对于扩展卡尔曼滤波的重要性。如图6所示，假定观测角α被一个0均值标准差为0.02rad的噪声v污染了，并且目标体在∑t中的(-50，50)的矩形区域内随机生成。令M＝10-10×I3，N＝9×10-3，P0＝A＝I3，θ*＝1，F*＝[45，20]T和pak＝[20cos(k/10)，20sin(k/10)]T+[k/50，k/50]T。
取初始值偏差较大的X0＝[200，-200，4]T，不经过预处理直接进行卡尔曼滤波，则估计值与真实值的误差如图6(a)(b)所示。若经过预处理，得到
则令扩展卡尔曼滤波的初始估计值为X0＝[45.92，19.67，0.97]T，结果如图6(c)(d)所示，当迭代次数较多时，初始估计值偏差小时，误差小，可以有较好的处理效果，能够实现精准定位。
以上结合附图对本发明的实施方式作了详细说明，但本发明不限于所描述的实施方式。对于本领域的技术人员而言，在不脱离本发明原理和精神的情况下，对这些实施方式包括部件进行多种变化、修改、替换和变型，仍落入本发明的保护范围内。
Claims (7)
1.一种基于方位的移动机器人协同校正算法，其特征在于，包括以下步骤：
A、定位体记录观测信息，所述观测信息包括目标体发送的播报信息以及定位体的自身坐标信息；
B、定位体接收观测信息并对观测信息进行预处理；
C、根据预处理后得到的观测值通过迭代计算不断校正更新，从而实现移动机器人的精确定位，
所述步骤A中限定目标体和定位体在二维平面内任意运行，且所述目标体间歇性发送坐标信息，
所述目标体发送的播报信息包括在全局坐标系Σt的坐标pt，所述定位体记录的坐标信息包括自身在局部惯性坐标系Σa中的位置pa、根据目标体发送的坐标信息测量得到目标体在其局部坐标系Σa中的位置pa中的方位角α，所述方位角α为观测到目标体的方向与局部坐标系x轴的夹角。
2.根据权利要求1所述的基于方位的移动机器人协同校正算法，其特征在于，所述步骤B将步骤A中径n次观测的pt、pa、α形成几何关系，并求解坐标系Σa的原点到坐标系Σt的原点的平移向量F和两个坐标系x轴之间的旋转角θ。
3.根据权利要求2所述的基于方位的移动机器人协同校正算法，其特征在于，所述观测次数不少于4次。
4.根据权利要求2或3所述的基于方位的移动机器人协同校正算法，其特征在于，所述步骤B中包括如下步骤：
B1)将pt通过平移向量F和旋转角θ转换到Σa内的坐标
B2)经过n次观测后
形成了一个多边形，要调整参数F和θ使得多边形的各个顶点依次落在对应的由Pa＝[pa1,…,pan]T和ρ＝[α1,…,αn]T形成的射线上。
B3)由
和Pa＝[pa1,…,pan]T对应的各个点形成的向量的角度与ρ＝[α1,…,αn]T定义最小二乘误差；
B4)最小二乘误差在n≥4时采用梯度下降法得到接近真实值的平移向量
和旋转角
5.根据权利要求4所述的基于方位的移动机器人协同校正算法，其特征在于，所述步骤C中将步骤B4中得到的
和
进行扩展卡尔曼滤波从而得到定位体在全局坐标系中的坐标的估计值。
6.根据权利要求5所述的基于方位的移动机器人协同校正算法，其特征在于，所述步骤C中通过更多的观测信息消除噪声带来的误差。
7.根据权利要求5所述的基于方位的移动机器人协同校正算法，其特征在于，所述步骤C中还包括通过获得的平移向量F和旋转角θ的估计值更新定位体在全局坐标系中的估计值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111167717.1A CN113984073B (zh) | 2021-09-29 | 2021-09-29 | 一种基于方位的移动机器人协同校正算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111167717.1A CN113984073B (zh) | 2021-09-29 | 2021-09-29 | 一种基于方位的移动机器人协同校正算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113984073A true CN113984073A (zh) | 2022-01-28 |
| CN113984073B CN113984073B (zh) | 2024-05-28 |
Family
ID=79737658
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111167717.1A Active CN113984073B (zh) | 2021-09-29 | 2021-09-29 | 一种基于方位的移动机器人协同校正算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113984073B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107084714A (zh) * | 2017-04-29 | 2017-08-22 | 天津大学 | 一种基于RoboCup3D的多机器人协作目标定位方法 |
| CN110207691A (zh) * | 2019-05-08 | 2019-09-06 | 南京航空航天大学 | 一种基于数据链测距的多无人车协同导航方法 |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN111307155A (zh) * | 2020-03-04 | 2020-06-19 | 南京工程学院 | 双协作机器人初始定位测量装置及初始定位方法 |
| US20200363212A1 (en) * | 2017-09-04 | 2020-11-19 | Nidec Corporation | Mobile body, location estimation device, and computer program |
| US20210107147A1 (en) * | 2019-10-10 | 2021-04-15 | Ubtech Robotics Corp Ltd | Localization correction method for robot, computer readable storage medium, and robot |
| CN113324544A (zh) * | 2021-04-19 | 2021-08-31 | 北京化工大学 | 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 |
-
2021
- 2021-09-29 CN CN202111167717.1A patent/CN113984073B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107084714A (zh) * | 2017-04-29 | 2017-08-22 | 天津大学 | 一种基于RoboCup3D的多机器人协作目标定位方法 |
| US20200363212A1 (en) * | 2017-09-04 | 2020-11-19 | Nidec Corporation | Mobile body, location estimation device, and computer program |
| CN110207691A (zh) * | 2019-05-08 | 2019-09-06 | 南京航空航天大学 | 一种基于数据链测距的多无人车协同导航方法 |
| US20210107147A1 (en) * | 2019-10-10 | 2021-04-15 | Ubtech Robotics Corp Ltd | Localization correction method for robot, computer readable storage medium, and robot |
| CN110967017A (zh) * | 2019-11-22 | 2020-04-07 | 河南科技大学 | 一种用于双移动机器人刚体协作搬运的协同定位方法 |
| CN111307155A (zh) * | 2020-03-04 | 2020-06-19 | 南京工程学院 | 双协作机器人初始定位测量装置及初始定位方法 |
| CN113324544A (zh) * | 2021-04-19 | 2021-08-31 | 北京化工大学 | 一种基于图优化的uwb/imu的室内移动机器人协同定位方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113984073B (zh) | 2024-05-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8861785B2 (en) | Information processing device, information processing method and program | |
| CN106482736B (zh) | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 | |
| Caballero et al. | Homography based Kalman filter for mosaic building. applications to UAV position estimation | |
| CN111161179A (zh) | 一种基于法向量的点云平滑光顺滤波方法 | |
| GB2436740A (en) | Video navigation | |
| Westman et al. | Underwater AprilTag SLAM and calibration for high precision robot localization | |
| Caballero et al. | Unmanned aerial vehicle localization based on monocular vision and online mosaicking: a new mapping framework | |
| CN114111818A (zh) | 一种通用视觉slam方法 | |
| CN109443355B (zh) | 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法 | |
| CN112985391B (zh) | 一种基于惯性和双目视觉的多无人机协同导航方法和装置 | |
| Cristofalo et al. | Vision-based control for fast 3-d reconstruction with an aerial robot | |
| CN116681733B (zh) | 一种空间非合作目标近距离实时位姿跟踪方法 | |
| Luong et al. | Consistent ICP for the registration of sparse and inhomogeneous point clouds | |
| CN113984073A (zh) | 一种基于方位的移动机器人协同校正算法 | |
| CN112268564A (zh) | 一种无人机降落空间位置和姿态端到端估计方法 | |
| CN111207754A (zh) | 基于粒子滤波器的多机器人编队定位方法及机器人设备 | |
| Gaspar et al. | Model-based H2 adaptive filter for 3D positioning and tracking systems | |
| CN111121779A (zh) | 一种无人机所处飞行区域的实时检测方法 | |
| CN111145267A (zh) | 基于imu辅助的360度全景视图多相机标定方法 | |
| Pei et al. | A decorrelated distributed EKF-SLAM system for the autonomous navigation of mobile robots | |
| CN109474892B (zh) | 基于信息形式的强鲁棒传感器网络目标跟踪方法 | |
| JP2007034964A (ja) | カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定方法、装置、カメラ視点運動並びに３次元情報の復元及びレンズ歪パラメータの推定プログラム | |
| CN112800889A (zh) | 一种基于分布式矩阵加权融合高斯滤波的目标跟踪方法 | |
| Tian et al. | Robust sensor registration with the presence of misassociations and ill conditioning | |
| KR20220077951A (ko) | 영상 기반의 협력적 동시 측위 및 지도 작성 시스템 및 방법 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |