WO2022242075A1

WO2022242075A1 - Robot positioning method and apparatus, robot and readable storage medium

Info

Publication number: WO2022242075A1
Application number: PCT/CN2021/131678
Authority: WO
Inventors: 何婉君; 熊友军; 赵嘉珩; 黄明强
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-05-19
Filing date: 2021-11-19
Publication date: 2022-11-24
Also published as: CN113137967B; CN113137967A

Abstract

A robot positioning method and apparatus (10), a robot (100) and a readable storage medium. The robot (100) comprises a communication module (130), wherein the communication module (130) is used for receiving pulse signals sent by positioning tags (A). The positioning method comprises: estimating an estimated posture of a robot (100) at the current moment on the basis of inertial data of the robot (100) (S300); determining an angle reliable tag in the positioning tags (A) by means of an estimated angle which corresponds to the estimated posture of the robot (100) at the current moment (S400); and determining the actual posture of the robot (100) at the current moment on the basis of a relative distance and a relative angle that correspond to the angle reliable tag (S500). By using the characteristic of a high-frequency inertial measurement unit collecting inertial data at a high frequency, inertial data of when the robot (100) moves is collected by using an inertial measurement unit, and positioning based on the high-frequency inertial measurement unit is fused with positioning based on a communication module-tag, so as to realize the accurate positioning of the robot (100) which moves rapidly.

Description

一种机器人定位方法、装置、机器人和可读存储介质A robot positioning method, device, robot and readable storage medium

相关申请的交叉引用Cross References to Related Applications

本申请要求于2021年05月19日提交中国专利局的申请号为2021105458957、名称为“一种机器人定位方法、装置、机器人和可读存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。This application claims the priority of the Chinese patent application with the application number 2021105458957 and the title "a robot positioning method, device, robot and readable storage medium" submitted to the China Patent Office on May 19, 2021, the entire content of which is passed References are incorporated in this application.

技术领域technical field

本申请涉及人工智能领域，尤其涉及一种机器人定位方法、装置、机器人和可读存储介质。The present application relates to the field of artificial intelligence, and in particular to a robot positioning method, device, robot and readable storage medium.

背景技术Background technique

定位技术是移动机器人导航的一项关键技术。通过定位装置对机器人进行实时精准定位，以使导航***能够根据机器人的精确定位，规划路径，进而控制机器人完成工作。Positioning technology is a key technology for mobile robot navigation. Real-time precise positioning of the robot through the positioning device, so that the navigation system can plan the path according to the precise positioning of the robot, and then control the robot to complete the work.

目前常用的机器人定位方式有固定轨道定位、基于虚拟轨道的视觉定位、基于深度学习的移动机器人视觉定位等，这些定位方式均存在不足之处。例如，固定轨道定位，需要在地面上铺设金属导线或磁钉的导轨，机器人在导轨上行走，但是，导轨不仅安装不方便，而且维护成本高；基于虚拟轨道的视觉定位，需要在地面上画引导线、或者铺设色带、或者铺设二维码导航带，但是，虚拟轨道容易被灰尘或可移动的遮挡物遮盖，导致机器人无法准确定位；基于深度学习的移动机器人视觉定位方法，算法复杂、计算难度大，该方法过于理想，实用性较低。At present, the commonly used robot positioning methods include fixed orbit positioning, visual positioning based on virtual orbit, and mobile robot visual positioning based on deep learning, etc., all of which have shortcomings. For example, fixed track positioning requires laying metal wires or magnetic nails on the ground, and the robot walks on the guide rails. However, the guide rails are not only inconvenient to install, but also costly to maintain; visual positioning based on virtual tracks requires drawing on the ground. Guide lines, or laying ribbons, or laying two-dimensional code navigation belts, but the virtual track is easily covered by dust or movable occluders, making the robot unable to locate accurately; the visual positioning method of mobile robots based on deep learning has complex algorithms, The calculation is difficult, the method is too ideal, and the practicability is low.

申请内容application content

鉴于上述问题，本申请提出一种机器人定位方法、装置、机器人和可读存储介质。In view of the above problems, the present application proposes a robot positioning method, device, robot and readable storage medium.

本申请提出一种机器人定位方法，所述方法包括：The present application proposes a robot positioning method, the method comprising:

获取机器人和各个定位标签之间的相对距离和相对角度；Obtain the relative distance and relative angle between the robot and each positioning tag;

获取所述机器人的惯性数据；obtaining inertial data of the robot;

根据惯性数据估计所述机器人在当前时刻的估计位姿；Estimating the estimated pose of the robot at the current moment according to the inertial data;

根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；determining a predetermined first number of angle-reliable tags from the respective positioning tags based on a comparison result of the estimated angle in the estimated pose and each of the relative angles;

根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。The actual pose of the robot at the current moment is determined according to the relative distances and relative angles corresponding to the predetermined first number of angle-reliable tags.

本申请所述的机器人定位方法，所述机器人的预定位置上安装有第一天线和第二天线，所述第一天线和所述第二天线用于接收所述各个定位标签发送的脉冲信号，所述获取机器人和各个定位标签之间的相对距离和相对角度，包括：In the robot positioning method described in the present application, a first antenna and a second antenna are installed on the predetermined position of the robot, and the first antenna and the second antenna are used to receive the pulse signals sent by the positioning tags, The relative distance and relative angle between the acquisition robot and each positioning tag includes:

获取第i个定位标签与所述第一天线之间的第一距离，i≤I，I为所述定位标签的总数；Obtain the first distance between the i-th positioning tag and the first antenna, i≤I, where I is the total number of the positioning tags;

获取第i个定位标签与所述第二天线之间的第二距离；Obtain a second distance between the i-th positioning tag and the second antenna;

确定第i个定位标签发送的脉冲信号到达所述第一天线和所述第二天线的到达相位差；determining the arrival phase difference between the first antenna and the second antenna when the pulse signal sent by the i-th positioning tag arrives;

根据所述第一距离、所述到达相位差和所述第二距离确定所述机器人与第i个定位标签之间的相对距离和相对角度。Determine the relative distance and relative angle between the robot and the i-th positioning tag according to the first distance, the arrival phase difference and the second distance.

本申请所述的机器人定位方法，所述惯性数据包括所述机器人的线加速度和角速度，所述根据所述惯性数据估计所述机器人在当前时刻的估计位姿，包括：In the robot positioning method described in the present application, the inertial data includes the linear acceleration and angular velocity of the robot, and the estimation of the estimated pose of the robot at the current moment according to the inertial data includes:

根据所述惯性数据中上一时刻的线加速度估计上一时刻到当前时刻内所述机器人的位移变化量；Estimating the displacement variation of the robot from the previous moment to the current moment according to the linear acceleration at the previous moment in the inertial data;

根据所述惯性数据中上一时刻所述角速度估计上一时刻到当前时刻内所述机器人的角度变化量；Estimating the angular variation of the robot from the previous moment to the current moment according to the angular velocity at the previous moment in the inertial data;

根据所述位移变化量和角度变化量估计所述机器人在当前时刻的估计位姿。Estimating the estimated pose of the robot at the current moment according to the displacement variation and the angle variation.

本申请所述的机器人定位方法，所述根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签，包括：In the robot positioning method described in the present application, the determination of a predetermined first number of angle-reliable tags from the positioning tags according to the comparison results of the estimated angle in the estimated pose and each of the relative angles includes:

计算第i个定位标签对应的相对角度与所述估计角度之差的绝对值，i≤I，I为所述定位标签的总数；Calculate the absolute value of the difference between the relative angle corresponding to the i-th positioning tag and the estimated angle, i≤I, and I is the total number of the positioning tags;

判断所述绝对值是否大于预设的角度阈值；judging whether the absolute value is greater than a preset angle threshold;

若小于等于所述角度阈值，则第i个定位标签可靠；If it is less than or equal to the angle threshold, the i-th positioning tag is reliable;

若大于所述角度阈值，则第i个定位标签不可靠。If it is greater than the angle threshold, the i-th positioning tag is unreliable.

本申请所述的机器人定位方法，所述根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿，包括：In the robot positioning method described in the present application, determining the actual pose of the robot at the current moment according to the relative distance and relative angle corresponding to the predetermined first number of reliable angle tags includes:

根据所述预定第一数目个角度可靠标签对应的相对角度计算相对角度对应的均值和相对角度对应的方差；Calculate the mean value corresponding to the relative angle and the variance corresponding to the relative angle according to the relative angle corresponding to the predetermined first number of reliable angle tags;

根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签；determining a predetermined second number of distance reliable tags according to the relative distances corresponding to the predetermined first number of angle reliable tags;

根据所述预定第二数目个距离可靠标签对应的相对距离度计算相对距离对应的均值和相对距离对应的方差；Calculate the mean value corresponding to the relative distance and the variance corresponding to the relative distance according to the relative distance degree corresponding to the predetermined second number of distance reliable tags;

