阅读说明：本技术 一种机器人的近距离混合跟随定位系统及方法 (Close-range hybrid following positioning system and method for robot ) 是由 郑凯 李岳坤 姜毅 于 2021-09-03 设计创作，主要内容包括：本发明公开了一种机器人的近距离混合跟随定位系统及方法,所述系统包括跟随者端模块和被跟踪目标端模块,所述被跟踪目标端模块包括无线通信模块A、超声波发射器和处理器A,所述跟随者端模块包括无线通信模块B、超声波接收阵列和处理器B。本发明融合了TOA算法与RSSI算法并通过神经网络训练,最终计算机器人与被跟踪目标端之间的位置关系。相较于传统的使用单一定位算法,实现了较高的定位精度。本发明考虑了多机器人同时工作时信号串扰的问题,提出了每个机器人发送不同频率超声波信号,并通过滤波实现机器人与其对应目标之间的特异性识别。本发明可实现一维、二维、三维定位,对超声波接受阵列布局无特殊限制。(The invention discloses a close-range hybrid following positioning system and a close-range hybrid following positioning method for a robot. The invention integrates the TOA algorithm and the RSSI algorithm, trains through a neural network, and finally calculates the position relation between the robot and the tracked target end. Compared with the traditional method using a single positioning algorithm, the method realizes higher positioning precision. The invention considers the problem of signal crosstalk when multiple robots work simultaneously, provides that each robot sends ultrasonic signals with different frequencies, and realizes specific identification between the robot and a corresponding target through filtering. The invention can realize one-dimensional, two-dimensional and three-dimensional positioning, and has no special limitation on the layout of the ultrasonic receiving array.)

1. A close-range hybrid following positioning system of a robot is characterized in that: the tracking target end module comprises a power circuit A, a wireless communication module A, an ultrasonic transmitter and a processor A, wherein the power circuit A, the wireless communication module A, the ultrasonic transmitter and the processor A are all arranged at the tracked target end, and the power circuit A, the wireless communication module A and the ultrasonic transmitter are respectively connected with the processor A; the follower end module comprises a power circuit B, a wireless communication module B, an ultrasonic receiving array and a processor B, wherein the power circuit B, the wireless communication module B, the ultrasonic receiving array and the processor B are all arranged at the follower end, and the power circuit B, the wireless communication module B and the ultrasonic receiving array are respectively connected with the processor B; the wireless communication module A and the wireless communication module B are communicated through a wireless network; the ultrasonic receiving array comprises n ultrasonic receivers, wherein n is more than or equal to 2;

the radio signal sent by the wireless communication module A comprises the state of the tracked target end module and the battery state information; the state of the tracked target end module comprises time, speed and direction of a tracked target end;

the ultrasonic transmitter sends out a frequency f after the wireless communication module A receives the pulse trigger signal_kThe ultrasonic pulse of (3);

frequency f_kAt the natural frequency f of the ultrasonic transmitter₀And satisfies f_k＝f₀+ k · Δ f, where k is an integer, the ultrasonic pulse frequency k values emitted by the ultrasonic emitters at different tracked target ends are different, and Δ f is the pulse frequency interval for distinguishing adjacent frequencies by the band-pass filter;

each time the ultrasonic transmitter sends out the frequency f_kHas an ultrasonic pulse duration of t_k，t_kGreater than the starting oscillation time t of the ultrasonic transmitter for transmitting ultrasonic waves₁₀2 times of the total weight of the composition;

the wireless communication module B of the follower end receives a radio signal sent by the wireless communication module A of the tracked target end;

the radio signal sent by the wireless communication module B of the follower end to the wireless communication module A of the tracked target end comprises: a pulse trigger signal and a state inquiry signal;

while the wireless communication module B of the follower end sends a pulse trigger signal to the wireless communication module A of the tracked target end, the processor B samples all ultrasonic receivers, and the sampling frequency is f_s，f_s≥2×f_kSampling time of T_k，T_k≥2×t_kTo ensure that the ultrasonic pulse sent by the tracked target is received within the distance range between the follower and the tracked target;

