KR102752454B1 - Method and apparatus for object following robot using uwb and odometry-based relative position estimation - Google Patents

Abstract

A method and device are disclosed for tracking the relative position of a target in a robot-centered coordinate system by fusing distance measurement data via UWB and odometer data, and for enabling the robot to track the target through an appropriate communication method and driving method. The target tracking method includes a step of initializing a tracking algorithm of the robot itself in response to a tracking request from the target, a step of transmitting an SS-TWR poll message, a step of receiving a single response message to the SS-TWR poll message from the target, wherein the single response message includes second driving record information of a second driving record measuring device of the target, a step of estimating based on a round-trip delay time calculated by the SS-TWR method, a step of predicting the position of the target through the second driving record information and the first driving record information of the first driving record measuring device of the robot, and a step of correcting the predicted position of the target based on the estimated distance.

Description

{METHOD AND APPARATUS FOR OBJECT FOLLOWING ROBOT USING UWB AND ODOMETRY-BASED RELATIVE POSITION ESTIMATION}

The present invention relates to object following robot technology, and more specifically, to a method and device for estimating the relative position of a target in a robot-centered coordinate system by fusing distance measurement data through UWB (ultra-wideband) and odometry data, and enabling a robot to follow a target through a suitable communication method and driving method.

Recently, as attempts are being made to install ultra-wideband (UWB)-based communication modules in smartphones, various technologies using UWB are being studied. UWB refers to a wireless communication protocol or wireless communication technology that can transmit large amounts of information at low power over a very wide band compared to existing spectrum.

The characteristics of UWB are very precise spatial recognition and directionality, and it is being actively utilized in research to create a robot that follows a user indoors by taking advantage of UWB's advantage of high distance measurement accuracy among short-range wireless communications.

Most existing UWB-based user-tracking robots use a method in which the user carries a UWB tag and the robot is equipped with four UWB anchors.

However, the existing UWB-based user tracking method cannot be used for ultra-small robots because the four anchors must be spaced a certain distance apart, and the four antennas must be installed very carefully due to the characteristic of UWB being blocked by obstacles.

There is a need for a method that enables robots to effectively track targets while overcoming the shortcomings of existing UWB anchor and tag-based user-tracking robots.

The present invention was derived to solve the shortcomings of the above-mentioned prior art, and an object of the present invention is to provide a method and device for tracking a target by a robot, which enables the robot to effectively track a target by estimating a relative position based on an independent UWB (ultra-wideband) module and odometry without dividing UWB into an anchor and tag system.

Another object of the present invention is to provide a target-tracking robot algorithm that enables a robot to effectively track a target by attaching only one UWB module and one odometry to each of the target and the robot, without the need to attach four UWB modules to the robot.

Another object of the present invention is to provide a target tracking method and device that utilizes a new driving method or a new communication method between a robot and a target, which is not only suitable for an ultra-small unmanned vehicle that does not have space for installing four UWB antennas, but is also useful for causing a robot tracking a target to become a target again and be tracked by another robot.

According to an aspect of the present invention for solving the above technical problem, a method for tracking a target by a robot is provided, wherein a robot equipped with a first ultra-wideband (UWB) communication module and a first driving record measuring device tracks a target, the method comprising: a step of initializing a tracking algorithm of the robot itself in response to a request for tracking of the target; a step of transmitting a single-sided two-way ranging (SS-TWR) poll message; a step of receiving a single response message to the SS-TWR poll message from the target, wherein the single response message includes second driving record information of a second driving record measuring device of the target; a step of estimating a distance to the target based on a round trip delay (RTD) calculated by the SS-TWR method; and a step of predicting a position of the target using the second driving record information and the first driving record information of the first driving record measuring device; and a step of correcting the predicted position of the target based on the estimated distance.

In one embodiment, the initializing step may initialize the relative position vector and covariance based on a tracking request message received from the target according to a predefined protocol.

In one embodiment, the tracking request message may include an address of the second UWB communication module of the target, a relative position tracking algorithm cycle, and precision-related information of the second driving record measuring device.