将所述相对角度对应的均值、所述相对距离对应的均值、所述相对角度对应的方差、所述相对距离对应的方差输入至预设的无迹卡尔曼滤波器，以获取所述机器人在当前时刻的实际位姿。Input the mean value corresponding to the relative angle, the mean value corresponding to the relative distance, the variance corresponding to the relative angle, and the variance corresponding to the relative distance into a preset unscented Kalman filter to obtain the The actual pose at the current moment.

本申请所述的机器人定位方法，所述根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签，包括：In the robot positioning method described in the present application, the determination of the predetermined second number of distance reliable tags according to the relative distance corresponding to the predetermined first number of angle reliable tags includes:

将所述预定第一数目个角度可靠标签分为K组，每组包括至少3个角度可靠标签；dividing the predetermined first number of angle-reliable tags into K groups, each group including at least 3 angle-reliable tags;

根据每组的角度可靠标签确定所述机器人的位置；determining the position of the robot from each set of angularly reliable tags;

计算K个位置对应的均值和K个位置对应的方差；Calculate the mean value corresponding to K positions and the variance corresponding to K positions;

若K个位置对应的方差大于预设的方差阈值，则将K个位置中距离所述K个位置对应的均值最远的位置删除，并重新计算K-1个位置对应的均值和K-1个位置对应的方差，直至剩余位置对应的方差小于等于预设的方差阈值，剩余位置对应的定位标签为距离可靠标签；If the variance corresponding to K positions is greater than the preset variance threshold, delete the position farthest from the mean value corresponding to the K positions among the K positions, and recalculate the mean value and K-1 position corresponding to K-1 positions The variance corresponding to each position until the variance corresponding to the remaining positions is less than or equal to the preset variance threshold, and the positioning labels corresponding to the remaining positions are distance reliable labels;

若K个位置对应的方差小于等于预设的方差阈值，则K个位置对应的定位标签为距离可靠标签。If the variances corresponding to the K positions are less than or equal to the preset variance threshold, the positioning labels corresponding to the K positions are distance reliable labels.

本申请提出一种机器人定位装置，所述机器人包括通信模块，所述通信模块用于接收各个定位标签发送的脉冲信号，所述装置包括：The present application proposes a robot positioning device, the robot includes a communication module, the communication module is used to receive pulse signals sent by each positioning tag, and the device includes:

获取单元，用于获取机器人和各个定位标签之间的相对距离和相对角度；还用于获取所述机器人的惯性数据；The acquisition unit is used to acquire the relative distance and relative angle between the robot and each positioning tag; it is also used to acquire the inertial data of the robot;

估计单元，用于根据惯性数据估计所述机器人在当前时刻的估计位姿；an estimation unit, configured to estimate the estimated pose of the robot at the current moment according to the inertial data;

筛选单元，用于根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；a screening unit, configured to determine a predetermined first number of angle-reliable tags from the respective positioning tags according to a comparison result of the estimated angle in the estimated pose and each of the relative angles;

定位单元，用于根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。The positioning unit is configured to determine the actual pose of the robot at the current moment according to the relative distances and relative angles corresponding to the predetermined first number of angle-reliable tags.

本申请所述的机器人定位装置，所述通信模块包括第一天线和第二天线，所述第一天线和所述第二天线用于接收所述各个定位标签发送的脉冲信号，所述获取机器人和各个定位标签之间的相对距离和相对角度，包括：In the robot positioning device described in this application, the communication module includes a first antenna and a second antenna, and the first antenna and the second antenna are used to receive pulse signals sent by each positioning tag, and the acquisition robot and the relative distance and relative angle between each positioning label, including:

本申请提出一种机器人，包括存储器、处理器和通信模块，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行所述的机器人定位方法，所述通信模块用于接收各个定位标签发送的脉冲信号。The present application proposes a robot, including a memory, a processor, and a communication module, the memory stores a computer program, and the computer program executes the robot positioning method when running on the processor, and the communication module is used for Receive the pulse signal sent by each positioning tag.

本申请提出一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行所述的机器人定位方法。The present application proposes a readable storage medium, which stores a computer program, and executes the robot positioning method when the computer program runs on a processor.

本申请公开的机器人定位方法，利用机器人的通信模块和各个定位标签之间的载波通信确定机器人和各个定位标签之间的位置关系，基于机器人的惯性数据估计机器人当前时刻的估计位姿，通过机器人当前时刻的估计位姿的估计角度确定各个定位标签中的角度可靠标签，基于角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。本申请利用高频率的惯性测量单元(Inertial measurement unit，IMU)具有高频率采集惯性数据的特点，使用惯性测量单元采集机器人移动时的惯性数据，将IMU定位与通信模块-标签定位融合，以对快速移动的机器人实现准确定位。本申请与现有的固定轨道定位和基于虚拟轨道的视觉定位相比实用性更强，无需安装导轨、无需专门人员对导轨和引导线进行维护；本申请与现有的基于深度学习的移动机器人视觉定位方法相比计算量更小，算法简单，便于计算。The robot positioning method disclosed in this application uses the communication module of the robot and the carrier communication between each positioning tag to determine the positional relationship between the robot and each positioning tag, and estimates the estimated pose of the robot at the current moment based on the inertial data of the robot. The estimated angle of the estimated pose at the current moment determines the reliable angle tags in each positioning tag, and determines the actual pose of the robot at the current moment based on the relative distance and relative angle corresponding to the reliable angle tags. This application utilizes the high-frequency inertial measurement unit (IMU) which has the characteristics of high-frequency collection of inertial data, uses the inertial measurement unit to collect the inertial data when the robot moves, and integrates the IMU positioning with the communication module-tag positioning to Fast-moving robots achieve precise positioning. Compared with the existing fixed track positioning and visual positioning based on virtual track, this application is more practical, and does not need to install guide rails, and does not require specialized personnel to maintain guide rails and guide lines; this application is compatible with existing mobile robots based on deep learning Compared with the visual positioning method, the calculation amount is smaller, the algorithm is simple, and the calculation is convenient.

附图说明Description of drawings

为了更清楚地说明本申请的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对本申请保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。In order to illustrate the technical solution of the present application more clearly, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the application, and therefore should not be regarded It is regarded as a limitation on the scope of protection of the present application. In the respective drawings, similar components are given similar reference numerals.

图1示出了本申请提出的一种机器人定位方法的流程示意图；Fig. 1 shows a schematic flow chart of a robot positioning method proposed by the present application;

图2示出了本申请提出的一种确定机器人与定位标签之间的相对距离和相对角度的方法示意图；Fig. 2 shows a schematic diagram of a method for determining the relative distance and relative angle between the robot and the positioning tag proposed by the present application;

图3示出了本申请提出的一种确定机器人与定位标签之间的相对距离和相对角度的原理示意图；Fig. 3 shows a schematic diagram of the principle of determining the relative distance and relative angle between the robot and the positioning tag proposed by the present application;

图4示出了本申请提出的一种确定机器人估计位姿的方法示意图；Fig. 4 shows a schematic diagram of a method for determining the estimated pose of a robot proposed by the present application;

图5示出了本申请提出的一种确定角度可靠标签的方法示意图；Fig. 5 shows a schematic diagram of a method for determining an angle-reliable label proposed by the present application;

图6示出了本申请提出的一种确定角度可靠标签的判断过程示意图；FIG. 6 shows a schematic diagram of a judgment process for determining an angle-reliable label proposed by the present application;

图7示出了本申请提出的一种确定机器人实际位姿的方法示意图；Fig. 7 shows a schematic diagram of a method for determining the actual pose of a robot proposed by the present application;

图8示出了本申请提出的一种确定距离可靠标签的方法示意图；Fig. 8 shows a schematic diagram of a method for determining a distance-reliable label proposed by the present application;

图9示出了本申请提出的一种机器人定位装置的结构示意图；Fig. 9 shows a schematic structural diagram of a robot positioning device proposed in the present application;

图10示出了本申请提出的一种机器人的结构示意图。Fig. 10 shows a schematic structural diagram of a robot proposed in this application.

主要元件符号说明：Description of main component symbols:

10-机器人定位装置；11-获取单元；12-估计单元；13-筛选单元；14-定位单元；100-机器人；110-存储器；120-处理器；130-通信模块；131-第一天线；132-第二天线。10-robot positioning device; 11-acquisition unit; 12-estimation unit; 13-screening unit; 14-positioning unit; 100-robot; 110-memory; 120-processor; 130-communication module; 131-first antenna; 132 - Second antenna.

具体实施方式Detailed ways

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all of them.

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。The components of the embodiments of the application generally described and illustrated in the figures herein may be arranged and designed in a variety of different configurations. Accordingly, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative efforts belong to the scope of protection of the present application.