the follower-fixed i-th ultrasonic receiver is at the sampling duration time delta t_1kiThen receiving an ultrasonic pulse signal sent by an ultrasonic transmitter at the tracked target end, wherein the duration time of the sampled ultrasonic pulse signal is St_kiThen, for the ith ultrasonic receiver, there are: t is t_k≤St_kiAnd Δ t_1ki+St_ki≤T_k(ii) a i is less than or equal to n, and St obtained by sampling_kiUltrasonic pulse signal in time is carried out with f_kFiltering received ultrasonic pulse signals with different frequencies, which are emitted by other nearby tracked targets, for the center frequency and the delta f for the band-pass broadband;

with f_kBand-pass filtering at a center frequency f with a passband width of Δ f_kThe resulting time lag is deltat_kTo capture a voltage threshold v_tAs a judgment condition, if the voltage of the ith ultrasonic receiver is greater than the acquisition voltage threshold v_tAt a time of Δ t_2kiThen Δ t_1ki≤Δt_2kiAnd Δ t_2ki-δt_kThe time of flight of the ultrasonic pulse sent by the ultrasonic transmitter at the tracked target end in a transmission medium is obtained;

for St_kiThe ultrasonic pulse signal obtained by sampling in time is subjected to Fast Fourier Transform (FFT) analysis and recorded at the frequency f_kAt an amplitude of At_ki；

Establishing the following frequency f for the ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed at the follower end_kRSSI model R as an indication model of the strength of the ultrasonic pulse signal_i(d_i,θ_i) The following were used:

A_i＝R_i(d_i,α_i,β_i) (1)

in the formula, A_iAmplitude of the ultrasonic pulse signal for the ith ultrasonic receiver, d_iIs the distance between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end, alpha_iAnd beta_iThe included angles in the horizontal direction and the vertical direction of the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end are respectively included;

an included angle alpha is formed between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end in the horizontal direction_iAngle beta with the vertical_iAnd the jth ultrasonic receiver and the tracked target end ultrasonicAngle alpha between wave emitters in horizontal direction_jAngle beta with the vertical_jSatisfies the following formula:

in the formula, Δ α_ijAnd Δ β_ijIs a constant;

the frequency of the ith ultrasonic receiver fixed on the ultrasonic transmitter and the follower at the tracked target end is f when the tracked target and the follower move on a constant straight line and the ultrasonic transmitter and the ultrasonic receiver are in positive phase opposition_kRSSI model R of ultrasonic pulse signal_i(x,α_i,β_i) Has the following forms:

in the formula, a_i、b_i、c_iThe parameters are the RSSI model parameters of the ith ultrasonic receiver and are obtained by off-line experimental data fitting;

for frequency f_kThe following equation system is solved to obtain the distance d between the ultrasonic transmitter and the ultrasonic receiver when the amplitude of the ultrasonic pulse signal is known to be measured_iAngle alpha_iAngle beta of_iNamely:

measuring result d of the ultrasonic wave pulse signal RSSI model for the ith ultrasonic wave receiver fixed on the ultrasonic wave transmitter and the follower of the tracked target end_iAnd time of flight t_ofiBy selecting neural networks NN_iAnd corresponding off-line training data, establishing the following frequency f_kThe neural network model between the ultrasonic pulse signal and the distance:

in the formula, r_iThe distance between an ultrasonic transmitter at a tracked target end fused by a neural network and an ith ultrasonic receiver fixed by a follower end is obtained;

RSSI model distance d obtained by measurement of kth ultrasonic receiver fixed by follower end_kiAnd time of flight Δ t_2kiAnd solving the fusion distance measured by the follower and the tracked target end ultrasonic transmitter for the kth time, namely:

r_ik＝NN_i(d_ki,Δt_2ki) (6)

for the ith ultrasonic receiver fixed on the ultrasonic transmitter at the tracked target end and the follower end, when the tracked target and the follower move on a constant straight line and the ultrasonic transmitter and the ultrasonic receiver are opposite, the distance between the ultrasonic transmitter and the ultrasonic receiver is directly obtained through the formula (6);

when the number of the ultrasonic receivers fixed to the follower end is 2, if the ultrasonic transmitter of the tracked target and the two ultrasonic receivers of the follower are installed on the same plane and move on the plane, a two-dimensional coordinate system is established on the plane by taking the ultrasonic receiver of the follower end as a reference, and the position coordinates (x, y) of the tracked target are given by solving the equation set of formula (7):

in the formula (x)₁,y₁) Is the position coordinate of the follower-end 1 st ultrasonic receiver, (x)₂,y₂) The position coordinate of the 2 nd ultrasonic receiver at the follower end is obtained;

