CN113670318B - 协同定位方法及定位系统 - Google Patents

协同定位方法及定位系统 Download PDF

Info

Publication number
CN113670318B
CN113670318B CN202110942359.0A CN202110942359A CN113670318B CN 113670318 B CN113670318 B CN 113670318B CN 202110942359 A CN202110942359 A CN 202110942359A CN 113670318 B CN113670318 B CN 113670318B
Authority
CN
China
Prior art keywords
unmanned vehicle
ultra
wireless communication
wideband wireless
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.)
Active
Application number
CN202110942359.0A
Other languages
English (en)
Other versions
CN113670318A (zh
Inventor
李志天
张海峰
邹旭东
郑帅康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Aerospace Information Research Institute of CAS
Original Assignee
Aerospace Information Research Institute of CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Aerospace Information Research Institute of CAS filed Critical Aerospace Information Research Institute of CAS
Priority to CN202110942359.0A priority Critical patent/CN113670318B/zh
Publication of CN113670318A publication Critical patent/CN113670318A/zh
Application granted granted Critical
Publication of CN113670318B publication Critical patent/CN113670318B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种协同定位方法及定位系统，所述定位方法包括：依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵；依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵；依据所述状态转移矩阵和所述测量矩阵进行融合定位，得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标。所述定位系统包括：第一无人驾驶车；第二无人驾驶车；所述第一无人驾驶车和所述第二无人驾驶车上分别安装有：惯性导航单元以及超宽带无线通信单元。

Description

协同定位方法及定位系统
技术领域
本发明涉及无人驾驶车定位技术领域，尤其涉及一种协同定位方法及定位系统。
背景技术
随着无人机、无人车等移动载具的发展，集群协同定位技术日益受到国内外的关注。集群协同技术具有工作范围大、可靠性高、可同时执行多重任务、整体效率高等优点，可用于灾害勘测和救援等领域。与单点定位不同，集群定位要求多个待定位节点在无任何先验信息(地图信息等)、无外部设施辅助(通讯基站等)的工况下，仅仅依托系统内各节点自身搭载传感器所获取的信息独立完成全部节点的定位任务。
目前而言，无人移动载具协同定位主要还是依赖于卫星信号差分定位(GNSS-RTK)方式，这不仅要求无人移动载具之间要保持固定的几何形态，而且所依赖的高精度RTK硬件成本高；除依托卫星信号的技术途径外，其他集群定位技术还如主从式相对定位方案、基于机器视觉的协同定位方案等。但此类方案在实际应用层面都有其固有缺陷：主从式相对定位要求在所有节点之间预先定义主机节点，当主机节点发生通信故障时，整个集群定位系统将瘫痪，安全性低；基于机器视觉的协同定位，受相机视野的约束，而且无法处理移动目标遮挡的问题，在动态环境下容易受到干扰导致定位精度大大降低。
发明内容
(一)要解决的技术问题
针对现有的技术问题本发明提供一种协同定位方法及定位系统，用于至少部分解决以上技术问题。
(二)技术方案
本发明示例性的公开一种协同定位方法，应用于无人驾驶车群，所述无人驾驶车群至少包括第一无人驾驶车和第二无人驾驶车，所述定位方法包括：
依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵；依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵；其中，所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息通过在所述第一无人驾驶车和第二无人驾驶车上分别安装的超宽带无线通信标签进行采集；依据所述状态转移矩阵和所述测量矩阵进行融合定位，得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标。
根据本公开的实施例，依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵，包括：
通过所述动作信息建立状态转移方程；根据所述状态转移方程建立所述状态转移矩阵。
根据本公开的实施例，所述动作信息包括：
vk、θyaw、ωk以及αk；
其中，通过所述动作信息建立所述状态转移方程，包括：以时间间隔划分观测点，在每个所述观测点依据动作信息进行迭代修正；
所述状态转移方程，采用下式表示：
式(1)表征根据第k次的结果预测得到第k+1次的预测值；
其中，pk表示所述第一无人驾驶车相对于所述第二无人驾驶车的位置坐标，vk表示所述第一无人驾驶车相对于所述第二无人驾驶车的速度，θyaw表示所述第一无人驾驶车相对于所述第二无人驾驶车的偏航角，σf表示加速度计的零偏，σw表示陀螺仪的零偏，ωk表示所述第一无人驾驶车相对于所述第二无人驾驶车的角速度；和/>表示高斯噪声；
αk表示所述第一无人驾驶车相对于所述第二无人驾驶车的的加速度；所述αk采用下式计算：
ak＝ak i-ak j 式(2)
ak i＝Ri k(fk-δf,k-wa)-g 式(3)
式(2)和式(3)中，表示由所述无人驾驶车的体坐标系到参考坐标系的二维旋转矩阵,i表示所述第一无人驾驶车的第i个超宽带无线通信标签,j表示所述第二无人驾驶车的第j个超宽带无线通信标签,fk表示加速度计在k时刻的输入,g表示重力加速度；将所述状态转移方程采用矩阵形式表示成所述状态转移矩阵，所述状态转移矩阵，采用下式表示：
根据本公开的实施例，依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵，包括：对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签的距离进行采集，并作为一个观测元素，多个所述观测元素生成一组观测向量，依据所述观测向量建立观测方程；
所述观测方程用于对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签的距离进行约束。
根据本公开的实施例，所述第一无人驾驶车和所述第二无人驾驶车上分别安装四个所述超宽无线通信标签，对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签分别进行测距，共采集十六个所述观测元素，依据十六个所述观测元素生成观测向量；依据所述观测向量建立的所述观测方程，采用下式表示：
基于所述观测方程更新观测向量建立所述测量矩阵，所述测量矩阵为雅克比矩阵，采用下式表示：
式(5)和式(6)中，表示所述观测元素；pk表示所述第一无人驾驶车相对于所述第二无人驾驶车的位置坐标，/>表示第m个所述无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>表示第n个所述无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>第m个所述无人驾驶车中的第i个所述超宽带无线通信标签的初始坐标；/>第n个所述无人驾驶车中的第j个所述超宽带无线通信标签的初始坐标；/>为观测噪声；xk表示所述无人驾驶车在水平方向的坐标系中x轴的位置，yk表示无人驾驶车在水平方向的坐标系中y轴的位置；其中，/>和/>为提前标定的值，/>由所用设备的参数决定。
根据本公开的实施例，其中，依据所述状态转移矩阵和所述测量矩阵进行融合定位，得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标，包括：
根据上一次的状态Xk计算本次的状态预测值，采用下式表示：
式(7)中，uk包括加速度fk和角速度ωk，表征在k时刻的输入；
根据上一次的状态协方差矩阵Pk计算本次的预测值与真实值之间的状态协方差矩阵所述/>采用下式表示：
其中，F表示所述状态转移矩阵；Q表示过程噪声的协方差矩阵，为人为设定值；
计算本次的卡尔曼增益Kk：
其中，H表示将预测状态映射到观测模型的雅克比矩阵；R为观测噪声矩阵，为人为设定值；
根据本次的观测模型得到当前的状态量最优估计：
其中，Zk表示所述超宽带无线通信标签的相对距离的测量值；
更新状态协方差矩阵：
其中，Pk为本次更新后的状态协方差矩阵，Pk＝(xk,yk)表征所述第一无人驾驶车相对于第二无人驾驶车之间相对的估计坐标。
作为本发明的另一个方面，还示例性的公开了一种协同定位系统，包括：
第一无人驾驶车；第二无人驾驶车，所述第一无人驾驶车和所述第二无人驾驶车上分别安装有：惯性导航单元，用于获取所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息；以及超宽带无线通信单元，用于获取所述第一无人驾驶车相对于所述第二无人驾驶车的位置信息；所述超宽带无线通信单元包括多个超宽带无线通信标签，通过所述第一无人驾驶车和所述第二无人驾驶车上的所述超宽带无线通信标签的距离计算所述位置信息，同一台所述无人驾驶车上的多个所述超宽带无线通信标签均匀布置，以每个所述超宽带无线通信标签的安装位置为顶点形成一个多边形，所述多边形的中心和所述惯性导航单元的安装位置重合。
根据本公开的实施例，所述惯性导航单元至少包括：
加速度计，用于获取所述第一无人驾驶车和所述第二无人驾驶车的相对加速度；以及陀螺仪，用于获取所述第一无人驾驶车和所述第二无人驾驶车的相对角速度。
根据本公开的实施例，所述超宽带无线通信单元，包括：
至少四个所述超宽带无线通信标签。
根据本公开的实施例，所述超宽带无线通信标签的数量为四个，其中，
所述第一无人驾驶车和所述第二无人驾驶车上分别均布四个点位，同一台所述无人驾驶车上的四个所述点位间隔设置在水平面内形成一个正方形的四个顶点，每个所述点位安装有一个所述超宽带无线通信标签。
(三)有益效果
本发明提供了一种协同定位系统，包括超宽带无线通信单元和惯性导航单元。其中，超宽带无线通信单元用于获取两个无人驾驶车的距离信息，惯性导航单元用于获取上述两个无人驾驶车的动作信息。基于距离信息和动作信息进行融合定位，进而得到一个无人驾驶车相对于另一个无人驾驶车的坐标信息。当无人驾驶车群中的某台无人驾驶车出现故障时，其他无人驾驶车依然可通过相互之间的协同定位进行正常工作。较现有技术而言，本申请的协同定位系统安全性更高，可靠性也更强。
本发明还提供一种协同定位方法，基于超宽带无线通信单元和惯性导航单元进行配合，与现有技术中的所采用的高精度RTK相比，本方法支持在GNSS拒止的环境(如室内、地下或其他GNSS拒止工况)也能正常工作，且成本更低。与现有技术中的其他集群定位技术相比，本方法不需要固定的基站，因此大大拓宽了定位场景的范围，且加入惯性导航单元后可以观测到移动节点的方向信息；不存在视野的限制，不依赖环境纹理。
附图说明
图1是根据本发明的一种示例性实施例的协同定位方法的流程图；
图2是根据本发明的一种示例性实施例的协同定位系统的结构示意图；
图3是应用本发明定位方法的一种示例性实施例的无人驾驶车群的相对轨迹的示意图；
图4a-4e是图3所示实施例中真值和估计值的对比图，其中，图4a是UGV2相对于UGV1偏航角度的对比图，图4b是UGV2相对于UGV1 X轴速度的对比图，图4c是UGV2相对于UGV1 Y轴速度的对比图，图4d是UGV2相对于UGV1 X轴向位移的对比图，图4e是UGV2相对于UGV1 Y轴向位移的对比图；
图5是应用本发明定位方法的另一种示意性实施例的无人驾驶车群的相对轨迹示意图；
图6是图5所示实施例中无人驾驶车群的相对位置示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。
在此使用的术语仅仅是为了描述具体实施例，而并非意在限制本公开。在此使用的术语“包括”、“包含”等表明了所述特征、步骤、操作和/或部件的存在，但是并不排除存在或添加一个或多个其他特征、步骤、操作或部件。
在此使用的所有术语(包括技术和科学术语)具有本领域技术人员通常所理解的含义，除非另外定义。应注意，这里使用的术语应解释为具有与本说明书的上下文相一致的含义，而不应以理想化或过于刻板的方式来解释。
在使用类似于“A、B和C等中至少一个”这样的表述的情况下，一般来说应该按照本领域技术人员通常理解该表述的含义来予以解释(例如，“具有A、B和C中至少一个的系统”应包括但不限于单独具有A、单独具有B、单独具有C、具有A和B、具有A和C、具有B和C、和/或具有A、B、C的系统等)。在使用类似于“A、B或C等中至少一个”这样的表述的情况下，一般来说应该按照本领域技术人员通常理解该表述的含义来予以解释(例如，“具有A、B或C中至少一个的系统”应包括但不限于单独具有A、单独具有B、单独具有C、具有A和B、具有A和C、具有B和C、和/或具有A、B、C的系统等)。
本发明提供了一种协同定位方法，应用于无人驾驶车群，所述无人驾驶车群至少包括第一无人驾驶车和第二无人驾驶车。
定位方法包括：
依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵；
依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵；其中，所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息通过在所述第一无人驾驶车和第二无人驾驶车上分别安装的超宽带无线通信标签进行采集；
依据所述状态转移矩阵和所述测量矩阵进行融合定位，得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标。
图1是根据本发明的一种示例性实施例的协同定位方法的流程图。
在一种示例性实施例中，如图1所示：第一无人驾驶车相对于第二无人驾驶车的动作信息通过在第一无人驾驶车和第二无人驾驶车上分别安装的惯性导航单元(IMU)进行采集；待动作信息采集后需对惯性导航单元数据进行预处理，建立状态转移矩阵。
在一种示例性实施例中，如图1所示：第一无人驾驶车相对于第二无人驾驶车的距离信息通过在第一无人驾驶车和第二无人驾驶车上分别安装的超宽带无线通信单元(UWB)中的多个超宽带无线通信标签进行采集；待距离信息采集后需对超宽带无线通信单元数据进行预处理，建立测量矩阵。
在一种示例性实施例中，如图1所示：依据状态转移矩阵和测量矩阵进行融合定位，包括：EKF参数初始化；三边定位最小二乘解；状态X、协方差P预测；计算卡尔曼增益K；以及依据状态X，对协方差P进行更新，进而得到第一无人驾驶车相对于第二无人驾驶车的估计坐标。
在一种示例性实施例中，依据第一无人驾驶车相对于第二无人驾驶车的动作信息建立状态转移矩阵，包括：
通过动作信息建立状态转移方程；
根据状态转移方程建立状态转移矩阵。
在一种示例性实施例中，动作信息包括：vk、θyaw、ωk以及αk；
其中，通过动作信息建立状态转移方程，包括：
以时间间隔划分观测点，在每个观测点依据动作信息进行迭代修正；
状态转移方程，采用下式表示：
式(1)表征根据第k次的结果预测得到第k+1次的预测值；
其中，pk表示第一无人驾驶车相对于第二无人驾驶车的位置坐标，vk表示第一无人驾驶车相对于第二无人驾驶车的速度，θyaw表示第一无人驾驶车相对于第二无人驾驶车的偏航角，σf表示加速度计的零偏，σw表示陀螺仪的零偏，ωk表示第一无人驾驶车相对于第二无人驾驶车的角速度；和/>表示高斯噪声；
αk表示第一无人驾驶车相对于第二无人驾驶车的的加速度；
αk采用下式计算：
ak＝ak i-ak j 式(2)
ak i＝Ri k(fk-δf,k-wa)-g 式(3)
式(2)和式(3)中，表示由无人驾驶车的体坐标系到参考坐标系的二维旋转矩阵,i表示第一无人驾驶车的第i个超宽带无线通信标签,j表示第二无人驾驶车的第j个超宽带无线通信标签,fk表示加速度计在k时刻的输入,g表示重力加速度；
将状态转移方程采用矩阵形式表示成状态转移矩阵，
状态转移矩阵，采用下式表示：
在一种示例性实施例中，依据第一无人驾驶车相对于第二无人驾驶车的距离信息建立测量矩阵，包括：
对第一无人驾驶车上的每个超宽带无线通信标签相对于第二无人驾驶车上的每个超宽带无线通信标签的距离进行采集，并作为一个观测元素，多个观测元素生成一组观测向量，依据观测向量建立观测方程；
观测方程用于对第一无人驾驶车上的每个超宽带无线通信标签相对于第二无人驾驶车上的每个超宽带无线通信标签的距离进行约束。
在一种示例性实施例中，第一无人驾驶车和第二无人驾驶车上分别安装四个超宽带无线通信标签，
对第一无人驾驶车上的每个超宽带无线通信标签相对于第二无人驾驶车上的每个超宽带无线通信标签分别进行测距，共采集十六个观测元素，依据十六个观测元素生成观测向量；
依据观测向量建立的观测方程，采用下式表示：
基于观测方程更新观测向量建立测量矩阵，测量矩阵为雅克比矩阵，采用下式表示：
式(5)和式(6)中，表示观测元素；pk表示第一无人驾驶车相对于第二无人驾驶车的位置坐标，/>表示第m个无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>表示第n个无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>第m个无人驾驶车中的第i个超宽带无线通信标签的初始坐标；/>第n个无人驾驶车中的第j个超宽带无线通信标签的初始坐标；/>为观测噪声；xk表示无人驾驶车在水平方向的坐标系中x轴的位置，yk表示无人驾驶车在水平方向的坐标系中y轴的位置；
其中，和/>为提前标定的值，/>由所用设备的参数决定。
在一种示例性实施例中，依据状态转移矩阵和测量矩阵进行融合定位，得到第一无人驾驶车相对于第二无人驾驶车的估计坐标，包括：
以相对初始速度为0，作为EKF算法的状态初始值对EKF参数进行初始化；
其中，初始位置为采用三边定位算法解算出的p0。
根据上一次的状态Xk计算本次的状态预测值，采用下式表示：
式(7)中，uk包括加速度fk和角速度ωk，表征在k时刻的输入；
根据上一次的状态协方差矩阵Pk计算本次的预测值与真实值之间的状态协方差矩阵采用下式表示：
其中，F表示状态转移矩阵；Q表示过程噪声的协方差矩阵，为人为设定值；
计算本次的卡尔曼增益Kk：
其中，H表示将预测状态映射到观测模型的雅克比矩阵；R为观测噪声矩阵，为人为设定值；
根据本次的观测模型得到当前的状态量最优估计：
其中，Zk表示超宽带无线通信标签的相对距离的测量值；
更新状态协方差矩阵：
其中，Pk为本次更新后的状态协方差矩阵，Pk＝(xk,yk)表征第一无人驾驶车相对于第二无人驾驶车之间相对的估计坐标。
图2是根据本发明的一种示例性实施例的协同定位系统的结构示意图。
一种协同定位系统，如图2所示，包括：
第一无人驾驶车3；
第二无人驾驶车4，第一无人驾驶车和第二无人驾驶车上分别安装有：
惯性导航单元1，用于获取第一无人驾驶车相对于第二无人驾驶车的动作信息(Tij表征第i无人驾驶车相对于第j无人驾驶车的动作信息)。
超宽带无线通信单元2，用于获取第一无人驾驶车相对于第二无人驾驶车的位置信息(Tij表征第i无人驾驶车相对于第j无人驾驶车的体坐标系之间的转换矩阵)。
超宽带无线通信单元包括多个超宽带无线通信标签，通过第一无人驾驶车和第二无人驾驶车上的超宽带无线通信标签的距离计算位置信息，同一台无人驾驶车上的多个超宽带无线通信标签均匀布置，以每个超宽带无线通信标签的安装位置为顶点形成一个多边形，多边形的中心和惯性导航单元的安装位置重合。
在一种示例性实施例中，相邻两个超宽带无线通信标签的间距应不小于1米。采用上述实施方式有利于提升超宽带无线通信单元定位的精确性。
在一种示例性实施例中，惯性导航单元至少包括：
加速度计，用于获取第一无人驾驶车和第二无人驾驶车的相对加速度；以及陀螺仪，用于获取第一无人驾驶车和第二无人驾驶车的相对角速度。
在一种示例性实施例中，超宽带无线通信单元，包括：至少四个超宽带无线通信标签。
在一种示例性实施例中，超宽带无线通信标签的数量为四个，其中，第一无人驾驶车和第二无人驾驶车上分别均布四个点位，同一台无人驾驶车上的四个点位间隔设置在水平面内形成一个正方形的四个顶点，每个点位安装有一个超宽带无线通信标签。
在一种示例性实施例中，惯性导航单元应布置在正方形的中心位置。
在一种示例性实施例中，上述四个超宽带无线通信标签安装在无人驾驶车的一角，四个超宽带无线通信标签位于一个边长为1米的正方形的四个顶点。其中，超宽带无线通信标签(UWB)的测距误差为0.1米，惯性导航单元(IMU)的角速度零偏为0.01，加速度零偏为0.01，惯性导航单元噪声为标准差为0.01的高斯白噪声。
以下分别以静态实验和动态实验作为一种示例性的实施例。
静态实验：
以第一无人驾驶车(UGV1)的初始位置作为原点，UGV1的几何中心和原点重合，UGV1保持静止状态。第二无人驾驶车(UGV2)围绕UGV1进行匀速圆周运动，其中圆周的半径为10米，角速度为0.01π。
图3是应用本发明定位方法的一种示例性实施例的无人驾驶车群的相对轨迹。
如图3所示，以UGV1的初始位置作为坐标原点建立XY坐标系。将UGV2移动过程中的轨迹真值6，EKF估计真值5以及三边定位解7在图中进行显示。
图4a-4e是图3所示实施例中真值和EKF估计真值的对比图。
如图4a-4e所示，基于上述静态实验，截取100米的轨迹范围。
如图4a所示，以100米的轨迹作为横轴，以UGV2相对于UGV1的偏航角为纵轴建立坐标系。在UGV2行进100米的状态下，大致环绕UGV1行进一圈。将轨迹真值5和EKF估计真值6在坐标系中进行比较。
如图4b所示，以100米的轨迹作为横轴，以图3中UGV2相对于UGV1的X向的位移作为纵轴，将轨迹真值5和EKF估计真值6在坐标系中进行比较。
如图4c所示，以100米的轨迹作为横轴，以图3中UGV2相对于UGV1的Y向的位移作为纵轴，将轨迹真值5和EKF估计真值6在坐标系中进行比较。
如图4d所示，以100米的轨迹作为横轴，以图3中UGV2相对于UGV1的X向的速度作为纵轴，将真值和EKF估计真值在坐标系中进行显示和比较。
如图4e所示，以100米的轨迹作为横轴，以图3中UGV2相对于UGV1的Y向的速度作为纵轴，将轨迹真值5和EKF估计真值6在坐标系中进行比较。
综合上述图4a至4e所示内容，可知EKF优化后的轨迹误差(RMSE)为0.08m，三边定位的轨迹误差为0.24m。上述经度优于GPS平均定位精度。
动态实验：以第一无人驾驶车(UGV1)的初始位置作为原点，UGV1的几何中心和原点重合，UGV1朝向坐标的X轴的正方向做匀速直线运动。第二无人驾驶车(UGV2)围绕UGV1进行匀速圆周运动，其中圆周的半径为10米，角速度为0.01π。
图5是应用本发明定位方法的另一种示意性实施例的无人驾驶车群的相对轨迹示意图。
如图5所示，第一无人驾驶车的移动轨迹如标号8，第二无人驾驶车的移动轨迹如标号9。以UGV1移动的中间点作为坐标原点建立XY坐标系。UGV1由X轴的-5位置移动至+5位置，UGV2环绕UGV1行驶一周。
图6是图5所示实施例中无人驾驶车群的相对位置示意图。
如图6所示，将UGV2移动过程中的轨迹真值，EKF估计真值以及三边定位解在图中进行显示和比较。
在UGV1和UGV2都进行运动的状态下，优化后相对位置的轨迹误差(RMSE)0.38m，仍然优于GPS定位的平均精度。
可知采用本公开实施例中涉及的协同定位方法可满足相应的精度要求。
以上对本公开的实施例进行了描述。但是，这些实施例仅仅是为了说明的目的，而并非为了限制本公开的范围。尽管在以上分别描述了各实施例，但是这并不意味着各个实施例中的措施不能有利地结合使用。本公开的范围由所附权利要求及其等同物限定。不脱离本公开的范围，本领域技术人员可以做出多种替代和修改，这些替代和修改都应落在本公开的范围之内。

Claims (5)

1.一种协同定位方法，应用于无人驾驶车群，所述无人驾驶车群至少包括第一无人驾驶车和第二无人驾驶车，所述定位方法包括：
依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵；
依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵；其中，所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息通过在所述第一无人驾驶车和第二无人驾驶车上分别安装的超宽带无线通信标签进行采集；
依据所述状态转移矩阵和所述测量矩阵进行融合定位，得到所述第一无人驾驶车相对于所述第二无人驾驶车的估计坐标；
其中，依据所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息建立状态转移矩阵，包括：
通过所述动作信息建立状态转移方程；
根据所述状态转移方程建立所述状态转移矩阵；
其中，所述动作信息包括：vk、θyaw、ωk以及αk；
其中，通过所述动作信息建立所述状态转移方程，包括：
以时间间隔划分观测点，在每个所述观测点依据动作信息进行迭代修正；
所述状态转移方程，采用下式表示：
式(1)表征根据第k次的结果预测得到第k+1次的预测值；
其中，pk表示所述第一无人驾驶车相对于所述第二无人驾驶车的位置坐标，vk表示所述第一无人驾驶车相对于所述第二无人驾驶车的速度，θyaw表示所述第一无人驾驶车相对于所述第二无人驾驶车的偏航角，σf表示加速度计的零偏，σw表示陀螺仪的零偏，ωk表示所述第一无人驾驶车相对于所述第二无人驾驶车的角速度；和/>表示高斯噪声；
αk表示所述第一无人驾驶车相对于所述第二无人驾驶车的加速度；
表示由所述无人驾驶车的体坐标系到参考坐标系的二维旋转矩阵；
将所述状态转移方程采用矩阵形式表示成所述状态转移矩阵，
所述状态转移矩阵，采用下式表示：
其中，依据所述第一无人驾驶车相对于所述第二无人驾驶车的距离信息建立测量矩阵，包括：
对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签的距离进行采集，并作为一个观测元素，多个所述观测元素生成一组观测向量，依据所述观测向量建立观测方程；
所述观测方程用于对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签的距离进行约束；
其中，所述第一无人驾驶车和所述第二无人驾驶车上分别安装四个所述超宽带无线通信标签，
对所述第一无人驾驶车上的每个所述超宽带无线通信标签相对于所述第二无人驾驶车上的每个所述超宽带无线通信标签分别进行测距，共采集十六个所述观测元素，依据十六个所述观测元素生成观测向量；
依据所述观测向量建立的所述观测方程，采用下式表示：
基于所述观测方程更新观测向量建立所述测量矩阵，所述测量矩阵为雅克比矩阵，采用下式表示：
式(5)和式(6)中，表示所述观测元素；pk表示所述第一无人驾驶车相对于所述第二无人驾驶车的位置坐标，/>表示第m个所述无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>表示第n个所述无人驾驶车在体坐标系到参考系下的二维旋转矩阵；/>第m个所述无人驾驶车中的第i个所述超宽带无线通信标签的初始坐标；/>第n个所述无人驾驶车中的第j个所述超宽带无线通信标签的初始坐标；/>为观测噪声；xk表示所述无人驾驶车在水平方向的坐标系中x轴的位置，yk表示无人驾驶车在水平方向的坐标系中y轴的位置；
其中，和/>为提前标定的值，/>由所用设备的参数决定。
2.一种应用权利要求1所述的协同定位方法的协同定位系统，包括：
第一无人驾驶车；
第二无人驾驶车；
所述第一无人驾驶车和所述第二无人驾驶车上分别安装有：
惯性导航单元，用于获取所述第一无人驾驶车相对于所述第二无人驾驶车的动作信息；以及
超宽带无线通信单元，用于获取所述第一无人驾驶车相对于所述第二无人驾驶车的位置信息；所述超宽带无线通信单元包括：多个超宽带无线通信标签，通过所述第一无人驾驶车和所述第二无人驾驶车上的所述超宽带无线通信标签的距离计算所述位置信息，同一台所述无人驾驶车上的多个所述超宽带无线通信标签均匀布置，以每个所述超宽带无线通信标签的安装位置为顶点形成一个多边形，所述多边形的中心和所述惯性导航单元的安装位置重合。
3.根据权利要求2所述的定位系统，其中，所述惯性导航单元至少包括：
加速度计，用于获取所述第一无人驾驶车和所述第二无人驾驶车的相对加速度；以及
陀螺仪，用于获取所述第一无人驾驶车和所述第二无人驾驶车的相对角速度。
4.根据权利要求2所述的定位系统，其中，所述超宽带无线通信单元，包括：
至少四个所述超宽带无线通信标签。
5.根据权利要求4所述的定位系统，其中，所述超宽带无线通信标签的数量为四个，其中，
所述第一无人驾驶车和所述第二无人驾驶车上分别均布四个点位，同一台所述无人驾驶车上的四个所述点位间隔设置在水平面内形成一个正方形的四个顶点，每个所述点位安装有一个所述超宽带无线通信标签。
CN202110942359.0A 2021-08-17 2021-08-17 协同定位方法及定位系统 Active CN113670318B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110942359.0A CN113670318B (zh) 2021-08-17 2021-08-17 协同定位方法及定位系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110942359.0A CN113670318B (zh) 2021-08-17 2021-08-17 协同定位方法及定位系统

Publications (2)

Publication Number Publication Date
CN113670318A CN113670318A (zh) 2021-11-19
CN113670318B true CN113670318B (zh) 2024-04-16

Family

ID=78543335

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110942359.0A Active CN113670318B (zh) 2021-08-17 2021-08-17 协同定位方法及定位系统

Country Status (1)

Country Link
CN (1) CN113670318B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116540617B (zh) * 2023-07-06 2023-09-08 中国科学院空天信息创新研究院 一种引入重量变量控制的无人农机协同工作控制方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103135117A (zh) * 2013-02-05 2013-06-05 中国人民解放军国防科学技术大学 一种分散式多机器人协同定位算法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN110244715A (zh) * 2019-05-23 2019-09-17 西安理工大学 一种基于超宽带技术的多移动机器人高精度协同跟踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103135117A (zh) * 2013-02-05 2013-06-05 中国人民解放军国防科学技术大学 一种分散式多机器人协同定位算法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN110244715A (zh) * 2019-05-23 2019-09-17 西安理工大学 一种基于超宽带技术的多移动机器人高精度协同跟踪方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Cooperative Relative Positioning of Mobile Users by Fusing IMU Inertial and UWB Ranging Information;Ran Liu 等;2017 IEEE International Conference on Robotics and Automation;全文 *
基于分散式EKF 的多机器人协同定位;卓书芳 等;重庆理工大学学报( 自然科学);第32卷(第3期);全文 *

Also Published As

Publication number Publication date
CN113670318A (zh) 2021-11-19

Similar Documents

Publication Publication Date Title
Wang et al. A simple and parallel algorithm for real-time robot localization by fusing monocular vision and odometry/AHRS sensors
EP2752725B1 (en) Augmented mobile platform localization
CN110554376A (zh) 用于运载工具的雷达测程法
CN112066982B (zh) 一种在高动态环境下的工业移动机器人定位方法
CN108426576B (zh) 基于标识点视觉导航与sins的飞行器路径规划方法及系统
CN106969784B (zh) 一种并发建图定位与惯性导航的组合误差融合系统
Zheng et al. An optimization-based UWB-IMU fusion framework for UGV
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN111426320A (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
JP2022027593A (ja) 可移動機器の測位方法、装置及び可移動機器
CN110160528B (zh) 一种基于角度特征识别的移动装置位姿定位方法
CN113670318B (zh) 协同定位方法及定位系统
CN110686684B (zh) 一种小天体环绕探测器光学协同定轨方法
CN113093759A (zh) 基于多传感器信息融合的机器人编队构造方法及系统
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN115060268A (zh) 一种机房融合定位方法、系统、设备及存储介质
JP5748174B2 (ja) 移動体の相対姿勢計測方法と装置
Liu et al. Multi-robot cooperative localization with range-only measurement by UWB
Noaman et al. Landmarks exploration algorithm for mobile robot indoor localization using VISION sensor
CN116972844A (zh) 基于ArUco阵列的移动机器人室内定位系统及方法
CN113375626B (zh) 一种基于惯性装置的空间矢量相对平行度测量方法
CN113721188B (zh) 拒止环境下的多无人机自身定位与目标定位方法
CN113156450B (zh) 无人机上的主动旋转激光雷达系统及其控制方法
CN113470342B (zh) 一种自运动估计的方法及装置
CN117635721A (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
GR01 Patent grant
GR01 Patent grant