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

Abstract

A method and device for tracking the relative position of a target in a robot-centered coordinate system by fusing distance measuring data and odometer data through UWB and enabling a robot to track the target through an appropriate communication method and driving method are disclosed. A target tracking method comprises the steps of: initializing a tracking algorithm of the robot according to a tracking request of the target; transmitting an 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 odometer information of a second odometer measuring device of the target; estimating based on the round-trip delay time calculated by using an SS-TWR method; predicting the position of the target through the second odometer information and first odometer information of a first odometer measuring device of the robot; and correcting the predicted position of the target based on the estimated distance.

Description

Method and apparatus for following a target of a robot using UWB and odometer-based relative position estimation

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

Recently, as an attempt to mount an ultra-wideband (UWB)-based communication module in a smartphone has occurred, various technologies using UWB have been studied. UWB refers to a wireless communication protocol or wireless communication technology capable of transmitting a large amount of information with low power over a very wide band compared to a conventional spectrum.

The characteristics of UWB are very precise spatial recognition and directionality, and UWB's advantage of high distance measurement accuracy during short-range wireless communication is being actively used in research to create robots that follow users indoors.

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

However, the conventional UWB-based user following method cannot be used for microrobots because the four anchors must be spaced at a certain distance or more, and the four antennas must be installed very carefully due to the nature of UWB being blocked by obstacles.

There is a demand for a method for a robot to effectively follow a target while overcoming the disadvantages of the existing UWB anchor and tag-based user-following robot.

Derived to solve the above-mentioned disadvantages of the prior art of the present invention, an object of the present invention is an independent UWB (ultra-wideband) module and odometer (without dividing UWB into anchor and tag systems) An object of the present invention is to provide a method and apparatus for following a target of a robot, which enables the robot to effectively follow a target by estimating a relative position based on odometry.

Another object of the present invention is a target-following robot that does not require attaching four UWB modules to the robot and enables the robot to effectively follow the target by attaching only one UWB module and odometry to the target and the robot. Algorithms are provided.

Another object of the present invention is a new driving method that is not only suitable for an ultra-small unmanned mobile vehicle with no space in which four UWB antennas are installed, but also useful for making a robot that follows a target become a target again and follow another robot. Or to provide a target tracking method and device using a new communication method between a robot and a target.

A method for following a target of a robot according to an aspect of the present invention for solving the above technical problem is a method for following a target in a robot equipped with a first ultra-wideband (UWB) communication module and a first driving record measurement device, Initializing the robot's own tracking algorithm according to the target's tracking request; sending 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 history information of a second driving history measuring device of the target; estimating a distance to the target based on a round trip delay (RTD) calculated by an SS-TWR method; and predicting a location of the target through the second driving record information and the first driving record information of the first driving record measuring device. and correcting the predicted position of the target based on the estimated distance.

In an embodiment, in the initializing, the relative position vector and covariance may be initialized based on a follow-up request message received from the target according to a predetermined protocol.

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

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

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

In one embodiment, the predicting may include estimating a relative position with the target through information on a distance to the target measured by the robot, the first driving record information, and the second driving record information.

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

In one embodiment, in the predicting and correcting steps, the relative position may be estimated through a relative position estimation algorithm to which an extended Kalman filter is applied:

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

In one embodiment, the method for following a target may include receiving tracking distance-related information from a target through UWB data communication while tracking the target based on the corrected position; and adjusting a tracking distance to the target based on the estimated distance related information.

In one embodiment, the first or second driving record measurement device may include an inertial measurement unit (IMU) for posture estimation, a camera sensor, an accelerometer for displacement estimation, or a combination thereof. Further, the distance and the position have the same attitude of the travel history reference coordinate system of each of the first and second travel history measurement devices, and each travel history measurement device may include a geomagnetic device for this same attitude.

In one embodiment, the displacement measurement value of the second UWB communication module of the target may be generated by coordinate transformation of the location change value of the second driving history measurement device.