when the number of the ultrasonic receivers fixed at the follower end exceeds 2, namely n is more than or equal to 3, establishing a space three-dimensional coordinate system by taking the ultrasonic receivers at the follower end as a reference, and giving out the position coordinates (x, y, z) of the tracked target through a solving equation (8) equation set

In the formula (x)_i,y_i,z_i) And i is the position coordinate of the ith ultrasonic receiver of the follower end in a three-dimensional space coordinate system, and is 1, …, n.

2. The close-range hybrid follow-up positioning system of a robot of claim 1, wherein: and the processor A and the processor B are STM32 single-chip microcomputers or other processors with the calculation capability and compatible interfaces not lower than that of an STM32 single-chip microcomputer.

3. The close-range hybrid follow-up positioning system of a robot of claim 1, wherein: the wireless communication module A and the wireless communication module B are both radio signal transceivers, and each radio signal transceiver comprises an infrared transceiver, a Bluetooth transceiver, a Wifi transceiver, a Zigbee transceiver, an RFID transceiver or an NFC transceiver.

4. A close-range hybrid following positioning method for a robot, which performs positioning using the close-range hybrid following positioning system for a robot according to claim 1, characterized in that: the method comprises the following steps:

step 1, initializing a system, namely initializing equipment of a tracked target end and a robot follower end, and initializing a wireless communication module A and a wireless communication module B;

step 2, the tracked target sends a wireless signal to the robot follower through the wireless communication module A, or the robot follower sends a wireless signal to the tracked target through the wireless communication module B, the wireless communication module connection between the tracked target and the tracked follower is established, real-time communication between the follower end and the tracked target end is realized, and the tracked target waits for the robot follower to send a wireless trigger signal;

step 3, if the two parties do not receive the wireless signal sent by the other party, returning to the step 2;

and 4, if the connection between the robot follower and the tracked target is established, the robot follower sends an ultrasonic wave sending starting instruction to the tracked target, simultaneously records the moment, and starts sampling with the sampling frequency f_sWith a starting timing period of T_kThe timer of (1);

step 5, after the tracked target receives the ultrasonic wave transmission starting command, immediately transmitting the ultrasonic wave with the frequency f_kAnd continuously transmits t_kStopping sending after the time;

step 6, if the robot follower is in the sampling period T_kTime to acquire ultrasonic signal is St_kiFor St obtained by sampling_kiUltrasonic pulse signal in time is carried out with f_kFiltering received ultrasonic pulse signals with different frequencies, which are emitted by other nearby tracked targets, for the center frequency and the delta f for the band-pass broadband;

step 7, the robot follower follows the user with f_kBand-pass filtering at a center frequency f with a passband width of Δ f_kThe resulting time lag is deltat_kTo capture a voltage threshold v_tAs a judgment condition, if the voltage of the ith ultrasonic receiver is greater than the acquisition voltage threshold v_tAt a time of Δ t_2kiThen Δ t_1ki≤Δt_2kiAnd Δ t_2ki-δt_kThe time of flight of the ultrasonic pulse sent by the ultrasonic transmitter at the tracked target end in a transmission medium is obtained;

step 8, the robot follower performs Fast Fourier Transform (FFT) analysis on the sampled ultrasonic pulse signal, and records the frequency f_kAt an amplitude of At_ki；

9, establishing a frequency f given by the formula (1) in a processor for an ith ultrasonic receiver fixed by an ultrasonic transmitter at a tracked target end and a follower end of the robot_kThe ultrasonic pulse signal intensity indicating model is utilized to calculate the horizontal included angle between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target endα_iAngle beta with the vertical_iAnd the horizontal direction included angle alpha between the jth ultrasonic receiver and the ultrasonic transmitter at the tracked target end_jAngle beta with the vertical_j；

Step 10, the robot follower gives the frequency f given by the formula (1)_kThe equation set given by the formula (4) is solved to obtain the distance d between the ultrasonic transmitter and the ultrasonic receiver when the amplitude of the ultrasonic pulse signal is known to be measured_iAngle alpha_iAngle beta of_i；

Step 11, the robot follower measures the ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed at the follower end based on the measurement result d of the ultrasonic pulse signal RSSI model_iAnd time of flight t_ofiAccording to the formula of a neural network model (6) obtained by training, the distance between the fused ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed by the follower end is obtained;