在下文中，可在本申请的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。Hereinafter, the terms "comprising", "having" and their cognates that may be used in various embodiments of the present application are only intended to represent specific features, numbers, steps, operations, elements, components or combinations of the foregoing, And it should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components or combinations of the foregoing or adding one or more features, numbers, steps, operations, elements, components or a combination of the foregoing possibilities.

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。In addition, the terms "first", "second", "third", etc. are only used for distinguishing descriptions, and should not be construed as indicating or implying relative importance.

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本申请的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本申请的各种实施例中被清楚地限定。Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art to which various embodiments of the application belong. The terms (such as those defined in commonly used dictionaries) will be interpreted as having the same meaning as the contextual meaning in the relevant technical field and will not be interpreted as having an idealized meaning or an overly formal meaning, Unless clearly defined in the various embodiments of the present application.

本申请公开的机器人定位方法基于超宽带(Ultra Wide Band，UWB)无线载波通信技术、到达相位差(phasedifferenceofarrival，PDOA)原理和高频率的惯性测量单元(Inertial measurement unit，IMU)，以较低的计算量对在室内快速移动的机器人进行精准定位。The robot positioning method disclosed in this application is based on ultra-wideband (Ultra Wide Band, UWB) wireless carrier communication technology, phase difference of arrival (phase difference of arrival, PDOA) principle and high-frequency inertial measurement unit (Inertial measurement unit, IMU). The amount of calculation is used to accurately position the robot moving quickly indoors.

超宽带(Ultra Wide Band，UWB)技术是一种无线载波通信技术，它不采用正弦载波，而是利用纳秒级的非正弦波窄脉冲传输数据，因此其所占的频谱范围很宽。UWB技术具有***复杂度低，发射信号功率谱密度低，对信道衰落不敏感，截获能力低，定位精度高等优点，尤其适用于机器人的室内定位。在室内定位中，UWB能够实现较高的测距精度。Ultra Wide Band (UWB) technology is a wireless carrier communication technology. It does not use sinusoidal carrier, but uses nanosecond-level non-sinusoidal narrow pulse to transmit data, so it occupies a wide spectrum range. UWB technology has the advantages of low system complexity, low power spectral density of transmitted signals, insensitivity to channel fading, low interception capability, and high positioning accuracy. It is especially suitable for indoor positioning of robots. In indoor positioning, UWB can achieve high ranging accuracy.

基于PDOA测距的UWB设备能够输出距离和角度值，基于PDOA-UWB的定位***能够完成机器人位置和朝向的估计。但由于时间带宽有限，PDOA-UWB只能以10Hz左右的频率进行测距，在室内机器人快速运动时，由飞行时间测量和相位测量估计的位置和朝向都会存在较大的误差。使用PDOA-UWB和IMU进行数据融合定位，结合高频率的IMU数据和低频率的UWB位姿测量，能够获得高频率的更准确的融合定位数据，有利于机器人在快速运动下的精准定位。The UWB device based on PDOA ranging can output distance and angle values, and the positioning system based on PDOA-UWB can complete the estimation of the robot's position and orientation. However, due to the limited time bandwidth, PDOA-UWB can only perform ranging at a frequency of about 10 Hz. When the indoor robot moves rapidly, the position and orientation estimated by the time-of-flight measurement and phase measurement will have large errors. Using PDOA-UWB and IMU for data fusion positioning, combined with high-frequency IMU data and low-frequency UWB pose measurement, can obtain high-frequency and more accurate fusion positioning data, which is conducive to the precise positioning of the robot under fast movement.

实施例1Example 1

本申请的一个实施例，如图1所示，提出一种机器人定位方法包括以下步骤：One embodiment of the present application, as shown in Figure 1, proposes a robot positioning method including the following steps:

S100：获取机器人和各个定位标签之间的相对距离和相对角度。S100: Obtain the relative distance and relative angle between the robot and each positioning tag.

在机器人的活动区域预先布设多个定位标签，机器人的特定位置安装有通信模块，各个定位标签实时的向机器人的通信模块发送脉冲信息(无线载波)。机器人根据通信模块接收的脉冲信息可以确定自身和各个定位标签之间的相对距离和相对角度。Multiple positioning tags are pre-arranged in the active area of the robot, and a communication module is installed at a specific position of the robot. Each positioning tag sends pulse information (wireless carrier) to the communication module of the robot in real time. According to the pulse information received by the communication module, the robot can determine the relative distance and relative angle between itself and each positioning tag.

进一步的，考虑到UWB具有穿透力强、功耗低、抗多径效果好、安全性高、***复杂度低、能提供精确定位精度等优点，可以利用PDOA-UWB定位技术确定机器人和各个定位标签之间的相对距离和相对角度。PDOA-UWB通信模块放置于机器人上，需要定位的场景放置多个PDOA-UWB标签，机器人可以通过串口获取每对标签和通信模块的距离和角度数据。Further, considering that UWB has the advantages of strong penetrating power, low power consumption, good anti-multipath effect, high security, low system complexity, and can provide precise positioning accuracy, the PDOA-UWB positioning technology can be used to determine the robot and each Position the relative distance and relative angle between labels. The PDOA-UWB communication module is placed on the robot, and multiple PDOA-UWB tags are placed in the scene that needs to be positioned. The robot can obtain the distance and angle data of each pair of tags and the communication module through the serial port.

可以理解，通信模块可以安装至机器人的中心位置，在利用通信模块和各个标签对机器人进行定位时，通信模块的位置即可代表机器人的位置。示范性的，可以将通信模块的一个通信天线的中心点作为坐标原点，在机器人静止时，以机器人朝向作为X轴的正方向建立坐标系(Y轴垂直与X轴，Y轴的朝向可不做限定)，在该坐标系下确定机器人和各个定位标签之间的相对距离和相对角度。其中相对距离是机器人(即坐标原点)与定位标签坐标之间距离，可以根据脉冲的传播速度和传播时间确定，相对角度是机器人(即坐标原点)与定位标签坐标之间连线与X轴正方向坐标轴连线的夹角。It can be understood that the communication module can be installed at the center of the robot, and when the robot is positioned by using the communication module and each tag, the position of the communication module can represent the position of the robot. Exemplarily, the center point of a communication antenna of the communication module can be used as the origin of coordinates, and when the robot is stationary, a coordinate system can be established with the orientation of the robot as the positive direction of the X-axis (the Y-axis is perpendicular to the X-axis, and the orientation of the Y-axis can be omitted. Defined), in this coordinate system, determine the relative distance and relative angle between the robot and each positioning tag. The relative distance is the distance between the robot (that is, the origin of the coordinates) and the coordinates of the positioning tag, which can be determined according to the propagation speed and propagation time of the pulse. The angle between the lines connecting the direction coordinate axes.

S200：获取所述机器人的惯性数据。S200: Obtain inertial data of the robot.

机器人的惯性数据包括所述机器人在某一时刻的线加速度和角速度。可以利用高频率的惯性测量单元获取机器人在某一时刻的线加速度和角速度。可以理解，高频率的惯性测量单元是测量物体三轴姿态角或角速率以及加速度的装置。一般情况下，高频率的惯性测量单元IMU内会装有三轴的陀螺仪和三个方向的加速度计，陀螺仪可以获取机器人在某一时刻的角速度，加速度计可以获取机器人在某一时刻的线加速度。机器人可以通过串口接收IMU获取的惯性数据。The inertial data of the robot includes the linear acceleration and angular velocity of the robot at a certain moment. A high-frequency inertial measurement unit can be used to obtain the linear acceleration and angular velocity of the robot at a certain moment. It can be understood that a high-frequency inertial measurement unit is a device for measuring the three-axis attitude angle or angular rate and acceleration of an object. Generally, the high-frequency inertial measurement unit IMU will be equipped with a three-axis gyroscope and an accelerometer in three directions. The gyroscope can obtain the angular velocity of the robot at a certain moment, and the accelerometer can obtain the line of the robot at a certain moment. acceleration. The robot can receive the inertial data obtained by the IMU through the serial port.

S300：根据惯性数据估计所述机器人在当前时刻的估计位姿。S300: Estimate the estimated pose of the robot at the current moment according to the inertial data.

可以对相邻时刻获取的惯性数据中的线加速度进行积分，以估计相邻时刻机器人的速度和位移变化量，对相邻时刻获取的惯性数据中的角速度进行积分，以估计相邻时刻机器人的旋转变化量。进而将变化量和旋转变化量累加到上一时刻机器人的估计位姿位移，以估计所述机器人在当前时刻的估计位姿。The linear acceleration in the inertial data acquired at adjacent moments can be integrated to estimate the velocity and displacement of the robot at adjacent moments, and the angular velocity in the inertial data acquired at adjacent moments can be integrated to estimate the robot’s velocity at adjacent moments. Rotation delta. Furthermore, the change amount and the rotation change amount are added to the estimated pose displacement of the robot at the last moment, so as to estimate the estimated pose of the robot at the current moment.

S400：根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签。S400: Determine a predetermined first number of angle-reliable tags from the positioning tags according to a comparison result of the estimated angle in the estimated pose and each of the relative angles.