A target tracking device according to another aspect of the present invention for solving the above technical problem is a device for following a target equipped with a first ultra-wideband (UWB) communication module and a first driving record measurement device, comprising: a processor; instructions executed by the processor; and a memory for storing the instructions, wherein when executed by the processor, the instructions cause the processor to: initialize a robot's own tracking algorithm according to a target tracking request; sending 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 history information of a second driving history measuring device of the target; estimating a distance to the target based on a round trip delay (RTD) calculated by an SS-TWR method; predicting a location of the target through 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 an embodiment, the target tracking device may cause the processor to initialize the relative position vector and the covariance based on a tracking request message received from the target according to a predetermined protocol in the initializing step.

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

In one embodiment, the target tracking device waits until the norm of the covariance becomes less than or equal to a specific value after the initializing step, and tracks the target when the norm of the covariance becomes less than or equal to the specific value. You can do more steps to get started.

In an embodiment, the target following device may cause a processor to estimate a relative position with the target through information on the distance to the target measured by the robot in the predicting step, the first driving record information, and the second driving record information. can make it

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

In one embodiment, the target tracking device includes: receiving, by a processor, tracking distance-related information from the target through UWB data communication while tracking the target based on the corrected position; and adjusting a tracking distance to the target based on the estimated distance related information.

According to the present invention, it is possible to provide a new algorithm that enables a robot to effectively follow a target based on a relative position estimated through ultra-wideband (UWB) and odometry. That is, the UWB system used in the existing target-following robot technology divided the roles into anchor and tag, but in this embodiment, the role of the UWB system is not divided and the odometer is used together to track the robot and the target. Each UWB communication module mounted on each is implemented to be a subject that is followed and a subject that is being followed, thereby enabling the first robot to effectively follow the user and another second robot to follow the second robot. It can be implemented simply and efficiently.

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

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

1 is an exemplary diagram for explaining the operating principle of a target following robot according to an embodiment of the present invention.
FIG. 2 is a diagram for explaining a message transfer process between the target following robot of FIG. 1 and a target.
FIG. 3 is a flowchart of a method for estimating a target of the target following robot of FIG. 1 .
FIG. 4 is a flowchart of additional procedures that may be employed in the target tracking method of FIG. 3 .
FIG. 5 is a flowchart of yet another additional procedure that may be employed in the target tracking method of FIG. 3 .
6 is a block diagram of the main components of a target following robot according to another embodiment of the present invention.
FIG. 7 is an exemplary view of a form in which the target following robot of FIG. 6 follows another robot while following a target.
FIG. 8 is an exemplary view of a form in which a drone, which is the target following robot of FIG. 6 , follows another drone as a target.

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

Terms such as first and second may be used to describe various components, but the components should not be limited by the terms. These terms are only used for the purpose of distinguishing one component from another. For example, a first element may be termed a second element, and similarly, a second element may be termed a first element, without departing from the scope of the present invention. The terms and/or include any combination of a plurality of related recited items or any of a plurality of related recited items.

It is understood that when an element is referred to as being "connected" or "connected" to another element, it may be directly connected or connected to the other element, but other elements may exist in the middle. It should be. On the other hand, when an element is referred to as “directly connected” or “directly connected” to another element, it should be understood that no other element exists in the middle.

Terms used in this application are only used to describe specific embodiments, and are not intended to limit the present invention. Singular expressions include plural expressions unless the context clearly dictates otherwise. In this application, the terms "include" or "have" are intended to designate that there is a feature, number, step, operation, component, part, or combination thereof described in the specification, but one or more other features It should be understood that the presence or addition of numbers, steps, operations, components, parts, or combinations thereof is not precluded.

Unless defined otherwise, 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 the present invention belongs. Terms such as those defined in commonly used dictionaries should be interpreted as having a meaning consistent with the meaning in the context of the related art, and unless explicitly defined in this application, they should not be interpreted in an ideal or excessively formal meaning. don't