step 12, the robot follower selects the formula (7) or the formula (8) according to the number of receivers carried by the robot follower to solve the current value of the space coordinate (x, y, z) of the tracked target and display the current value;

and step 13, returning to the step 5 to calculate the space coordinates (x, y, z) of the tracked target for the next time until the process is manually quitted.

Technical Field

The invention belongs to the field of robot target positioning, and particularly relates to a close-range hybrid following positioning system and method for a robot.

Background

With the development of science and technology, intelligent robots have been slowly applied to the lives of people, and a robot target positioning technology has also become one of the research hotspots. In the field of robot target positioning, it becomes a great research hotspot to improve the positioning accuracy of robots. The ultrasonic wave has the advantages of strong directivity, low cost and the like, and shows good practicability in short-distance positioning.

An ultrasonic indoor positioning method and system introduces a repeater carried by a positioning target, transmits ultrasonic signals through a base station, forwards the ultrasonic signals to the next base station through the positioning target, and realizes positioning by processing the flight time of ultrasonic waves among all base stations;

an ultrasonic positioning system based on composite ultrasonic signals introduces a method for realizing positioning by transmitting carrier coded signals and calculating the flight time of ultrasonic waves by a cross-correlation algorithm.

The two methods both need the synchronization of the ultrasonic receiving and sending time, have higher requirements on the calculation of the ultrasonic flight time, and are difficult to accurately calculate the ultrasonic flight time in the practical application process, so that the accurate positioning is difficult to realize. In addition, the intelligent mobile phone indoor positioning system and method capable of identifying LOS/NLOS acoustic signals introduces a method that an intelligent mobile phone emits pseudo ultrasonic signals, a time difference of arrival (TDOA) algorithm is used as a positioning algorithm, and measurement data are identified and discarded through a Support Vector Machine (SVM) and an evidence theory (DS), so that positioning is finally completed. Although the TDOA algorithm can effectively reduce the requirement on time synchronization, the complexity of the algorithm is improved, and the TDOA algorithm is not easy to solve in the practical application process.

Disclosure of Invention

In order to overcome the defects in the prior art, the invention aims to provide a close-range hybrid following positioning system and a close-range hybrid following positioning method for a robot, which can effectively solve the problem of mutual interference among signals of a plurality of sensors and improve the relative positioning accuracy of the close-range following system.

In order to achieve the purpose, the technical scheme of the invention is as follows:

a close-range hybrid following positioning system of a robot comprises a follower end module and a tracked target end module, wherein the tracked target end module comprises a power circuit A, a wireless communication module A, an ultrasonic transmitter and a processor A, the power circuit A, the wireless communication module A, the ultrasonic transmitter and the processor A are all installed at a tracked target end, and the power circuit A, the wireless communication module A and the ultrasonic transmitter are respectively connected with the processor A; the follower end module comprises a power circuit B, a wireless communication module B, an ultrasonic receiving array and a processor B, wherein the power circuit B, the wireless communication module B, the ultrasonic receiving array and the processor B are all arranged at the follower end, and the power circuit B, the wireless communication module B and the ultrasonic receiving array are respectively connected with the processor B; the wireless communication module A and the wireless communication module B are communicated through a wireless network; the ultrasonic receiving array comprises n ultrasonic receivers, wherein n is more than or equal to 2;

the radio signal sent by the wireless communication module A comprises the state of the tracked target end module and the battery state information; the state of the tracked target end module comprises time, speed and direction of a tracked target end;

the ultrasonic transmitter sends out a frequency f after the wireless communication module A receives the pulse trigger signal_kThe ultrasonic pulse of (3);

frequency f_kAt the natural frequency f of the ultrasonic transmitter₀And satisfies f_k＝f₀+ k · Δ f, where k is an integer, the ultrasonic pulse frequency k values emitted by the ultrasonic emitters at different tracked target ends are different, and Δ f is the pulse frequency interval for distinguishing adjacent frequencies by the band-pass filter;

each time the ultrasonic transmitter sends out the frequency f_kHas an ultrasonic pulse duration of t_k，t_kGreater than the starting oscillation time t of the ultrasonic transmitter for transmitting ultrasonic waves₁₀2 times of the total weight of the composition;

the wireless communication module B of the follower end receives a radio signal sent by the wireless communication module A of the tracked target end;