从各个定位标签中选择角度可靠标签，可以依次遍历各个定位标签对应的相对角度，将各个相对角度和估计位姿的估计角度进行比较，计算各个相对角度与估计位姿的估计角度之间的差值，将差值的绝对值按照从小到大的顺序排序，选取排序靠前的N个绝对值对应的相对角度，且排序靠前的N个绝对值应小于预设的角度阈值，N为预定第一数目。可以理解，排序靠前的N个绝对值对应的相对角度与机器人的朝向最为接近，因此，N个相对角度对应的定位标签可以作为角度可靠标签。Select an angle-reliable label from each positioning label, you can traverse the relative angles corresponding to each positioning label in turn, compare each relative angle with the estimated angle of the estimated pose, and calculate the difference between each relative angle and the estimated angle of the estimated pose value, sort the absolute values of the differences in ascending order, select the relative angles corresponding to the top N absolute values, and the top N absolute values should be less than the preset angle threshold, and N is the predetermined first number. It can be understood that the relative angles corresponding to the top N absolute values are closest to the orientation of the robot. Therefore, the positioning tags corresponding to the N relative angles can be used as angle reliable tags.

S500：根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。S500: Determine the actual pose of the robot at the current moment according to the relative distances and relative angles corresponding to the predetermined first number of angle-reliable tags.

进一步的，筛选N个角度可靠标签，根据角度可靠标签对应的相对距离和相对角度从N个角度可靠标签中选取M个距离可靠标签，可以理解，N≥M，然后，根据N个角度可靠标签和M个距离可靠标签确定所述机器人在当前时刻的实际位姿。Further, N angle reliable tags are screened, and M distance reliable tags are selected from N angle reliable tags according to the relative distance and relative angle corresponding to the angle reliable tags. It can be understood that N≥M, and then, according to the N angle reliable tags and M distance reliable tags to determine the actual pose of the robot at the current moment.

可以理解，上述步骤S100可以放在步骤S300后面执行，本申请对确定估计位姿和获取机器人和各个定位标签之间的相对距离和相对角度之间的顺序不做限定。It can be understood that the above step S100 can be performed after step S300, and the present application does not limit the order of determining the estimated pose and obtaining the relative distance and relative angle between the robot and each positioning tag.

本实施例公开的机器人定位方法利用机器人的通信模块和各个定位标签之间的载波通信确定机器人和各个定位标签之间的位置关系，基于机器人的惯性数据估计机器人当前时刻的估计位姿，通过机器人当前时刻的估计位姿的估计角度确定各个定位标签中的角度可靠标签，基于角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。本技术方案利用高频率的惯性测量单元IMU具有高频率采集惯性数据的特点，使用惯性测量单元IMU采集机器人移动时的惯性数据，将IMU定位与通信模块-标签定位融合，以对快速移动的机器人实现准确定位。本技术方案与固定轨道定位和基于虚拟轨道的视觉定位相比实用性更强，无需安装导轨、无需专门人员对导轨和引导线进行维护；本技术方案与基于深度学习的移动机器人视觉定位方法相比计算量更小，算法简单，便于计算。The robot positioning method disclosed in this embodiment uses the carrier communication between the communication module of the robot and each positioning tag to determine the positional relationship between the robot and each positioning tag, and estimates the estimated pose of the robot at the current moment based on the inertial data of the robot. The estimated angle of the estimated pose at the current moment determines the reliable angle tags in each positioning tag, and determines the actual pose of the robot at the current moment based on the relative distance and relative angle corresponding to the reliable angle tags. This technical solution uses the high-frequency inertial measurement unit IMU to collect inertial data at high frequency, uses the inertial measurement unit IMU to collect the inertial data when the robot is moving, and integrates the IMU positioning with the communication module-tag positioning to accurately detect the fast-moving robot. achieve accurate positioning. Compared with fixed track positioning and visual positioning based on virtual track, this technical solution is more practical. It does not need to install guide rails, and does not require specialized personnel to maintain guide rails and guide lines; this technical solution is similar to the mobile robot visual positioning method based on deep learning. The calculation amount is smaller, the algorithm is simple, and it is easy to calculate.

实施例2Example 2

进一步的，可以在机器人的预定位置上安装第一天线和第二天线，第一天线和第二天线可以接收各个定位标签发送的脉冲信号，可以通过图2所示的步骤确定机器人和各个定位标签之间的相对距离和相对角度：Further, the first antenna and the second antenna can be installed on the predetermined position of the robot, the first antenna and the second antenna can receive the pulse signals sent by each positioning tag, and the robot and each positioning tag can be determined through the steps shown in Figure 2 The relative distance and relative angle between:

S110：获取第i个定位标签与所述第一天线之间的第一距离，i≤I，I为所述定位标签的总数。S110: Obtain a first distance between the i-th positioning tag and the first antenna, where i≤I, where I is the total number of the positioning tags.

S120：获取第i个定位标签与所述第二天线之间的第二距离。S120: Obtain a second distance between the i-th positioning tag and the second antenna.

S130：确定第i个定位标签发送的脉冲信号到达所述第一天线和所述第二天线的到达相位差。S130: Determine an arrival phase difference between the first antenna and the second antenna when the pulse signal sent by the i-th positioning tag arrives.

S140：根据所述第一距离、所述到达相位差和所述第二距离确定所述机器人与第i个定位标签之间的相对距离和相对角度。S140: Determine a relative distance and a relative angle between the robot and the i-th positioning tag according to the first distance, the arrival phase difference, and the second distance.

示范性的，如图3所示，O1表示机器人的第一天线，O2表示机器人的第二天线，在机器人上安装第一天线和第二天线时，需要保证第一天线和第二天线连线的射线可以指向机器人的朝向，即在机器人静止时，图3中的X轴的方向代表机器人的朝向(可以理解，在机器人朝向发生变化时，X坐标轴的正方向也随之变化)。进一步的，D表示O1和O2之间的距离，D已知，A表示定位标签，O1到A距离为R，O2到A距离为R-P，P可以通过A发出的脉冲信号到达O1和O2的到达相位差进行计算。可以理解，

表示A发出的脉冲信号到达O1和O2的到达相位差，λ表示脉冲信号的波长。 Exemplarily, as shown in Figure 3, O1 represents the first antenna of the robot, and O2 represents the second antenna of the robot. When installing the first antenna and the second antenna on the robot, it is necessary to ensure that the first antenna and the second antenna are connected The ray can point to the orientation of the robot, that is, when the robot is stationary, the direction of the X-axis in Figure 3 represents the orientation of the robot (it can be understood that when the orientation of the robot changes, the positive direction of the X-coordinate axis also changes). Further, D represents the distance between O1 and O2, D is known, A represents the positioning tag, the distance from O1 to A is R, and the distance from O2 to A is RP, and P can reach the arrival of O1 and O2 through the pulse signal sent by A The phase difference is calculated. understandable,

Indicates the arrival phase difference of the pulse signal sent by A to O1 and O2, and λ indicates the wavelength of the pulse signal.

进一步的，计算θ1，根据图3可知：O2到O1A的垂线段对应两个直角三角形，根据勾股定理可以推导：Further, calculate θ1. According to Figure 3, it can be seen that the vertical line segment from O2 to O1A corresponds to two right triangles. According to the Pythagorean theorem, it can be deduced:

(R-P)2-(R-(P+a))2＝D2-(P+a)2，则P+a＝(D2-P2+2RP)/2R，根据余弦定理进一步推导可得，

(RP)2-(R-(P+a))2=D2-(P+a)2, then P+a=(D2-P2+2RP)/2R can be further deduced according to the law of cosines,

由于，第一天线和第二天线安装在机器人的特定位置，在安装第一天线时可以将第一天线安装在机器人的中心位置，并保证第一天线和第二天线连线的射线可以指向机器人的朝向，以机器人的朝向作为X轴的正方向，以使机器人静止时，θ1可以作为机器人与定位标签A之间的相对角度，R可以作为机器人与定位标签A之间的相对距离。Since the first antenna and the second antenna are installed at a specific position of the robot, the first antenna can be installed at the center of the robot when the first antenna is installed, and the ray connecting the first antenna and the second antenna can be guaranteed to point to the robot Take the orientation of the robot as the positive direction of the X-axis, so that when the robot is stationary, θ1 can be used as the relative angle between the robot and the positioning tag A, and R can be used as the relative distance between the robot and the positioning tag A.

进一步的，根据图3所示的原理，分别确定机器人和各个定位标签之间的相对距离和相对角度，并将确定的各个相对距离和各个相对角度反馈给机器人，以使机器人可以根据各个相对距离和各个相对角度确定自身的实际位姿。Further, according to the principle shown in Figure 3, the relative distances and relative angles between the robot and each positioning tag are respectively determined, and the determined relative distances and relative angles are fed back to the robot, so that the robot can and various relative angles to determine its own actual pose.

实施例3Example 3

本申请的一个实施例，如图4所示，根据所述惯性数据估计所述机器人在当前时刻的估计位姿，包括以下步骤：In one embodiment of the present application, as shown in FIG. 4 , estimating the estimated pose of the robot at the current moment according to the inertial data includes the following steps:

S310：根据所述惯性数据中上一时刻的线加速度估计上一时刻到当前时刻内所述机器人的位移变化量。S310: Estimate the displacement variation of the robot from the previous moment to the current moment according to the linear acceleration at the previous moment in the inertial data.

示范性的，上一时刻可以表示为t-1时刻，当前时刻可以表示为t时刻，由于用于采集机器人惯性数据的IMU具有高频采集能力，t-1时刻与t时刻之间的间隔很短，因此，在利用t-1时刻的惯性数据估计t时刻的估计位姿时，可以有效降低估计误差，进而可以有效提高机器人的定位精度。Exemplarily, the previous moment can be expressed as t-1 moment, and the current moment can be expressed as t moment. Since the IMU used to collect robot inertial data has high-frequency acquisition capabilities, the interval between t-1 moment and t moment is very short. Therefore, when using the inertial data at time t-1 to estimate the estimated pose at time t, the estimation error can be effectively reduced, and the positioning accuracy of the robot can be effectively improved.

对t-1时刻获取的惯性数据中的线加速度a _t-1进行积分可以确定t时刻机器人的速度和位移变化量。示范性的，t时刻机器人的速度

t时刻机器人的位移变化量

Integrating the linear acceleration a _t-1 in the inertial data acquired at time t-1 can determine the velocity and displacement variation of the robot at time t. Exemplary, the speed of the robot at time t

The displacement change of the robot at time t

S320：根据所述惯性数据中上一时刻所述角速度估计上一时刻到当前时刻内所述机器人的角度变化量。S320: Estimate an angular variation of the robot from the previous moment to the current moment according to the angular velocity at the previous moment in the inertial data.

对t-1时刻获取的惯性数据中的线加速度w _t-1进行积分可以确定t时刻机器人的角度变化量。示范性的，t时刻机器人的角度变化量

Integrating the linear acceleration w _t-1 in the inertial data acquired at time t-1 can determine the angular change of the robot at time t. Exemplary, the angle change of the robot at time t

S330：根据所述位移变化量和角度变化量估计所述机器人在当前时刻的估计位姿。S330: Estimate the estimated pose of the robot at the current moment according to the displacement variation and the angle variation.

将位移变化量累加至上一时刻机器人的实际位姿对应的实际位置上，以估计当前时刻机器人的实际位姿对应的估计位置；将角度变化量累加至上一时刻机器人的实际位姿对应的实际角度上，以估计当前时刻机器人的实际位姿对应的估计角度，进而根据估计位置和估计角度估计机器人当前时刻的估计位姿。以使机器人可以根据IMU确定的当前时刻的估计位姿对机器人当前时刻的实际位姿进行定位。IMU的高频采集功能，可以对快速移动的机器人预先估计机器人当前时刻的估计位姿，并且保证估计位姿与实际位姿的偏差较小，进而保证对快速移动的机器人进行准确定位。Add the amount of displacement change to the actual position corresponding to the actual pose of the robot at the previous moment to estimate the estimated position corresponding to the actual pose of the robot at the current moment; add the amount of change in angle to the actual angle corresponding to the actual pose of the robot at the last moment On the basis of estimating the estimated angle corresponding to the actual pose of the robot at the current moment, and then estimating the estimated pose of the robot at the current moment according to the estimated position and estimated angle. So that the robot can locate the actual pose of the robot at the current moment according to the estimated pose at the current moment determined by the IMU. The high-frequency acquisition function of the IMU can pre-estimate the estimated pose of the robot at the current moment for a fast-moving robot, and ensure that the deviation between the estimated pose and the actual pose is small, thereby ensuring accurate positioning of the fast-moving robot.

实施例4Example 4

本申请的一个实施例，如图5所示，确定角度可靠标签包括以下步骤：In one embodiment of the present application, as shown in FIG. 5, determining an angle reliable label includes the following steps:

S410：计算第i个定位标签对应的相对角度与所述估计角度之差的绝对值，i≤I，I为所述定位标签的总数。S410: Calculate the absolute value of the difference between the relative angle corresponding to the i-th positioning tag and the estimated angle, i≤I, where I is the total number of the positioning tags.

S420：判断所述绝对值是否大于预设的角度阈值。S420: Determine whether the absolute value is greater than a preset angle threshold.

若小于等于所述角度阈值，则执行步骤S430；若大于所述角度阈值，则执行步骤S440。进而从I个定位标签中确定角度可靠标签，以利用角度可靠标签对机器人进行定位。If it is less than or equal to the angle threshold, execute step S430; if it is greater than the angle threshold, execute step S440. Then determine the angle reliable label from one positioning label, so as to use the angle reliable label to locate the robot.

S430：确定第i个定位标签可靠。S430: Determine that the i-th positioning tag is reliable.

S440：第i个定位标签不可靠。S440: The i-th positioning label is unreliable.

示范性的，如图6所示，O1表示机器人的第一天线，O2表示机器人的第二天线，X坐标轴的正方向代表基于通信模块-标签定位时，机器人对应的实时估计朝向，但是，由于基于通信模块-标签定位的时间带宽有限，例如，PDOA-UWB只能以10Hz左右的频率进行测距，在室内机器人快速运动时，由飞行时间测量和相位测量估计的位置和朝向都会存在较大的误差，因此，本实施例利用高频率的惯性测量单元确定的当前时刻的估计位姿中的估计角度θ0，通过将θ0与定位标签A之间的相对角度θ1之差的绝对值与预设的角度阈值进行比较，以确定定位标签A是否是角度可靠标签。利用该判别方法分别判断各个定位标签，已从各个定位标签中确定角度可靠标签。通过角度阈值对各个定位标签进行筛选，过滤去除角度误差大的定位标签，基于剩下的角度可靠标签对机器人进行定位，不仅有效降低机器人定位过程的计算量，而且还可以保证对快速移动的机器人进行准确定位。Exemplarily, as shown in Figure 6, O1 represents the first antenna of the robot, O2 represents the second antenna of the robot, and the positive direction of the X coordinate axis represents the real-time estimated orientation of the robot when positioning based on the communication module-tag. However, Due to the limited time bandwidth based on the communication module-tag positioning, for example, PDOA-UWB can only measure distance at a frequency of about 10 Hz. When the indoor robot moves quickly, the position and orientation estimated by the time-of-flight measurement and phase measurement will have a large gap. Therefore, in this embodiment, the estimated angle θ0 in the estimated pose at the current moment determined by the high-frequency inertial measurement unit is used, and the absolute value of the difference between θ0 and the relative angle θ1 between the positioning tag A and the predicted Compared with the set angle threshold to determine whether the positioning tag A is an angle reliable tag. Each location tag is judged separately by using the discrimination method, and the angle reliable tag has been determined from each location tag. Screen each positioning tag through the angle threshold, filter and remove the positioning tags with large angle errors, and locate the robot based on the remaining reliable angle tags, which not only effectively reduces the calculation amount of the robot positioning process, but also ensures fast-moving robots. for accurate positioning.

实施例5Example 5

本申请的一个实施例，如图7所示，确定机器人在当前时刻的实际位姿包括以下步骤：In one embodiment of the present application, as shown in Figure 7, determining the actual pose of the robot at the current moment includes the following steps:

S510：根据所述预定第一数目个角度可靠标签对应的相对角度计算相对角度对应的均值和相对角度对应的方差。S510: Calculate a mean value corresponding to the relative angle and a variance corresponding to the relative angle according to the relative angles corresponding to the predetermined first number of angle-reliable tags.

根据N个角度可靠标签对应的相对角度可以计算相对角度对应的均值和相对角度对应的方差。The mean value corresponding to the relative angle and the variance corresponding to the relative angle can be calculated according to the relative angles corresponding to the N angular reliable labels.

S520：根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签。S520: Determine a predetermined second number of distance reliable tags according to the relative distances corresponding to the predetermined first number of angle reliable tags.

S530：根据所述预定第二数目个距离可靠标签对应的相对距离度计算相对距离对应的均值和相对距离对应的方差。S530: Calculate the mean value corresponding to the relative distance and the variance corresponding to the relative distance according to the relative distance degrees corresponding to the predetermined second number of distance reliable tags.

S540：将所述相对角度对应的均值、所述相对距离对应的均值、所述相对角度对应的方差、所述相对距离对应的方差输入至预设的无迹卡尔曼滤波器，以获取所述机器人在当前时刻的实际位姿。S540: Input the mean value corresponding to the relative angle, the mean value corresponding to the relative distance, the variance corresponding to the relative angle, and the variance corresponding to the relative distance into a preset unscented Kalman filter to obtain the The actual pose of the robot at the current moment.