Throughout the specification, ultra-wideband (UWB) is a wireless communication technology that transmits a large amount of information with low power over a very wide band compared to the existing spectrum. It refers to wireless communication in which short-distance high-speed communication is possible regardless of the bandwidth (non-bandwidth) corresponding to the center frequency of 20% or 25% or more.

In addition, the term robot refers to a machine with a human-like appearance and function, or a mechanical device that can be operated by a single computer program and automatically performs a complex series of actions. , it can refer to a mechanical device that automates certain tasks or operations. 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. Robots may include drones. In this case, the first drone may follow the front target or the second drone at a certain altitude and at a certain distance.

Hereinafter, with reference to the accompanying drawings, preferred embodiments of the present invention will be described in more detail. In order to facilitate overall understanding in the description of 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.

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

Referring to FIG. 1 , a target following robot (hereinafter simply a robot) 30 follows a target 10 . To this end, the robot 30 includes a first UWB communication module 32 and a second driving recorder 34, and similarly, the target 10 also has a second ultra-wideband (UWB) communication module ( 12) and a second travel recording device 14.

A UWB communication module may be referred to as a UWB communication device or a UWB module. The driving recorder may be referred to as an odometer or an odometer measuring device. And the target 10 may have the form of a person's smart phone or portable device.

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

First, when the robot 30 receives a follow-up request from a target, the robot 30 executes its own estimation algorithm initialization. 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 a response message. The poll message may include a single side two way ranging (SS-TWR) poll message (see FIG. 2).

The robot 30 estimates the distance to the target 10 through the second driving record information. In addition, the robot 30 predicts the relative position through its own driving record information (hereinafter referred to as first driving record information) and second driving record information, and then corrects the predicted relative position based on the previously estimated distance. Prediction and correction may follow an extended Kalman filter (EKF) scheme.

The robot 30 maintains a certain distance from the target based on the corrected relative position information and controls its own direction and speed to follow the target and move. While following the target 10, the robot 30 may control a master/serve distance through UWB data communication.

The operation method or the target tracking method of the above-described robot 30 will be described. As shown in FIG. 3, a robot following a target equipped with a first UWB (ultra-wideband) communication module and a first driving record measurement device initializes its own tracking algorithm according to the target's following 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 travel history information of the second travel history measuring device of the target.

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

Next, the robot predicts the location of the target through 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 above-described target 10 and the robot 30, respectively, will be described in more detail.

It is assumed that the UWB communication device is communication equipment using a general UWB method, and can perform general data communication with other UWB communication devices and measure distance. However, unlike a general UWB real-time location system (RTLS), the UWB communication device of the present embodiment does not distinguish between an anchor and a tag, and allows corresponding functional operation between single modules. That is, 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, while each other needs to know the address for communication. The target may acquire and store information about the robot through a web service of a robot manufacturer accessible through a network.

The driving record measuring device is a device capable of estimating the movement path of each device of a target or robot. It is not necessary to measure the absolute position coordinates, and it is only necessary to compare the position information of a certain time ago and the current position. The driving record measuring equipment may include all devices that satisfy the conditions of driving record measuring equipment, such as position estimation through an inertial mesaurement unit (IMU) and position change estimation through accelerometer integration.

Coordinate values used in this embodiment are all processed as occurring in the center coordinate system of the UWB communication device. The measurement of the position change of the UWB measuring device can be obtained by converting the position change value of the driving record measuring device into coordinates.

To implement the target-following robot algorithm of this embodiment, a UWB communication device and a driving record measurement device are mounted on both the target and the robot. However, the UWB communication device and driving record measurement device in the target and the robot do not necessarily have to be the same device. Also, the target is not limited to a portable terminal or portable electronic device carried by a user, and may include other robots that move by themselves.

The following describes follow-up initialization and sending/receiving messages in more detail.

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