In one embodiment, the target tracking method may further include, prior to the estimating step, a step of initializing a relative position vector and a covariance.

In one embodiment, the target tracking method may further include a step of waiting until the norm of the covariance becomes less than or equal to a specific value after the initializing step, and starting tracking the target when the norm of the covariance becomes less than or equal to the specific value.

In one embodiment, the predicting step may estimate a relative position with respect to the target using distance information measured by the robot with respect to the target, the first driving record information, and the second driving record information.

In one embodiment, the predicting step can estimate the relative position vector and covariance simultaneously using an extended Kalman filter (EKF).

In one embodiment, the predicting step and the correcting step can estimate the relative position through a relative position estimation algorithm applying an extended Kalman filter:

In one embodiment, the predicting step and the correcting step may be performed to track the target when the covariance of the position of the target falls below a preset value through the relative position estimation algorithm.

In one embodiment, the target tracking method may further include a step of receiving tracking distance related information from the target via UWB data communication while tracking the target based on the corrected position; and a step of adjusting the tracking distance to the target based on the estimated distance related information.

In one embodiment, the first or second driving record measuring device may include an inertial measurement unit (IMU) for attitude estimation, a camera sensor, an accelerometer for displacement estimation, or a combination thereof. In addition, the distance and the position are the same as the attitude of the driving record reference coordinate system of each of the first and second driving record measuring devices, and for this same attitude, each driving record layer device may be equipped with a geomagnetic field.

In one embodiment, the displacement measurement value of the second UWB communication module of the target can be generated by transforming the position change value of the second driving record measuring device into coordinates.

According to another aspect of the present invention for solving the above technical problem, a target tracking device is provided, which is a device for tracking a target, which comprises a first UWB (ultra-wideband) communication module and a first driving record measuring device, and which comprises a processor; instructions executed by the processor; and a memory storing the instructions, wherein, when executed by the processor, the instructions cause the processor to perform the following steps: initializing a tracking algorithm of the robot itself in response to a request for tracking of the target; transmitting a single-sided two-way ranging (SS-TWR) poll message; receiving a single response message to the SS-TWR poll message from the target, wherein the single response message includes second driving record information of a second driving record measuring device of the target; estimating a distance to the target based on a round trip delay (RTD) calculated by the SS-TWR method; predicting a position of the target using the second driving record information and the first driving record information of the first driving record measuring device; and correcting the position of the target based on the estimated distance.

In one embodiment, the target tracking device may cause the processor to initialize the relative position vector and covariance based on a tracking request message received from the target according to a predefined protocol in the initializing step.

In one embodiment, the target tracking device may further cause the processor to perform, prior to the estimating step, a step of initializing a relative position vector and covariance.

In one embodiment, the target tracking device may further perform a step of waiting for the norm of the covariance to become less than or equal to a specific value after the initializing step, and starting tracking of the target when the norm of the covariance becomes less than or equal to the specific value.

In one embodiment, the target tracking device may cause the processor to estimate a relative position with respect to the target using distance information with respect to the target measured by the robot in the predicting step, the first driving record information, and the second driving record information.

In one embodiment, the target tracking device may enable the processor to simultaneously estimate the relative position vector and covariance using an extended Kalman filter during the prediction step.

In one embodiment, the target tracking device may further perform the step of: receiving tracking distance related information from the target via UWB data communication while tracking the target based on the corrected position; and the step of adjusting the tracking distance to the target based on the estimated distance related information.

According to the present invention, a new algorithm can be provided for a robot to effectively track a target based on a relative position estimated through UWB (ultra-wideband) and odometry. That is, while the UWB system used in existing target-tracking robot technology has divided the roles into an anchor and a tag, in this embodiment, the roles of the UWB system are not divided, and an odometry is used together, so that each UWB communication module mounted on the robot and the target respectively becomes a tracking subject and a followed subject, thereby making it possible to simply and efficiently implement a system in which a first robot effectively follows a user while another second robot follows the second robot.

In addition, according to the present invention, it is possible to provide a new target tracking protocol capable of obtaining data required for a tracking algorithm with a small number of communications and an initialization method with less system load than existing UWB systems, and a target tracking method and device using this protocol.