其中输入的所述相对角度对应的均值和所述相对距离对应的均值作为机器人实际位姿的测量值，所述相对角度对应的方差是机器人实际位姿对应的测量值中角度的不确定度，所述相对距离对应的方差是机器人实际位姿对应的测量值中位置的不确定度。无迹卡尔曼滤波器可以根据输入的所述相对角度对应的均值、所述相对距离对应的均值、所述相对角度对应的方差、所述相对距离对应的方差判断机器人实际位姿对应的测量值是否可信，例如，若相对距离对应的方差较小，则说明相对距离对应的均值可信，相对角度对应的方差较小，则相对角度对应的均值可信，即相对距离对应的均值和相对角度对应的均值确定的机器人实际位姿的测量值可信，在无迹卡尔曼滤波器进行定位时，会将机器人在当前时刻的实际位姿偏向机器人实际位姿的测量值；反之，若相对距离对应的方差较大，则说明相对距离对应的均值不可信，相对角度对应的方差较大，则相对角度对应的均值不可信，即相对距离对应的均值和相对角度对应的均值确定的机器人实际位姿的测量值不可信，在无迹卡尔曼滤波器进行定位时，会将机器人在当前时刻的实际位姿远离机器人实际位姿的测量值。The mean value corresponding to the relative angle and the mean value corresponding to the relative distance input are used as the measured value of the actual pose of the robot, and the variance corresponding to the relative angle is the uncertainty of the angle in the measured value corresponding to the actual pose of the robot, The variance corresponding to the relative distance is the uncertainty of the position in the measurement value corresponding to the actual pose of the robot. The unscented Kalman filter can judge the measured value corresponding to the actual pose of the robot according to the mean value corresponding to the input relative angle, the mean value corresponding to the relative distance, the variance corresponding to the relative angle, and the variance corresponding to the relative distance Is it credible? For example, if the variance corresponding to the relative distance is small, it means that the mean value corresponding to the relative distance is credible. If the variance corresponding to the relative angle is small, the mean value corresponding to the relative angle is credible. The measured value of the actual pose of the robot determined by the mean value corresponding to the angle is credible. When the unscented Kalman filter is used for positioning, the actual pose of the robot at the current moment will be biased towards the measured value of the actual pose of the robot; otherwise, if relative If the variance corresponding to the distance is large, it means that the mean value corresponding to the relative distance is unreliable. The measured value of the pose is unreliable. When the unscented Kalman filter is used for positioning, the actual pose of the robot at the current moment will be far away from the measured value of the actual pose of the robot.

可以理解，利用无迹卡尔曼滤波器确定机器人在当前时刻的实际位姿，不需要对***方程进行解析计算雅克比矩阵，在非线性的优化中具有更高的精度和计算更方便，进而使得机器人的定位更加准确，定位过程更加便捷。It can be understood that using the unscented Kalman filter to determine the actual pose of the robot at the current moment does not require analytical calculation of the Jacobian matrix for the system equations, which has higher precision and more convenient calculations in nonlinear optimization, thereby making The positioning of the robot is more accurate and the positioning process is more convenient.

进一步的，如图8所示，步骤S520中确定距离可靠标签包括以下步骤：Further, as shown in FIG. 8, determining the distance reliable label in step S520 includes the following steps:

S521：将所述预定第一数目个角度可靠标签分为K组，每组包括至少3个角度可靠标签。S521: Divide the predetermined first number of angle-reliable tags into K groups, each group including at least 3 angle-reliable tags.

可以理解，若角度可靠标签总数为N，每组包括p个，p≥3，则利用排列组合公式，可以确定K组，

It can be understood that if the total number of angle-reliable tags is N, each group includes p, and p≥3, then K groups can be determined by using the permutation and combination formula,

S522：根据每组的角度可靠标签确定所述机器人的位置。S522: Determine the position of the robot according to the angle-reliable tags of each group.

可以利用三角形定位法基于每组的任意3个角度可靠标签确定所述机器人的位置，分别计算每一组对应的位置，则可以确定K个位置。三角形定位法为定位领域常用技术手段，在此，不在进一步解释说明。The position of the robot can be determined based on any 3 angle reliable tags in each group by using the triangular positioning method, and the corresponding positions of each group can be calculated separately, then K positions can be determined. The triangle positioning method is a commonly used technical means in the positioning field, and will not be further explained here.

S523：计算K个位置对应的均值和K个位置对应的方差。S523: Calculate the mean value corresponding to the K positions and the variance corresponding to the K positions.

S524：若K个位置对应的方差大于预设的方差阈值，则将K个位置中距离所述K个位置对应的均值最远的位置删除，并重新计算K-1个位置对应的均值和K-1个位置对应的方差，直至剩余位置对应的方差小于等于预设的方差阈值，剩余位置对应的定位标签为距离可靠标签。S524: If the variance corresponding to the K positions is greater than the preset variance threshold, delete the position farthest from the mean value corresponding to the K positions among the K positions, and recalculate the mean value and K value corresponding to the K-1 positions The variance corresponding to -1 position, until the variance corresponding to the remaining positions is less than or equal to the preset variance threshold, and the positioning labels corresponding to the remaining positions are distance reliable labels.

S525：若K个位置对应的方差小于等于预设的方差阈值，则K个位置对应的定位标签为距离可靠标签。S525: If the variances corresponding to the K positions are less than or equal to the preset variance threshold, then the positioning labels corresponding to the K positions are distance reliable labels.

本实施例公开了对角度可靠标签进行进一步筛选的方法，以从角度可靠标签中选择出距离可靠标签，然后，根据各个距离可靠标签的相对角度对应的均值、相对距离对应的均值、相对角度对应的方差、相对距离对应的方差输入至预设的无迹卡尔曼滤波器，以获取所述机器人在当前时刻的实际位姿。可以理解，无迹卡尔曼滤波器的优势在于不需要对***方程进行解析计算雅克比矩阵，在非线性的优化中具有更高的精度，并且计算更方便简单，该优势有效降低机器人定位的复杂程度，使得定位过程更为高效快捷。This embodiment discloses a method for further screening angle-reliable tags to select distance-reliable tags from angle-reliable tags, and then, according to the mean value corresponding to the relative angle, the mean value corresponding to the relative distance, and the corresponding The variance of and the variance corresponding to the relative distance are input to the preset unscented Kalman filter to obtain the actual pose of the robot at the current moment. It can be understood that the advantage of the unscented Kalman filter is that it does not require analytical calculation of the Jacobian matrix for the system equations, it has higher precision in nonlinear optimization, and the calculation is more convenient and simple. This advantage effectively reduces the complexity of robot positioning The degree makes the positioning process more efficient and faster.

实施例6Example 6

本申请的一个实施例，如图9所示，一种机器人定位装置10包括：获取单元11、估计单元12、筛选单元13和定位单元14。In one embodiment of the present application, as shown in FIG. 9 , a robot positioning device 10 includes: an acquisition unit 11 , an estimation unit 12 , a screening unit 13 and a positioning unit 14 .

获取单元11，用于获取机器人和各个定位标签之间的相对距离和相对角度，所述机器人包括通信模块，所述通信模块用于接收所述各个定位标签发送的脉冲信号；获取单元11，还用于获取所述机器人的惯性数据；估计单元12，用于根据惯性数据估计所述机器人在当前时刻的估计位姿；筛选单元13，用于根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；定位单元14，用于根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。The acquisition unit 11 is used to acquire the relative distance and relative angle between the robot and each positioning tag, the robot includes a communication module, and the communication module is used to receive the pulse signal sent by each positioning tag; the acquisition unit 11 is also For obtaining the inertial data of the robot; the estimation unit 12 is used for estimating the estimated pose of the robot at the current moment according to the inertial data; the screening unit 13 is used for according to the estimated angle in the estimated pose and each According to the comparison result of the relative angle, a predetermined first number of reliable angle tags are determined from the respective positioning tags; the positioning unit 14 is configured to determine the relative distance and relative angle corresponding to the predetermined first number of reliable angle tags. The actual pose of the robot at the current moment.

进一步的，可以在机器人的预定位置上安装第一天线和第二天线，所述第一天线和所述第二天线用于接收所述各个定位标签发送的脉冲信号，所述获取机器人和各个定位标签之间的相对距离和相对角度，包括：获取第i个定位标签与所述第一天线之间的第一距离，i≤I，I为所述定位标签的总数；获取第i个定位标签与所述第二天线之间的第二距离；确定第i个定位标签发送的脉冲信号到达所述第一天线和所述第二天线的到达相位差；根据所述第一距离、所述到达相位差和所述第二距离确定所述机器人与第i个定位标签之间的相对距离和相对角度。Further, a first antenna and a second antenna may be installed on a predetermined position of the robot, the first antenna and the second antenna are used to receive the pulse signals sent by each positioning tag, and the acquiring robot and each positioning tag The relative distance and the relative angle between the tags include: obtaining the first distance between the i-th positioning tag and the first antenna, i≤I, where I is the total number of the positioning tags; obtaining the i-th positioning tag and the second distance between the second antenna; determine the arrival phase difference of the pulse signal sent by the i-th positioning tag to the first antenna and the second antenna; according to the first distance, the arrival The phase difference and the second distance determine the relative distance and relative angle between the robot and the i-th positioning tag.