Initialization of follow-up may consist of an operation in which the robot receives a follow-up request from a target and initializes a relative position vector and covariance. The robot may receive a follow-up request message from a target according to a predetermined protocol. The tracking request message may include information related to the target UWB communication address, relative position tracking algorithm cycle, and accuracy of odometry.

After receiving the follow-up request, the robot can send an SS-TWR poll message to the target's UWB communication address included in the follow-up request message. And the robot can receive the 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 resp message. After measuring the distance, the relative position vector and its covariance can be initialized by the method of [Equation 1] below.

In [Equation 1], s() is a three-dimensional vector meaning a relative position vector from the robot to the target, P() is the covariance of s(), and k is a value tunable by the user. Initialization gain gain), d ₀ is the distance to the target, and I is the 3D identity matrix.

After initialization, the robot may wait until the norm of the covariance becomes equal to or less than a specific value specified by the user (see S34 in FIG. 4 ). In addition, when the norm of covariance becomes less than or equal to a specific value, the robot may start a main process including target tracking or estimation, prediction, and correction for this purpose. As a reference, right before the start of the process, for effective estimation, the target may move slightly, in which case the norm value may decrease appropriately. Such an operation of the target may be performed according to the self-setting of the target that has sent the follow-up request or according to the request of the target estimation robot.

Next, the relative position estimation algorithm will be described in more detail.

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

In [Equation 2], s(k) is a 3D vector meaning the relative position vector from the robot to the target at a specific time k, which is a value tunable by the user, P(k) is the covariance of s(k), Q _object represents the accuracy of the second driving record measuring device of the target received at the time of initialization, and Q _robot represents the precision of the first driving record measuring device of the robot, respectively, and the measurement model H(k) represents the first and second driving records It can be obtained through differences in measurement devices, and R represents the distance measurement accuracy of UWB.

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

The relative position estimation algorithm that can be employed in the target estimation method of this embodiment can use a unique UWB message transfer protocol.

In other words, since the general UWB module separately uses the function for distance measurement and the data transmission function, SS-TWR poll message is sent from the robot to the target, SS-TWR resp message is sent from the target to the robot, and the robot is driven from the target. It takes at least three message exchanges in total to convey the historical data. In addition, when driven asynchronously, the UWB communication module of a general target requires time to request data from the driving record measuring device and acquire the driving record data.

On the other hand, referring to FIG. 2 again, the UWB module or the target following robot equipped with the UWB module of this embodiment uses a message structure combining synchronous SS-TWR and data communication or a communication method using such a message structure.

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

The robot and the object exchange two messages, SS-TWR poll message and SS-TWR resp message, according to the period set in the follow-up initialization stage. All information can be obtained.

Next, the target tracking algorithm of the robot will be described in more detail.

After the tracking is initialized, if the covariance of the relative position through the relative position following algorithm goes down below a specific value specified by the user or the like, the robot can be controlled through the target following algorithm to follow the target.

It is assumed that the robot can estimate its own direction (yaw) and its own speed, and can control its own direction (yaw rate) and forward speed (velocity_front). At this time, the vertical movement of the robot is not considered. This movement is a good assumption for drones as well as ground mobile robots.

The control goal of the target tracking method according to the present embodiment is to control the drone so that the front of the drone always faces the target and the distance from the target is constant. In order to reach the control target, a control algorithm such as [Equation 3] may be used.

In [Equation 3], K _yaw is a coefficient for attitude control, and K _vel is a coefficient for speed control. The coefficients are given generically and the user can tune them for desired performance. And D _desired is the distance between the target and the robot desired by the user, and can be changed to general data communication through UWB while the tracking algorithm is running.

That is, as shown in FIG. 5 , the robot may receive tracking distance related information from a target (S38) and adjust the following distance based on the following tracking distance related information (S39).