In addition, since the target-following robot algorithm according to the present invention can control the robot using control inputs commonly used in almost all robots, including unmanned vehicles and drones, it has the advantage of being effectively applicable to various robots.

FIG. 1 is an exemplary diagram for explaining the operating principle of a target-tracking robot according to one embodiment of the present invention.
Figure 2 is a drawing for explaining the message transmission process between the target following robot of Figure 1 and the target.
Figure 3 is a flowchart of a target estimation method of the target following robot of Figure 1.
Figure 4 is a flowchart of an additional procedure that can be employed in the target tracking method of Figure 3.
Figure 5 is a flowchart of another additional procedure that can be employed in the target tracking method of Figure 3.
FIG. 6 is a block diagram of the main configuration of a target-following robot according to another embodiment of the present invention.
Figure 7 is an example of a form in which the target-tracking robot of Figure 6 tracks a target while being followed by another robot.
Figure 8 is an example of a form in which a drone, which is a target-tracking robot of Figure 6, follows another drone, which is a target.

The present invention can have various modifications and various embodiments, and specific embodiments are illustrated in the drawings and described in detail. However, this is not intended to limit the present invention to specific embodiments, but should be understood to include all modifications, equivalents, or substitutes included in the spirit and technical scope of the present invention.

The terms first, second, etc. may be used to describe various components, but the components should not be limited by the terms. The terms are only used to distinguish one component from another. For example, without departing from the scope of the present invention, the first component may be referred to as the second component, and similarly, the second component may also be referred to as the first component. The term and/or includes a combination of a plurality of related described items or any one of a plurality of related described items.

When it is said that a component is "connected" or "connected" to another component, it should be understood that it may be directly connected or connected to that other component, but that there may be other components in between. On the other hand, when it is said that a component is "directly connected" or "directly connected" to another component, it should be understood that there are no other components in between.

The terminology used in this application is only used to describe specific embodiments and is not intended to limit the present invention. The singular expression includes the plural expression unless the context clearly indicates otherwise. In this application, it should be understood that the terms "comprises" or "has" and the like are intended to specify the presence of a feature, number, step, operation, component, part or combination thereof described in the specification, but do not exclude in advance the possibility of the presence or addition of one or more other features, numbers, steps, operations, components, parts or combinations thereof.

Unless otherwise defined, all terms used herein, including technical or scientific terms, have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. Terms defined in commonly used dictionaries, such as those defined in common dictionaries, should be interpreted as having a meaning consistent with the meaning they have in the context of the relevant art, and will not be interpreted in an idealized or overly formal sense unless expressly defined in this application.

Throughout the specification, ultra-wideband (UWB) is a wireless communication technology that transmits a large amount of information at low power over a very wide band compared to existing spectrums. In a broad sense, UWB refers to wireless communication that uses a bandwidth of 500㎒ or more, enables short-distance high-speed communication regardless of the presence or absence of a carrier wave or modulation, and has a bandwidth (partial bandwidth) corresponding to its center frequency of 20% or 25% or more.

Also, the term robot may refer to a machine having a human-like appearance and function, or a mechanical device that can operate with a single computer program and automatically perform a complex series of actions, or a mechanical device that automates a task or operation. In this specification, a robot includes any movable mechanical device that automatically adjusts its movement according to the movement of a target in order to follow the target. A robot may include a drone. In this case, a first drone may follow a target in front or a second drone at a certain altitude and at a certain distance.

Hereinafter, with reference to the attached drawings, a preferred embodiment of the present invention will be described in more detail. In order to facilitate an overall understanding in describing the present invention, the same reference numerals are used for the same components in the drawings, and redundant descriptions of the same components are omitted.

FIG. 1 is an exemplary diagram for explaining the operating principle of a target-tracking robot according to an embodiment of the present invention. FIG. 2 is a diagram for explaining a message transmission process between the target-tracking robot of FIG. 1 and a target. FIG. 3 is a flowchart for a target estimation method of the target-tracking robot of FIG. 1. FIG. 4 is a flowchart for an additional procedure that can be employed in the target-tracking method of FIG. 3. And FIG. 5 is a flowchart for another additional procedure that can be employed in the target-tracking method of FIG. 3.