进一步的，所述惯性数据包括所述机器人的线加速度和角速度，所述根据所述惯性数据估计所述机器人在当前时刻的估计位姿，包括：根据所述惯性数据中上一时刻的线加速度估计上一时刻到当前时刻内所述机器人的位移变化量；根据所述惯性数据中上一时刻所述角速度估计上一时刻到当前时刻内所述机器人的角度变化量；根据所述位移变化量和角度变化量估计所述机器人在当前时刻的估计位姿。Further, the inertial data includes the linear acceleration and angular velocity of the robot, and estimating the estimated pose of the robot at the current moment according to the inertial data includes: according to the linear acceleration of the previous moment in the inertial data Estimating the displacement variation of the robot from the previous moment to the current moment; estimating the angular variation of the robot from the previous moment to the current moment according to the angular velocity in the inertial data at the previous moment; according to the displacement variation and the angle variation to estimate the estimated pose of the robot at the current moment.

进一步的，所述根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签，包括：计算第i个定位标签对应的相对角度与所述估计角度之差的绝对值，i≤I，I为所述定位标签的总数；判断所述绝对值是否大于预设的角度阈值；若小于等于所述角度阈值，则第i个定位标签可靠；若大于所述角度阈值，则第i个定位标签不可靠。Further, the determining a predetermined first number of angle-reliable tags from the positioning tags according to the comparison result of the estimated angle in the estimated pose and each of the relative angles includes: calculating the i-th positioning tag corresponding to The absolute value of the difference between the relative angle and the estimated angle, i≤I, I is the total number of the positioning tags; judge whether the absolute value is greater than the preset angle threshold; if less than or equal to the angle threshold, then the first The i-th positioning tag is reliable; if it is greater than the angle threshold, the i-th positioning tag is unreliable.

进一步的，所述根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿，包括：根据所述预定第一数目个角度可靠标签对应的相对角度计算相对角度对应的均值和相对角度对应的方差；根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签；根据所述预定第二数目个距离可靠标签对应的相对距离度计算相对距离对应的均值和相对距离对应的方差；将所述相对角度对应的均值、所述相对距离对应的均值、所述相对角度对应的方差、所述相对距离对应的方差输入至预设的无迹卡尔曼滤波器，以获取所述机器人在当前时刻的实际位姿。Further, the determining the actual pose of the robot at the current moment according to the relative distances and relative angles corresponding to the predetermined first number of reliable angle tags includes: Calculate the mean value corresponding to the relative angle and the variance corresponding to the relative angle for the relative angle; determine a predetermined second number of distance reliable tags according to the relative distance corresponding to the predetermined first number of angle reliable tags; according to the predetermined second number of distance reliable tags The relative distance degree corresponding to the label calculates the mean value corresponding to the relative distance and the variance corresponding to the relative distance; the mean value corresponding to the relative angle, the mean value corresponding to the relative distance, the variance corresponding to the relative angle, and the variance corresponding to the relative distance The variance is input to a preset unscented Kalman filter to obtain the actual pose of the robot at the current moment.

进一步的，所述根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签，包括：将所述预定第一数目个角度可靠标签分为K组，每组包括至少3个角度可靠标签；根据每组的角度可靠标签确定所述机器人的位置；计算K个位置对应的均值和K个位置对应的方差；若K个位置对应的方差大于预设的方差阈值，则将K个位置中距离所述K个位置对应的均值最远的位置删除，并重新计算K-1个位置对应的均值和K-1个位置对应的方差，直至剩余位置对应的方差小于等于预设的方差阈值，剩余位置对应的定位标签为距离可靠标签；若K个位置对应的方差小于等于预设的方差阈值，则K个位置对应的定位标签为距离可靠标签。Further, the determining the predetermined second number of distance reliable tags according to the relative distances corresponding to the predetermined first number of angle reliable tags includes: dividing the predetermined first number of angle reliable tags into K groups, each group Including at least 3 angular reliable tags; determining the position of the robot according to the angular reliable tags of each group; calculating the mean value corresponding to the K positions and the variance corresponding to the K positions; if the variance corresponding to the K positions is greater than the preset variance threshold , then delete the position farthest from the mean value corresponding to the K positions among the K positions, and recalculate the mean value corresponding to K-1 positions and the variance corresponding to K-1 positions until the variance corresponding to the remaining positions is less than Equal to the preset variance threshold, the positioning labels corresponding to the remaining positions are distance reliable labels; if the variances corresponding to K positions are less than or equal to the preset variance threshold, then the positioning labels corresponding to K positions are distance reliable labels.

本申请公开的机器人定位装置10通过获取单元11、估计单元12、筛选单元13和定位单元14的配合使用，用于执行上述实施例所述的机器人定位方法，上述实施例所涉及的实施方案以及有益效果在本实施例中同样适用，在此不再赘述。The robot positioning device 10 disclosed in this application is used in conjunction with the acquisition unit 11, the estimation unit 12, the screening unit 13 and the positioning unit 14 to implement the robot positioning method described in the above-mentioned embodiments, the implementation schemes involved in the above-mentioned embodiments and The beneficial effects are also applicable in this embodiment, and will not be repeated here.

本申请的一个实施例，如图10所示，提出一种机器人100，包括存储器110、处理器120和通信模块130，所述存储器110存储有计算机程序，所述计算机程序在所述处理器120上运行时执行本申请所述的机器人定位方法，通信模块130用于接收各个定位标签发送的脉冲信号。One embodiment of the present application, as shown in FIG. 10 , proposes a robot 100, including a memory 110, a processor 120 and a communication module 130, the memory 110 stores a computer program, and the computer program runs on the processor 120 The robot positioning method described in this application is executed when the system is running, and the communication module 130 is used to receive pulse signals sent by each positioning tag.

进一步的，通信模块130包括第一天线131和第二天线132，通信模块130利用第一天线131和第二天线132接收各个定位标签发送的脉冲信号。Further, the communication module 130 includes a first antenna 131 and a second antenna 132, and the communication module 130 uses the first antenna 131 and the second antenna 132 to receive pulse signals sent by each positioning tag.

本申请的一个实施例，提出一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上运行时执行本申请所述的机器人定位方法。An embodiment of the present application provides a readable storage medium, which stores a computer program, and executes the robot positioning method described in the present application when the computer program runs on a processor.

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。In the several embodiments provided in this application, it should be understood that the disclosed devices and methods may also be implemented in other ways. The device embodiments described above are only illustrative. For example, the flow charts and structural diagrams in the accompanying drawings show the possible implementation architecture and functions of devices, methods and computer program products according to multiple embodiments of the present application. and operation. In this regard, each block in a flowchart or block diagram may represent a module, program segment, or part of code that includes one or more Executable instructions. It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved. It is also to be noted that each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams, can be implemented by a dedicated hardware-based system that performs the specified function or action may be implemented, or may be implemented by a combination of special purpose hardware and computer instructions.

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。In addition, each functional module or unit in each embodiment of the present application may be integrated to form an independent part, each module may exist independently, or two or more modules may be integrated to form an independent part.

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个可读存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的可读存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。If the functions are realized in the form of software function modules and sold or used as independent products, they can be stored in a readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned readable storage media include: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), magnetic disk or optical disc, etc., which can store program codes. medium.

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。The above is only a specific implementation of the application, but the scope of protection of the application is not limited thereto. Anyone familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the application. Should be covered within the protection scope of this application.

Claims

一种机器人定位方法，其特征在于，所述机器人包括通信模块，所述通信模块用于接收各个定位标签发送的脉冲信号，所述方法包括：A robot positioning method, characterized in that the robot includes a communication module, the communication module is used to receive pulse signals sent by each positioning tag, and the method includes:

获取机器人和各个定位标签之间的相对距离和相对角度；Obtain the relative distance and relative angle between the robot and each positioning tag;

获取所述机器人的惯性数据；obtaining inertial data of the robot;

根据惯性数据估计所述机器人在当前时刻的估计位姿；Estimating the estimated pose of the robot at the current moment according to the inertial data;

根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；determining a predetermined first number of angle-reliable tags from the respective positioning tags based on a comparison result of the estimated angle in the estimated pose and each of the relative angles;

根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。The actual pose of the robot at the current moment is determined according to the relative distances and relative angles corresponding to the predetermined first number of angle-reliable tags.
根据权利要求1所述的机器人定位方法，其特征在于，所述机器人的预定位置上安装有第一天线和第二天线，所述第一天线和所述第二天线用于接收所述各个定位标签发送的脉冲信号，所述获取机器人和各个定位标签之间的相对距离和相对角度，包括：The robot positioning method according to claim 1, wherein a first antenna and a second antenna are installed on the predetermined position of the robot, and the first antenna and the second antenna are used to receive the positioning The pulse signal sent by the tag, the acquisition of the relative distance and relative angle between the robot and each positioning tag includes:

获取第i个定位标签与所述第一天线之间的第一距离，i≤I，I为所述定位标签的总数；Obtain the first distance between the i-th positioning tag and the first antenna, i≤I, where I is the total number of the positioning tags;

获取第i个定位标签与所述第二天线之间的第二距离；Obtain a second distance between the i-th positioning tag and the second antenna;

确定第i个定位标签发送的脉冲信号到达所述第一天线和所述第二天线的到达相位差；determining the arrival phase difference between the first antenna and the second antenna when the pulse signal sent by the i-th positioning tag arrives;

根据所述第一距离、所述到达相位差和所述第二距离确定所述机器人与第i个定位标签之间的相对距离和相对角度。Determine the relative distance and relative angle between the robot and the i-th positioning tag according to the first distance, the arrival phase difference and the second distance.
根据权利要求1所述的机器人定位方法，其特征在于，所述惯性数据包括所述机器人的线加速度和角速度，所述根据所述惯性数据估计所述机器人在当前时刻的估计位姿，包括：The robot positioning method according to claim 1, wherein the inertial data includes linear acceleration and angular velocity of the robot, and estimating the estimated pose of the robot at the current moment according to the inertial data includes:

根据所述惯性数据中上一时刻的线加速度估计上一时刻到当前时刻内所述机器人的位移变化量；Estimating the displacement variation of the robot from the previous moment to the current moment according to the linear acceleration at the previous moment in the inertial data;

根据所述惯性数据中上一时刻所述角速度估计上一时刻到当前时刻内所述机器人的角度变化量；Estimating the angular variation of the robot from the previous moment to the current moment according to the angular velocity at the previous moment in the inertial data;

根据所述位移变化量和角度变化量估计所述机器人在当前时刻的估计位姿。Estimating the estimated pose of the robot at the current moment according to the displacement variation and the angle variation.
根据权利要求1所述的机器人定位方法，其特征在于，所述根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签，包括：The robot positioning method according to claim 1, wherein the predetermined first number of angles are determined from each positioning tag according to the comparison result of the estimated angle in the estimated pose and each of the relative angles Reliable labels, including:

计算第i个定位标签对应的相对角度与所述估计角度之差的绝对值，i≤I，I为所述定位标签的总数；Calculate the absolute value of the difference between the relative angle corresponding to the i-th positioning tag and the estimated angle, i≤I, and I is the total number of the positioning tags;

判断所述绝对值是否大于预设的角度阈值；judging whether the absolute value is greater than a preset angle threshold;

若小于等于所述角度阈值，则第i个定位标签可靠；If it is less than or equal to the angle threshold, the i-th positioning tag is reliable;

若大于所述角度阈值，则第i个定位标签不可靠。If it is greater than the angle threshold, the i-th positioning tag is unreliable.
根据权利要求1至4任一项所述的机器人定位方法，其特征在于，所述根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿，包括：The robot positioning method according to any one of claims 1 to 4, wherein the actual position of the robot at the current moment is determined according to the relative distances and relative angles corresponding to the predetermined first number of reliable angle tags. posture, including:

根据所述预定第一数目个角度可靠标签对应的相对角度计算相对角度对应的均值和相对角度对应的方差；Calculate the mean value corresponding to the relative angle and the variance corresponding to the relative angle according to the relative angle corresponding to the predetermined first number of reliable angle tags;

根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签；determining a predetermined second number of distance reliable tags according to the relative distances corresponding to the predetermined first number of angle reliable tags;

根据所述预定第二数目个距离可靠标签对应的相对距离度计算相对距离对应的均值和相对距离对应的方差；Calculate the mean value corresponding to the relative distance and the variance corresponding to the relative distance according to the relative distance degree corresponding to the predetermined second number of distance reliable tags;

将所述相对角度对应的均值、所述相对距离对应的均值、所述相对角度对应的方差、所述相对距离对应的方差输入至预设的无迹卡尔曼滤波器，以获取所述机器人在当前时刻的实际位姿。Input the mean value corresponding to the relative angle, the mean value corresponding to the relative distance, the variance corresponding to the relative angle, and the variance corresponding to the relative distance into a preset unscented Kalman filter to obtain the The actual pose at the current moment.
根据权利要求5所述的机器人定位方法，其特征在于，所述根据所述预定第一数目个角度可靠标签对应的相对距离确定预定第二数目个距离可靠标签，包括：The robot positioning method according to claim 5, wherein the determining a predetermined second number of distance reliable tags according to the relative distances corresponding to the predetermined first number of angle reliable tags includes:

将所述预定第一数目个角度可靠标签分为K组，每组包括至少3个角度可靠标签；dividing the predetermined first number of angle-reliable tags into K groups, each group including at least 3 angle-reliable tags;

根据每组的角度可靠标签确定所述机器人的位置；determining the position of the robot from each set of angularly reliable tags;

计算K个位置对应的均值和K个位置对应的方差；Calculate the mean value corresponding to K positions and the variance corresponding to K positions;

若K个位置对应的方差大于预设的方差阈值，则将K个位置中距离所述K个位置对应的均值最远的位置删除，并重新计算K-1个位置对应的均值和K-1个位置对应的方差，直至剩余位置对应的方差小于等于预设的方差阈值，剩余位置对应的定位标签为距离可靠标签；If the variance corresponding to K positions is greater than the preset variance threshold, delete the position farthest from the mean value corresponding to the K positions among the K positions, and recalculate the mean value and K-1 position corresponding to K-1 positions The variance corresponding to each position until the variance corresponding to the remaining positions is less than or equal to the preset variance threshold, and the positioning labels corresponding to the remaining positions are distance reliable labels;

若K个位置对应的方差小于等于预设的方差阈值，则K个位置对应的定位标签为距离可靠标签。If the variances corresponding to the K positions are less than or equal to the preset variance threshold, the positioning labels corresponding to the K positions are distance reliable labels.
一种机器人定位装置，其特征在于，所述机器人包括通信模块，所述通信模块用于接收各个定位标签发送的脉冲信号，所述装置包括：A robot positioning device, characterized in that the robot includes a communication module, the communication module is used to receive pulse signals sent by each positioning tag, and the device includes:

获取单元，用于获取机器人和各个定位标签之间的相对距离和相对角度；还用于获取所述机器人的惯性数据；The acquisition unit is used to acquire the relative distance and relative angle between the robot and each positioning tag; it is also used to acquire the inertial data of the robot;

估计单元，用于根据惯性数据估计所述机器人在当前时刻的估计位姿；an estimation unit, configured to estimate the estimated pose of the robot at the current moment according to the inertial data;

筛选单元，用于根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；a screening unit, configured to determine a predetermined first number of angle-reliable tags from the respective positioning tags according to a comparison result of the estimated angle in the estimated pose and each of the relative angles;

定位单元，用于根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。A positioning unit, configured to determine the actual pose of the robot at the current moment according to the relative distances and relative angles corresponding to the predetermined first number of angle-reliable tags.
根据权利要求7所述的机器人定位装置，其特征在于，所述通信模块包括第一天线和第二天线，所述第一天线和所述第二天线用于接收所述各个定位标签发送的脉冲信号，所述获取机器人和各个定位标签之间的相对距离和相对角度，包括：The robot positioning device according to claim 7, wherein the communication module includes a first antenna and a second antenna, and the first antenna and the second antenna are used to receive the pulses sent by the positioning tags signal, the relative distance and relative angle between the acquisition robot and each positioning tag, including:

获取第i个定位标签与所述第一天线之间的第一距离，i≤I，I为所述定位标签的总数；Obtain the first distance between the i-th positioning tag and the first antenna, i≤I, where I is the total number of the positioning tags;

获取第i个定位标签与所述第二天线之间的第二距离；Obtain a second distance between the i-th positioning tag and the second antenna;

确定第i个定位标签发送的脉冲信号到达所述第一天线和所述第二天线的到达相位差；determining the arrival phase difference between the first antenna and the second antenna when the pulse signal sent by the i-th positioning tag arrives;

根据所述第一距离、所述到达相位差和所述第二距离确定所述机器人与第i个定位标签之间的相对距离和相对角度。Determine the relative distance and relative angle between the robot and the i-th positioning tag according to the first distance, the arrival phase difference and the second distance.
一种机器人，其特征在于，包括存储器、处理器和通信模块，所述存储器存储有计算机程序，所述计算机程序在所述处理器上运行时执行权利要求1至6任一项所述的机器人定位方法，所述通信模块用于接收各个定位标签发送的脉冲信号。A robot, characterized in that it includes a memory, a processor and a communication module, the memory stores a computer program, and the computer program executes the robot according to any one of claims 1 to 6 when running on the processor In the positioning method, the communication module is used to receive pulse signals sent by each positioning tag.
一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上运行时执行权利要求1至6任一项所述的机器人定位方法。A readable storage medium, characterized in that it stores a computer program, and the computer program executes the robot positioning method according to any one of claims 1 to 6 when running on a processor.