As such, in the robot target estimation technology according to the present embodiment, an algorithm for estimating the relative position of a target in the robot center coordinate system by fusing distance measurement data and odometry data through UWB, and a communication method suitable for this algorithm, In addition, a driving method for making the robot follow the target after estimating the relative position may be provided.

6 is a block diagram of the main components of a target following robot according to another embodiment of the present invention. FIG. 7 is an exemplary view of a form in which the target following robot of FIG. 6 follows another robot while following a target. 8 is an exemplary view of a form in which the drone, which is the target-following robot of FIG. 6, follows another drone as a target.

Referring to FIG. 6 , the target following robot 100 may include at least one processor 110 and a memory 120 . Also, 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 also be included in the target following robot 100 . In addition, the target following robot 100 may be connected to the UWB communication module 150 and the odometry measurement equipment 160 . The UWB communication module 150 and the odometry measurement equipment 160 may be included in the robot 100 .

The processor 110 may execute programs or instructions stored in the memory 120 or a storage device corresponding thereto. The processor 110 may refer to a central processing unit (CPU), a graphics processing unit (GPU), or a dedicated processor on which methods according to embodiments of the present invention are performed. The memory 120 or the storage device may be composed of at least one of a volatile storage medium and a non-volatile storage medium. For example, the memory 120 may include 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, an atmospheric control module 132, an estimation module 133, a prediction module 134, a correction module 135, and a tracking distance adjustment module 136. there is. The target following robot algorithm 130 may be stored in the memory 120 as data 122 or part of a program or part of instructions 124, and may be loaded and executed in the processor 110.

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

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

After initialization, the standby control module 132 may delay the starting point of the follow-up process until the norm of the covariance becomes equal to or less than a specific value designated by the user.

The estimation module 133 may estimate a relative position with 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 may estimate the distance to the target based on the round trip delay (RTT) calculated using the SS-TWR method.

The prediction module 134 may predict the location of the target through the second driving record information and the first driving record information of the first driving record measuring device.

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

The following distance adjustment module 136 may adjust the following distance according to the following distance related information received from the target.

According to this embodiment, as shown in FIG. 7, the first robot 30 follows the target 10, and the third UWB communication module 52 and the third driving record measuring device 54 are installed. The second robot 50 can follow the first robot. At this time, the first robot 30 obtains location information for following the target through one-time round-trip message transmission and reception with the target 10, and the second robot 50 receives one round-trip message with the first robot 30. Position information for following the first robot 30 may be acquired through transmission and reception.

In addition, as shown in FIG. 8, it can be applied to the drones 70 and 90, and each drone can estimate its own direction, for example, its own yaw direction and its own speed, and its own yaw rate. ) and forward speed (velocity_front) are assumed to be controllable. At this time, the vertical movement of the drone is not considered. The second drone 90 may share the distance measurement and driving record with the first drone 70, estimate the relative position Lp of the first drone 70, and based on this estimate, the second drone 70 performs a target tracking algorithm. 1 drone 70 can be followed (Fm). The target following method according to the present embodiment may be used to control the front of the second drone 90 to face the first drone 70 and to have a constant distance from the first drone 70 .

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

Examples of computer readable media include hardware devices specially configured to store and execute program instructions, such as ROM, RAM, flash memory, and the like. Examples of program instructions include high-level language codes that can be executed by a computer using an interpreter or the like as well as machine language codes such as those produced by a compiler. The hardware device described above may be configured to operate with at least one software module to perform the operations of the present invention, and vice versa.

Although described with reference to the above embodiments, those skilled in the art will understand that the present invention can be variously modified and changed without departing from the spirit and scope of the present invention described in the claims below. You will be able to.

Claims (20)