Referring to Fig. 1, a target-tracking robot (hereinafter simply referred to as a robot) (30) tracks a target (10). To this end, the robot (30) is equipped with a first UWB communication module (32) and a second driving recorder (34), and similarly, the target (10) is equipped with a second ultra-wideband (UWB) communication module (12) and a second driving recorder (14).

The UWB communication module may be referred to as a UWB communication device or a UWB module. The driving recorder may be referred to as a driving recorder or a driving record measuring device. And the target (10) may have a form of a person's smartphone or a portable device.

It is assumed that the target (10) can measure its own driving record (odometry) with a second driving recorder (14) and can communicate data through a second UWB communication module (12).

First, when the robot (30) receives a tracking request from the target, the robot (30) executes initialization of its own estimation algorithm. Then, the robot (30) periodically sends a poll message to the target (10) and receives a response message to the poll message from the target (10). The robot (30) obtains odometry information (hereinafter, referred to as second odometry information) of the target (10) through the response message. The poll message may include a SS-TWR (single side two way ranging) poll message (see FIG. 2).

The robot (30) estimates the distance to the target (10) using the second driving record information. Then, the robot (30) predicts the relative position using its own driving record information (hereinafter, the first driving record information) and the second driving record information, and then corrects the predicted relative position based on the previously estimated distance. The prediction and correction may follow the extended Kalman filter (EKF) method.

The robot (30) controls its own direction and speed to follow the target while maintaining a certain distance from the target based on the corrected relative position information. The robot (30) can control the main and subordinate distances, etc. through UWB data communication while following the target (10).

The operating method or target tracking method of the aforementioned robot (30) is described as follows. As shown in Fig. 3, a robot equipped with a first UWB (ultra-wideband) communication module and a first driving record measuring device and tracking a target initializes its own tracking algorithm according to a target tracking request (S31).

Next, the robot transmits a single-sided two-way ranging (SS-TWR) poll message (S32).

Next, the robot receives a response message to the SS-TWR poll message from the target (S33). The response message includes second driving record information of the second driving record measuring device of the target.

Next, the robot estimates the distance to the target based on the round trip delay (RTT) calculated using the SS-TWR method (S35).

Next, the robot predicts the location of the target using the second driving record information and the first driving record information of the first driving record measuring device (S36).

Next, the robot corrects the previously predicted position of the target based on the estimated distance (S37).

The UWB communication device and odometry measurement equipment mounted on the aforementioned target (10) and robot (30) will be described in more detail.

The UWB communication device is a communication device using a general UWB method, and it is assumed that general data communication is possible with other UWB communication devices and that distance measurement is possible. However, unlike a general UWB RTLS (real-time location system), the UWB communication device of the present embodiment does not distinguish between anchors and tags, and can perform the corresponding function operation between single modules. In other words, it is configured so that tags can also operate with each other. In addition, it is assumed that the target knows the UWB communication address of the robot, and the addresses of each other must be known for communication. The target can obtain and store information about the robot through a web service of a robot manufacturer that can be accessed through a network.

Driving record measuring equipment is equipment that can estimate the movement path of each device of a target or robot. It is not necessary to measure the absolute position coordinates, and it is sufficient to be able to compare the current position information with that of a certain time ago. Such driving record measuring equipment can include all devices that satisfy the conditions of driving record measuring equipment, such as estimating attitude through an IMU (inertial measurement unit) indoors and estimating position changes through accelerometer integration.

All coordinate values used in this embodiment are processed as occurring in the central coordinate system of the UWB communication device. The position change measurement of the UWB measurement device can be obtained by transforming the position change value of the driving record measurement device into coordinates.

In order to implement the target-following robot algorithm of this embodiment, both the target and the robot are equipped with a UWB communication device and a driving record measuring device, respectively. However, the UWB communication device and the driving record measuring device of the target and the robot do not necessarily have to be the same devices. In addition, the target is not limited to a portable terminal or portable electronic device carried by a user, and may include another robot that moves on its own.