the radio signal sent by the wireless communication module B of the follower end to the wireless communication module A of the tracked target end comprises: a pulse trigger signal and a state inquiry signal;

while the wireless communication module B of the follower end sends a pulse trigger signal to the wireless communication module A of the tracked target end, the processor B samples all ultrasonic receivers, and the sampling frequency is f_s，f_s≥2×f_kSampling time of T_k，T_k≥2×t_kTo ensure that the ultrasonic pulse sent by the tracked target is received within the distance range between the follower and the tracked target;

the follower-fixed i-th ultrasonic receiver is at the sampling duration time delta t_1kiThen receiving an ultrasonic pulse signal sent by an ultrasonic transmitter at the tracked target end, wherein the duration time of the sampled ultrasonic pulse signal is St_kiThen, for the ith ultrasonic receiver, there are: t is t_k≤St_kiAnd Δ t_1ki+St_ki≤T_k(ii) a i is less than or equal to n, and St obtained by sampling_kiUltrasonic pulse signal in time is carried out with f_kFiltering received ultrasonic pulse signals with different frequencies, which are emitted by other nearby tracked targets, for the center frequency and the delta f for the band-pass broadband;

with f_kBand-pass filtering at a center frequency f with a passband width of Δ f_kThe resulting time lag is deltat_kTo capture a voltage threshold v_tAs a judgment condition, if the voltage of the ith ultrasonic receiver is greater than the acquisition voltage threshold v_tAt a time of Δ t_2kiThen Δ t_1ki≤Δt_2kiAnd Δ t_2ki-δt_kThe time of flight of the ultrasonic pulse sent by the ultrasonic transmitter at the tracked target end in a transmission medium is obtained;

for St_kiSampling in time to obtainThe ultrasonic pulse signal is subjected to Fast Fourier Transform (FFT) analysis and recorded at the frequency f_kAt an amplitude of At_ki；

Establishing the following frequency f for the ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed at the follower end_kRSSI model R as an indication model of the strength of the ultrasonic pulse signal_i(d_i,θ_i) The following were used:

A_i＝R_i(d_i,α_i,β_i) (1)

in the formula, A_iAmplitude of the ultrasonic pulse signal for the ith ultrasonic receiver, d_iIs the distance between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end, alpha_iAnd beta_iThe included angles in the horizontal direction and the vertical direction of the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end are respectively included;

an included angle alpha is formed between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end in the horizontal direction_iAngle beta with the vertical_iAnd the horizontal direction included angle alpha between the jth ultrasonic receiver and the ultrasonic transmitter at the tracked target end_jAngle beta with the vertical_jSatisfies the following formula:

in the formula, Δ α_ijAnd Δ β_ijIs a constant.

The frequency of the ith ultrasonic receiver fixed on the ultrasonic transmitter and the follower at the tracked target end is f when the tracked target and the follower move on a constant straight line and the ultrasonic transmitter and the ultrasonic receiver are in positive phase opposition_kRSSI model R of ultrasonic pulse signal_i(x,α_i,β_i) Has the following forms:

in the formula, a_i、b_i、c_iThe parameters are the RSSI model parameters of the ith ultrasonic receiver and are obtained by off-line experimental data fitting;

for frequency f_kThe following equation system is solved to obtain the distance d between the ultrasonic transmitter and the ultrasonic receiver when the amplitude of the ultrasonic pulse signal is known to be measured_iAngle alpha_iAngle beta of_iNamely:

measuring result d of the ultrasonic wave pulse signal RSSI model for the ith ultrasonic wave receiver fixed on the ultrasonic wave transmitter and the follower of the tracked target end_iAnd time of flight t_ofiBy selecting neural networks NN_iAnd corresponding off-line training data, establishing the following frequency f_kThe neural network model between the ultrasonic pulse signal and the distance:

in the formula, r_iThe distance between an ultrasonic transmitter at a tracked target end fused by a neural network and an ith ultrasonic receiver fixed by a follower end is obtained;

RSSI model distance d obtained by measurement of kth ultrasonic receiver fixed by follower end_kiAnd time of flight Δ t_2kiAnd solving the fusion distance measured by the follower and the tracked target end ultrasonic transmitter for the kth time, namely:

r_ik＝NN_i(d_ki,Δt_2ki) (6)