A method for following a target in a robot equipped with a first ultra-wideband (UWB) communication module and a first driving record measuring device, comprising:
Initializing the robot's own tracking algorithm according to the target's tracking request;
sending 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 history information of a second driving history measuring device of the target;
estimating a distance to the target based on a round trip delay calculated using the SS-TWR method; and
predicting a location of the target through 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;
Target tracking method comprising a.
The method of claim 1,
The initializing step may include initializing a relative position vector and a covariance based on a follow-up request message received from the target according to a predetermined protocol.
The method of claim 2,
The tracking request message includes an address of a second UWB communication module of the target, a period of a relative position tracking algorithm, and accuracy-related information of the second driving record measuring device.
The method of claim 1,
The target tracking method of claim 1 , further comprising initializing a relative position vector and covariance before the estimating step.
The method of claim 4,
After the initializing step, waiting until the norm of the covariance becomes less than or equal to a specific value, and starting tracking the target when the norm of the covariance becomes less than or equal to the specific value; Way.
The method of claim 1,
The predicting step may include estimating a relative position with the target based on distance information from the target measured by the robot, the first driving record information, and the second driving record information.
The method of claim 6,
In the step of predicting, a relative position vector and covariance are simultaneously estimated using an extended Kalman filter.
The method of claim 1,
In the predicting step and the correcting step, the relative position is estimated through a relative position estimation algorithm to which an extended Kalman filter is applied,
[Relative Position Estimation Algorithm]

In the relative position estimation algorithm, s() is a 3D vector representing the relative position vector from the robot to the target, P() is the covariance of s(), k is a value tunable by the user, and Q_object is initialized and Q_robot represents the precision of the first driving record measuring device of the robot, respectively, and the measurement model H can be obtained through the difference between the first and second driving record measuring devices. and R represents the distance measurement accuracy of UWB.
The method of claim 8,
The predicting step and the correcting step are performed to track the target when the covariance of the position of the target through the relative position estimation algorithm goes down to a predetermined value or less.
The method of claim 1,
receiving tracking distance-related information from the target through UWB data communication while tracking the target based on the corrected position; and
and adjusting a tracking distance to the target based on the estimated distance related information.
The method of claim 1,
The first or second driving record measurement device includes an inertial measurement unit (IMU) for attitude estimation and an accelerometer for displacement estimation,
The distance and the location are calculated as coordinate values of a module coordinate system centered on the first UWB communication module.
The method of claim 11,
The displacement measurement value of the second UWB communication module of the target is generated by coordinate transformation of the position change value of the second driving record measuring device.
A device for following a target equipped with a first ultra-wideband (UWB) communication module and a first driving record measuring device, comprising:
processor;
instructions executed by the processor; and
a memory for storing the instructions;
When executed by the processor, the instructions cause the processor to:
Initializing the robot's own tracking algorithm according to the target's tracking request;
sending 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 history information of a second driving history measuring device of the target;
estimating a distance to the target based on a round trip delay calculated using the SS-TWR method; and
predicting a location of the target through 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;
A target tracking device to perform
The method of claim 13,
wherein the processor initializes a relative position vector and covariance based on a follow-up request message received from the target according to a predetermined protocol in the initializing step.
The method of claim 14,
The tracking request message includes an address of a second UWB communication module of the target, a period of a relative position tracking algorithm, and accuracy-related information of the second driving record measuring device.
The method of claim 13,
and causing the processor to further perform a step of initializing a relative position vector and a covariance before the step of estimating.
The method of claim 16
After the initializing step, the processor waits until the norm of the covariance becomes less than or equal to a specific value, and starts following the target when the norm of the covariance becomes less than or equal to the specific value. target tracking device.
The method of claim 13,
wherein the processor estimates a relative position with the target through distance information to the target measured by the robot in the predicting step, the first driving record information, and the second driving record information.
The method of claim 18
and causing the processor to simultaneously estimate a relative position vector and a covariance using an extended Kalman filter in the predicting step.
The method of claim 13,
the processor,
receiving tracking distance-related information from the target through UWB data communication while tracking the target based on the corrected position; and
and adjusting a tracking distance to the target based on the estimated distance related information.

Images