The following describes the tracking initialization and sending/receiving messages in more detail.

The robot measures the distance using the SS-TWR method through the target's response (resp) message. The robot can estimate the relative position with the target using the EKF method. When using the EKF method, etc., the robot needs to initialize the relative position vector and covariance before estimating the relative position.

Tracking initialization can be done by the robot receiving a tracking request from the target and initializing the relative position vector and covariance. The robot can receive a tracking request message from the target according to a predefined protocol. The tracking request message can include information related to the target UWB communication address, the relative position tracking algorithm cycle, and the accuracy of the odometry.

After receiving a tracking request, the robot can send a SS-TWR poll message to the UWB communication address of the target included in the tracking request message. Then, the robot can receive a SS-TWR resp message from the target that received the message. The robot measures the distance to the target through the SS-TWR of the response (resp) message. After measuring the distance, the relative position vector and its own covariance can be initialized by the method of [Mathematical Formula 1] below.

In [Mathematical Formula 1], s() represents a three-dimensional vector representing the relative position vector from the robot to the target, P() represents the covariance of s(), k represents the initialization gain as a user-tunable value, _d0 represents the distance to the target, and I represents a three-dimensional identity matrix.

After initialization, the robot can wait until the norm of the covariance becomes less than or equal to a specific value specified by the user (see S34 in Fig. 4). Then, when the norm of the covariance becomes less than or equal to the specific value, the robot can start the main process including target tracking or estimation, prediction, and compensation for the same. As a reference, just before the process starts, for effective estimation, the target can move slightly, in which case the norm value can be appropriately reduced. This target motion can be performed according to the self-setting of the target that sent the tracking request or according to the request of the target estimation robot.

Next, we will explain the relative position estimation algorithm in more detail.

The relative position estimation algorithm is an algorithm that estimates the relative position to the target using the distance information d(k) measured by the robot to the target, the driving record information (odom_object) received from the target, and the robot's own driving record information (odom_robot). Since this algorithm is based on the extended Kalman filter, it uses a method of simultaneously estimating the relative position vector and covariance. The prediction and correction of the extended Kalman filter are as shown in [Mathematical Formula 2].

[In Mathematical Expression 2], s(k) is a three-dimensional vector representing a relative position vector from a robot to a target at a specific time k, which is a user-tunable value; P(k) represents the covariance of s(k); Q _object represents the precision of the second driving record measuring device of the target received at initialization; and Q _robot represents the precision of the first driving record measuring device of the robot. The measurement model H(k) can be obtained through the difference between the first and second driving record measuring devices; and R represents the distance measurement precision of UWB.

Each precision is information that can be obtained by analyzing sensor noise, but can be tuned and used according to the user's convenience. The measurement model H(k) can be obtained through the difference between driving records. R means the distance measurement precision of UWB (R _UWB ). Based on this data, an extended Kalman filter can be applied, and the relative position of the target can be estimated through this.

A relative position estimation algorithm that can be employed in the target estimation method of the present embodiment can utilize a unique UWB message transmission protocol.

In other words, since a general UWB module uses the function for distance measurement and the function for data transmission separately, at least three message exchanges are required: sending an SS-TWR poll message from the robot to the target, sending an SS-TWR resp message from the target to the robot, and transmitting driving record data from the target to the robot. In addition, when operated asynchronously, the UWB communication module of a general target also needs time to request data from the driving record measuring equipment and acquire the driving record data.

On the other hand, referring back to FIG. 2, the UWB module of the present embodiment or the target-tracking robot equipped with the same uses a message structure combining synchronous SS-TWR and data communication or a communication method utilizing such a message structure.