for the ith ultrasonic receiver fixed on the ultrasonic transmitter at the tracked target end and the follower end, when the tracked target and the follower move on a constant straight line and the ultrasonic transmitter and the ultrasonic receiver are opposite, the distance between the ultrasonic transmitter and the ultrasonic receiver is directly obtained through the formula (6);

when the number of the ultrasonic receivers fixed to the follower end is 2, if the ultrasonic transmitter of the tracked target and the two ultrasonic receivers of the follower are installed on the same plane and move on the plane, a two-dimensional coordinate system is established on the plane by taking the ultrasonic receiver of the follower end as a reference, and the position coordinates (x, y) of the tracked target are given by solving the equation set of formula (7):

in the formula (x)₁,y₁) Is the position coordinate of the follower-end 1 st ultrasonic receiver, (x)₂,y₂) The position coordinate of the 2 nd ultrasonic receiver at the follower end is obtained;

when the number of the ultrasonic receivers fixed at the follower end exceeds 2, namely n is more than or equal to 3, establishing a space three-dimensional coordinate system by taking the ultrasonic receivers at the follower end as a reference, and giving out the position coordinates (x, y, z) of the tracked target through a solving equation (8) equation set

In the formula (x)_i,y_i,z_i) And i is the position coordinate of the ith ultrasonic receiver of the follower end in a three-dimensional space coordinate system, and is 1, …, n.

Further, the processor A and the processor B are STM32 single-chip microcomputers or other processors with the computing power not lower than that of the STM32 single-chip microcomputers and compatible interfaces.

Further, the wireless communication module a and the wireless communication module B are both radio signal transceivers, and the radio signal transceivers include an infrared transceiver, a bluetooth transceiver, a Wifi transceiver, a Zigbee transceiver, an RFID transceiver, or an NFC transceiver.

A close-range hybrid following positioning method of a robot utilizes a close-range hybrid following positioning system to perform positioning, and comprises the following steps:

step 1, initializing a system, namely initializing equipment of a tracked target end and a robot follower end, and initializing a wireless communication module A and a wireless communication module B;

step 2, the tracked target sends a wireless signal to the robot follower through the wireless communication module A, or the robot follower sends a wireless signal to the tracked target through the wireless communication module B, the wireless communication module connection between the tracked target and the tracked follower is established, real-time communication between the follower end and the tracked target end is realized, and the tracked target waits for the robot follower to send a wireless trigger signal;

step 3, if the two parties do not receive the wireless signal sent by the other party, returning to the step 2;

and 4, if the connection between the robot follower and the tracked target is established, the robot follower sends an ultrasonic wave sending starting instruction to the tracked target, simultaneously records the moment, and starts sampling with the sampling frequency f_sWith a starting timing period of T_kThe timer of (1);

step 5, after the tracked target receives the ultrasonic wave transmission starting command, immediately transmitting the ultrasonic wave with the frequency f_kAnd continuously transmits t_kStopping sending after the time;

step 6, if the robot follower is in the sampling period T_kTime to acquire ultrasonic signal is St_kiFor St obtained by sampling_kiUltrasonic pulse signal in time is carried out with f_kFiltering received ultrasonic pulse signals with different frequencies, which are emitted by other nearby tracked targets, for the center frequency and the delta f for the band-pass broadband;

step 7, the robot follower follows the user with f_kBand-pass filtering at a center frequency f with a passband width of Δ f_kThe resulting time lag is deltat_kTo capture a voltage threshold v_tAs a judgment condition, if the voltage of the ith ultrasonic receiver is greater than the acquisition voltage threshold v_tAt a time of Δ t_2kiThen Δ t_1ki≤Δt_2kiAnd Δ t_2ki-δt_kThe time of flight of the ultrasonic pulse sent by the ultrasonic transmitter at the tracked target end in a transmission medium is obtained;

step 8, the robot follower performs Fast Fourier Transform (FFT) analysis on the sampled ultrasonic pulse signal, and records the frequency f_kAt an amplitude of At_ki；

9, establishing a frequency f given by the formula (1) in a processor for an ith ultrasonic receiver fixed by an ultrasonic transmitter at a tracked target end and a follower end of the robot_kThe ultrasonic pulse signal intensity indicating model is utilized to calculate the horizontal included angle alpha between the ith ultrasonic receiver and the ultrasonic transmitter at the tracked target end_iAngle beta with the vertical_iAnd the horizontal direction included angle alpha between the jth ultrasonic receiver and the ultrasonic transmitter at the tracked target end_jAngle beta with the vertical_j；