That is, in the present embodiment, the robot transmits a SS-TWR poll message (poll msg) to the object at the first time (T_0) and receives an SS-TWR resp message (resp msg) from the object. The poll message may include message transmission time (Tx_poll_time) information. At this time, the UWB communication module (UWB module) of the object of the present embodiment, unlike the conventional general method, obtains and stores odometry data from the second odometry measurement device (odom module) according to a predetermined cycle. In addition, the object additionally inserts its own odometry data, such as a odometry message (odometry msg), into the SS-TWR resp message and transmits it. The SS-TWR resp message may include a protocol header and 1 byte data for SS-TWR, or may include 3 symbols of 8 bytes each (8bytes*3 symbol). For example, the SS-TWR resp message may include the reception time of the poll message (Rx_poll_time), the transmission time of the response message (Tx_resp_time), and odometry data.

In accordance with the period set in the tracking initialization phase, the robot and the object exchange two messages, the SS-TWR poll message and the SS-TWR resp message, through which the robot can obtain all the information for estimating the relative position.

Next, we will explain the robot's target tracking algorithm in more detail.

After tracking initialization, if the covariance of the relative position goes below a specific value specified by the user or a third party through the relative position tracking algorithm, the robot can be controlled through the target tracking algorithm to track the target.

We assume that the robot can estimate its own heading (yaw) and its own velocity, and control its own heading (yaw rate) and forward (velocity_front) velocity. We do not consider the robot's up-and-down motion. This motion is an assumption that is suitable for drones as well as ground-based mobile robots.

The control goal of the target tracking method according to the present embodiment is to control the drone so that its front always faces the target and the distance from the target is constant. To achieve the control goal, a control algorithm such as [Mathematical Formula 3] can be used.

In [Mathematical expression 3], K _yaw is a coefficient for attitude control, and K _vel is a coefficient for velocity control. The coefficients are generally given, and the user can tune the coefficients for desired performance. In addition, D _desired is the distance between the target and the robot desired by the user, and can be changed to general data communication via UWB during the tracking algorithm operation.

That is, as illustrated in Fig. 5, the robot receives tracking distance related information from the target (S38) and can adjust the tracking distance based on the tracking distance related information (S39).

In this way, the target estimation technology of the robot according to the present embodiment can provide an algorithm for estimating the relative position of the target in the robot-centered coordinate system by fusing distance measurement data through UWB and odometry data, a communication method suitable for the algorithm, and a driving method for making the robot follow the target after the relative position estimation.

Fig. 6 is a block diagram of the main configuration of a target-following robot according to another embodiment of the present invention. Fig. 7 is an exemplary diagram of a form in which the target-following robot of Fig. 6 follows a target while being followed by another robot. And Fig. 8 is an exemplary diagram of a form in which the target-following robot, which is a drone, of Fig. 6 follows another drone, which is a target.

Referring to FIG. 6, the target following robot (100) may include at least one processor (110) and memory (120). In addition, the target following robot (100) may be connected to a network interface (I/F) that is connected to a network and performs communication. The network interface (140) may be included in the target following robot (100). In addition, the target following robot (100) may be connected to a UWB communication module (150) and an odometry measurement device (160). The UWB communication module (150) and the odometry measurement device (160) may be included in the robot (100).

The processor (110) can execute a program or instructions stored in the memory (120) or a storage device corresponding thereto. The processor (110) may mean a central processing unit (CPU), a graphics processing unit (GPU), or a dedicated processor in which methods according to embodiments of the present invention are performed. The memory (120) or the storage device may be configured with at least one of a volatile storage medium and a nonvolatile storage medium. For example, the memory (120) may be configured with at least one of a read only memory (ROM) and a random access memory (RAM).

In addition, the target following robot (100) may include a target following robot algorithm (130). The target following robot algorithm (130) may include an initialization module (131), a waiting control module (132), an estimation module (133), a prediction module (134), a correction module (135), and a following distance adjustment module (136). The target following robot algorithm (130) may be stored in a memory (120) as data (122) or a part of a program, a part of instructions (124), and may be loaded onto a processor (110) and executed.

The message transmission/reception module can transmit a single-sided two-way ranging (SS-TWR) poll message to a target through a network interface (140) and receive a response message to the SS-TWR poll message from the target. The response message includes second driving record information of the second driving record measuring device of the target.

The initialization module (131) initializes the relative position vector and covariance to be used in the robot's own tracking algorithm according to the target tracking request.

The standby control module (132) can delay the start of the operation of the tracking process until the norm of the covariance becomes less than or equal to a user-specified specific value after initialization.

The estimation module (133) can estimate the relative position to the target based on the center coordinate system of the UWB module of the robot based on the response message of the target. That is, the estimation module (133) can estimate the distance to the target based on the round trip delay (RTT) calculated using the SS-TWR method.

The prediction module (134) can predict the location of the target using the second driving record information and the first driving record information of the first driving record measuring device.

The correction module (135) can correct the previously predicted position of the target based on the estimated distance.

The tracking distance adjustment module (136) can adjust the tracking distance according to tracking distance related information received from the target.

According to the present embodiment, as shown in Fig. 7, a first robot (30) can track a target (10), and a second robot (50) equipped with a third UWB communication module (52) and a third driving record measuring device (54) can track the first robot. At this time, the first robot (30) can obtain position information for tracking the target through one round-trip message transmission and reception with the target (10), and the second robot (50) can obtain position information for tracking the first robot (30) through one round-trip message transmission and reception with the first robot (30).

Also, as illustrated in FIG. 8, it can be applied to drones (70, 90), and it is assumed that each drone can estimate its own direction, for example, the yaw direction and its own speed, and control its own yaw rate and velocity_front speed. At this time, the up-and-down movement of the drone is not considered. The second drone (90) can share distance measurement and driving records with the first drone (70), estimate the relative position (Lp) of the first drone (70), and track (Fm) the first drone (70) based on this through a target tracking algorithm. The target tracking method according to the present embodiment can be used to control the front of the second drone (90) to face the first drone (70) and the distance from the first drone (70) to be constant.

The methods according to the present invention may be implemented in the form of program commands that can be executed through various computer means and recorded on a computer-readable medium. The computer-readable medium may include program commands, data files, data structures, etc., alone or in combination. The program commands recorded on the computer-readable medium may be those specially designed and configured for the present invention or may be known and available to those skilled in the art of computer software.

Examples of computer-readable media include hardware devices specifically configured to store and execute program instructions, such as ROM, RAM, and flash memory. Examples of program instructions include not only machine language codes generated by a compiler, but also high-level language codes that can be executed by a computer using an interpreter, etc. The above-described hardware devices can be configured to operate with at least one software module to perform the operations of the present invention, and vice versa.

Although the present invention has been described with reference to the above embodiments, it will be understood by those skilled in the art that various modifications and changes can be made to the present invention without departing from the spirit and scope of the present invention as set forth in the claims below.

Claims (20)

A method for tracking a target in a robot equipped with a first UWB (ultra-wideband) communication module and a first driving record measuring device,
A step of initializing the robot's own tracking algorithm according to the target's tracking request;
A step of transmitting a SS-TWR (single-sided two-way ranging) poll message;
A step of receiving a single response message to the SS-TWR poll message from the target, the single response message including second driving record information of a second driving record measuring device of the target;
A step of estimating the distance to the target based on the round trip delay calculated using the SS-TWR method; and
A step of predicting the location of the target using the second driving record information and the first driving record information of the first driving record measuring device; and
A step of correcting the position of the target based on the estimated distance;
Including,
The first or second driving record measuring device includes an inertial measurement unit (IMU) for attitude estimation and an accelerometer for displacement estimation.
The above distance and the above location are calculated as coordinate values of the module coordinate system centered on the first UWB communication module,
A target tracking method in which the displacement measurement value of the second UWB communication module of the above target is generated by converting the position change value of the second driving record measuring device into coordinates.
In claim 1,
A target tracking method, wherein the initializing step initializes a relative position vector and covariance based on a tracking request message received from the target according to a pre-specified protocol.
In claim 2,
A target tracking method, wherein the tracking request message includes an address of the second UWB communication module of the target, a relative position tracking algorithm cycle, and information related to the precision of the second driving record measuring device.
In claim 1,
A target tracking method, further comprising a step of initializing a relative position vector and covariance before the above estimating step.
In claim 4,
A target tracking method further comprising: a step of waiting until the norm of the covariance becomes less than or equal to a specific value after the initializing step, and starting tracking of the target when the norm of the covariance becomes less than or equal to the specific value.
In claim 1,
The above-mentioned predicting step is a target tracking method in which the robot estimates a relative position with respect to the target using distance information with respect to the target measured by the robot, the first driving record information, and the second driving record information.
In claim 6,
The above-mentioned predicting step is a target tracking method that simultaneously estimates the relative position vector and covariance using an extended Kalman filter.
In claim 1,
The above-mentioned predicting step and the above-mentioned correcting step estimate the relative position through a relative position estimation algorithm that applies an extended Kalman filter,
[Relative Position Estimation Algorithm]