Step 10, the robot follower gives the frequency f given by the formula (1)_kThe equation set given by the formula (4) is solved to obtain the distance d between the ultrasonic transmitter and the ultrasonic receiver when the amplitude of the ultrasonic pulse signal is known to be measured_iAngle alpha_iAngle beta of_i；

Step 11, the robot follower measures the ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed at the follower end based on the measurement result d of the ultrasonic pulse signal RSSI model_iAnd time of flight t_ofiAccording to the formula of a neural network model (6) obtained by training, the distance between the fused ultrasonic transmitter at the tracked target end and the ith ultrasonic receiver fixed by the follower end is obtained;

step 12, the robot follower selects the formula (7) or the formula (8) according to the number of receivers carried by the robot follower to solve the current value of the space coordinate (x, y, z) of the tracked target and display the current value;

and step 13, returning to the step 5 to calculate the space coordinates (x, y, z) of the tracked target for the next time until the process is manually quitted.

Compared with the prior art, the invention has the following beneficial effects:

1. the invention integrates the TOA algorithm and the RSSI algorithm, trains through a neural network, and finally calculates the position relation between the robot and the tracked target end. Compared with the traditional method using a single positioning algorithm, the method realizes higher positioning precision.

2. The invention considers the problem of signal crosstalk when multiple robots work simultaneously, provides that each robot sends ultrasonic signals with different frequencies, and realizes specific identification between the robot and a corresponding target through filtering.

3. The invention can realize one-dimensional, two-dimensional and three-dimensional positioning, has no special limitation on the layout of the ultrasonic receiving array, and has the advantages of simple structure, lower cost and the like.

Drawings

Fig. 1 is a schematic diagram of the principle of the present invention.

FIG. 2 is a schematic process flow diagram of the present invention.

Fig. 3 is a schematic diagram of the positioning method of the present invention.

FIG. 4 is a schematic diagram of a BP neural network model according to the present invention.

Detailed Description

In order to explain the implementation mode of the invention more clearly, the technical scheme in the embodiment of the invention is described in detail below with reference to the accompanying drawings. It is to be understood that the described embodiments are merely a few embodiments of the invention, and not all embodiments. All other embodiments that can be derived by one of ordinary skill in the art from the embodiments given herein are intended to be within the scope of the present invention.

As shown in fig. 1, a close-range hybrid following positioning system for a robot includes a robot part, i.e., a following end, and a tracked target end. And a power supply circuit, a wireless communication module, an ultrasonic transmitter and an STM32 singlechip are arranged at the tracked target end. And a power supply circuit, a wireless communication module, an ultrasonic receiving array and an STM32 singlechip are arranged at the robot end.

This example is illustrated with a three-dimensional planar orientation. The wireless communication module in this example adopts a bluetooth module as an example, the ultrasonic module adopts a T/R40-16 model ultrasonic sensor, 1 ultrasonic transmitter (shown as point O in fig. 3) and 4 ultrasonic receivers form an ultrasonic receiving array (shown as points a, B, C and D in fig. 3), and the present invention is specifically described with this as an example.

Considering that signal crosstalk is easy to occur when a plurality of robots work simultaneously, an STM32 single chip microcomputer at target ends of different robots should transmit a square wave with a duration of 15ms and a center frequency of 40kHz (near a resonant frequency) ± 1kHz (frequency division in a unit of 0.1 kHz) as a trigger signal of an ultrasonic transmitter (here, 40.4kHz is taken as an example), and trigger a bluetooth module to communicate with the robot end. And after receiving the corresponding target end Bluetooth communication signal, the Bluetooth module of the robot end receives and records the ultrasonic signals received by the 4 ultrasonic receivers at the sampling frequency of 400kHz, wherein the receiving time lasts for 50 ms.

When a plurality of robots work simultaneously, received ultrasonic signals are easy to generate crosstalk, so that a band-pass filter is needed to be adopted to respectively filter the ultrasonic signals received by the 4 ultrasonic receivers, filter out the ultrasonic signals with other frequencies, and keep the ultrasonic signals of 40.4kHz corresponding to the transmitting frequency. Meanwhile, a threshold voltage V is set, and when the filtered ultrasonic signal is greater than the threshold voltage V, the moment is recorded as the flight time t of the ultrasonic wave₁、t₂、t₃、t₄。

FFT analysis is carried out on the ultrasonic signals filtered by the 4 ultrasonic receivers to obtain the amplitudes A with the frequency of 40.4kHz₁、A₂、A₃、A₄And calculating the corresponding distance according to the fitted RSSI model as follows.

Wherein d is₁、d₂、d₃、d₄Respectively representing the distances, alpha, of the four ultrasonic receivers from the followed object calculated by the model₁、α₂、α₃、α₄And beta₁、β₂、β₃、β₄Respectively representing the included angles between the four ultrasonic receivers and the ultrasonic transmitter in the horizontal direction and the vertical direction, A₁、A₂、A₃、A₄Representing the amplitude of the ultrasonic signal received by the four ultrasonic receivers.

Through the calculated voltage amplitude and the flight time of the ultrasonic wave, the NN of the BP neural network model shown in FIG. 4 can be trained_iThe network model is composed of an input layer, two hidden layer structures and an output layer. The inputs to the network model are the voltage amplitude and time of flight (d) received by the ultrasonic receiver_i,t_i) The output of the network is the distance r between the ultrasonic receiver and the ultrasonic transmitter_i。

Distance d obtained from RSSI model₁、d₂、d₃、d₄And ultrasonic flight time t₁、t₂、t₃、t₄NN model of BP neural network through off-line training₁、NN₂、NN₃、NN₄The distances between the four ultrasonic receivers and the followed target can be obtained. Wherein r is₁、r₂、r₃、r₄Representing the distances calculated by the neural network between the four ultrasonic receivers and the ultrasonic transmitter on the tracked target.

3-4, according to the calculation result of the BP neural network model, the distances r between the four ultrasonic receivers and the ultrasonic transmitter on the tracked target can be obtained₁、r₂、r₃、r₄. According to the following formula, it can be calculated to establish a three-dimensional plane coordinate system with reference to the ultrasonic receiver on the robot, wherein the coordinates of the tracked object are (x, y, z). (x)₁,y₁),(x₂,y₂),(x₃,y₃),(x₄,y₄) Respectively representing the positions of four ultrasonic receiversAnd (4) coordinates.

In case of two-dimensional planar positioning, the ultrasonic transmitter and the ultrasonic receiver are in the same plane, so that only 2 ultrasonic receivers (point a and point B in fig. 3) and one ultrasonic transmitter (point O in fig. 3) are used.

As with the three-dimensional space, in the two-dimensional plane, band-pass filters are used to filter the ultrasonic signals received by the 2 ultrasonic receivers, respectively, and the ultrasonic signals of 40.4kHz corresponding to the transmission frequency are retained. Meanwhile, a threshold voltage V is set, and when the filtered ultrasonic signal is greater than the threshold voltage V, the moment is recorded as the flight time t of the ultrasonic wave₁、t₂. FFT analysis is carried out on the ultrasonic signals filtered by the 2 ultrasonic receivers to obtain the amplitudes A with the frequency of 40.4kHz₁、A₂According to the fitted RSSI model, the corresponding distance can be calculated as follows:

wherein d is₁、d₂Each representing the distance, alpha, of 2 ultrasonic receivers from the followed object calculated by the model₁、α₂Respectively representing the included angles of 2 ultrasonic receivers and ultrasonic transmitters, A₁、A₂Representing the amplitude of the ultrasonic signal received by the 2 ultrasonic receivers.

Distance d obtained from RSSI model₁、d₂And ultrasonic flight time t₁、t₂By training the BP neural network model NN₁、NN₂The distance between 2 ultrasonic receivers and the followed target can be obtained. Wherein r is₁、r₂Represents the distance between 2 ultrasonic receivers and the ultrasonic transmitter on the tracked target calculated by the BP neural network:

according to the distance r between the 2 ultrasonic receivers and the ultrasonic transmitter on the tracked target₁、r₂And calculating to establish a two-dimensional plane coordinate system by taking the ultrasonic receiver on the robot as a reference, wherein the coordinate of the tracked target is (x, y). (x)₁,y₁),(x₂,y₂) And respectively represent A, B the position coordinates of the two ultrasonic receivers.

The above description is only exemplary of the present invention and should not be taken as limiting the invention, as any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.