A target tracking method wherein, in the above relative position estimation algorithm, s() represents a three-dimensional vector meaning a relative position vector from a robot to a target, P() represents the covariance of s(), k represents a user-tunable value, Q_object represents the precision of the second driving record measuring device of the target received at initialization, and Q_robot represents the precision of the first driving record measuring device of the robot, a measurement model H can be obtained through the difference between the first and second driving record measuring devices, and R represents the distance measurement precision of UWB.
In claim 8,
A target tracking method, wherein the above-mentioned predicting step and the above-mentioned correcting step are performed for tracking the target when the covariance of the position of the target falls below a preset value through the relative position estimation algorithm.
In claim 1,
A step of receiving tracking distance related information from the target via UWB data communication while tracking the target based on the corrected position; and
A target tracking method further comprising a step of adjusting a tracking distance to the target based on the tracking distance related information.
delete delete A device that tracks a target and is equipped with a first UWB (ultra-wideband) communication module and a first driving record measuring device.
processor;
instructions executed by said processor; and
Contains a memory for storing the above commands,
When executed by said processor, said instructions cause said processor to:
A step of initializing the robot's own tracking algorithm according to the target's tracking request;
A step of transmitting a SS-TWR (single-sided two-way ranging) poll message;
A step of receiving a single response message to the SS-TWR poll message from the target, the single response message including second driving record information of a second driving record measuring device of the target;
A step of estimating the distance to the target based on the round trip delay calculated using the SS-TWR method; and
A step of predicting the location of the target using the second driving record information and the first driving record information of the first driving record measuring device; and
A step of correcting the position of the target based on the estimated distance;
To perform,
The first or second driving record measuring device includes an inertial measurement unit (IMU) for attitude estimation and an accelerometer for displacement estimation,
The above distance and the above location are calculated as coordinate values of the module coordinate system centered on the first UWB communication module,
A target tracking device in which the displacement measurement value of the second UWB communication module of the above target is generated by converting the position change value of the second driving record measuring device into coordinates.
In claim 13,
A target tracking device, wherein the processor initializes a relative position vector and covariance based on a tracking request message received from the target according to a pre-designated protocol in the initializing step.
In claim 14,
A target tracking device, wherein the tracking request message includes the address of the second UWB communication module of the target, a relative position tracking algorithm cycle, and information related to the precision of the second driving record measuring device.
In claim 13,
A target tracking device, wherein the processor further performs, before the estimating step, a step of initializing a relative position vector and covariance.
In claim 16,
A target tracking device which causes the processor to further perform a step of waiting until the norm of the covariance becomes less than or equal to a specific value after the initializing step, and starting tracking of the target when the norm of the covariance becomes less than or equal to the specific value.
In claim 13,
A target tracking device that causes the processor to estimate a relative position with respect to the target using distance information from the target measured by the robot in the predicting step, the first driving record information, and the second driving record information.
In claim 18,
A target tracking device, wherein the processor simultaneously estimates a relative position vector and covariance using an extended Kalman filter in the predicting step.
In claim 13,
The above processor,
A step of receiving tracking distance related information from the target via UWB data communication while tracking the target based on the corrected position; and
A target tracking device further comprising: a step of adjusting a tracking distance to the target based on the tracking distance related information.

Images