TWI608320B - Three dimensional trace verification apparatus and method thereof - Google Patents

Description

Three-dimensional trajectory verification device and method thereof

The present invention relates to a trajectory verification apparatus and method thereof, and more particularly to a three-dimensional trajectory verification apparatus and method thereof.

With the advancement of science and technology, the quality of human requirements for living standards has also increased relatively. In recent years, electronic devices with different functions on the market have sprung up. Among them, electronic devices such as drones, balance robots, sweepers, and camera heads that require attitude balance control from time to time have received increasing attention.

The above-mentioned electronic device that needs to perform attitude balance control from time to time usually includes a sensing module and a platform module, and the sensing module is used to detect and define the position of the platform module in a three-dimensional space. However, the platform module is in a three-dimensional space. The location is difficult to define and track, and the electronic devices are often subject to external interference, causing the motion trajectory of the platform module to not match the expected trajectory. In addition, the difference between the motion trajectory of the platform module and the expected trajectory is also difficult to quantify.

In summary, it can be seen that in the prior art, it has been a long time since the position of the platform module in the three-dimensional space is difficult to define and track, and the difference between the motion trajectory of the platform module and the expected trajectory is also difficult to quantify the record, so It is necessary to propose improved technical means to solve this problem.

The invention discloses a three-dimensional trajectory verification device and a method thereof.

First, the present invention discloses a three-dimensional trajectory verification device, which comprises: a platform module, a track dialing module, a signal transmitting module and a three-dimensional signal receiving module. The platform module is configured to receive a motion track; the track play module is configured to output a pre-recorded track; and the signal transmitting module is connected to the track play-out module for transmitting a position signal according to the pre-recorded track, wherein the signal transmitting module and the signal transmitting module The platform module maintains a fixed relative position; and the three-dimensional signal receiving module is fixed to the platform module for receiving the position signal transmitted by the signal transmitting module and outputting the sensing signal to the track dialing module. When the pre-recorded track is the same as the motion track, the track play module outputs a fixed signal according to the sensing signal; when the pre-recorded track is different from the motion track, the track play module outputs the deviation signal according to the sensing signal.

In addition, the present invention discloses a three-dimensional trajectory verification method, which comprises the steps of: receiving a motion trajectory by using a platform module; outputting a pre-recorded trajectory by using a trajectory play-out module; and transmitting a position signal according to the pre-recorded trajectory by using a signal transmitting module, wherein the signal is transmitted. The module and the platform module maintain a fixed relative position; the three-dimensional signal receiving module receives the position signal, and outputs the sensing signal to the track play-out module; when the pre-recorded track and the motion track are the same, the track play-out module is based on The sensing signal outputs a fixed signal; and when the pre-recording track is different from the motion track, the track playing module outputs the deviation signal according to the sensing signal.

The system and method disclosed in the present invention are different from the prior art in that the present invention maintains a fixed relative position through the signal transmitting module and the platform module, so that the motion track of the platform module and the pre-recording of the track play module are When the track is different, the track play module outputs a deviation signal according to the sensing signal.

Through the above technical means, the invention can achieve whether the actual motion trajectory of the automatic verification platform module is the same as the trajectory pre-recorded by the user, and record the technical effect of the error.

The embodiments of the present invention will be described in detail below with reference to the drawings and embodiments, so that the application of the technical means to solve the technical problems and achieve the technical effects can be fully understood and implemented.

Before describing the three-dimensional trajectory verification apparatus and the method thereof disclosed in the present invention, the present invention first determines whether the trajectories of the two objects are different, wherein the trajectory is a moving path of an object, and is also a set of a plurality of position points. When the present invention determines whether the trajectories of the two objects are different, whether the relative position between the positions of the two objects changes according to the same time is determined as a judgment basis, and if the relative position between the positions of the two objects changes at the same time, the judgment is made. The trajectories of the two objects are different; otherwise, the trajectories of the two objects are the same. In addition, the deviation signal according to the present invention includes the amount of change when the relative position between the positions of the two objects changes at the same time, and the amount of change may be an angle or a displacement.

Please refer to "1", "2", "3A" and "3B", "1" is a schematic diagram of an embodiment of a three-dimensional trajectory verification device of the present invention, "Fig. 2 The structure of the platform module, the track-and-play module and the signal-emitting module of the "Picture 1", and the "3A" is a schematic view of the three-dimensional signal receiving module of "Figure 1". Figure 3B is a schematic view of another perspective of the three-dimensional signal receiving module of Figure 3A. The three-dimensional trajectory verification device 100 includes a platform module 110, a trajectory play module 120, a signal transmitting module 130, and a three-dimensional signal receiving module 140.

The platform module 110 is configured to receive a motion track. In more detail, the platform module 110 can be combined with the motion platform (ie, whether the motion track to be verified conforms to the object expected by the user), and therefore, the platform module 110 is dependent on the motion platform, and the motion platform and the platform module 110 are trajected. In the same way, the motion platform can perform three-dimensional motion in three-dimensional space. When the motion platform performs Roll, Pitch or Yaw, the platform module 110 receives a motion track generated by the motion platform. The three-dimensional trajectory verification apparatus 100 can track and verify the motion trajectory of the motion platform.

The track play module 120 is configured to output a pre-recorded track, wherein the pre-recorded track is a pre-recorded track data recorded by the user in advance, and the pre-recorded track data may include Roll, Pitch, Yaw and time, so that the track is played. The module 120 can reproduce the trajectory that the user wants to run according to Roll, Pitch and Yaw at each time.

In this embodiment, the track play module 120 may include a processing unit (not drawn), a storage unit (not drawn), and an organization unit. The processing unit controls the mechanism unit to output a pre-record according to the pre-recorded track data stored in the storage unit. Track. The processing unit and the storage unit can be disposed in the accommodating space in the platform module 110. However, this embodiment is not intended to limit the present invention, and the position adjustment can be performed according to requirements. It should be noted that in order to avoid the complexity of the drawing, the configuration positions of the processing unit and the storage unit are not drawn in the drawing.

In more detail, the mechanism unit may include a first motor 50, a second motor 52, a third motor 54, a first rod 40 and a second rod 42, the processing unit is connected to the storage unit, the first motor 50, the second motor 52 and the third motor 54, the storage unit is configured to store the pre-recorded track data. One end of the first rod 40 is connected to the platform module 110, the other end is connected to the second rod 42 , and the other end of the second rod 42 is connected to the signal transmitting module 130 . The first motor 50 is disposed at a joint between the first rod 40 and the platform module 110 and the first motor 50 is located between the first rod 40 and the platform module 110 for controlling the operation of the Yaw; the second motor 52 is configured The second rod 52 is disposed at the junction of the second rod 42 and the signal emitting module 130, and the second motor 52 and the signal emitting module 130 are located on opposite sides of the second rod 42 for controlling the operation of the Pitch; The junction of the second rod 42 and the first rod 40 and the third motor 54 and the second rod 42 are located on opposite sides of the first rod 40 for controlling the operation of the Roll. As can be seen from the above, the track play module 120 can control the first rod 40 and the second by using the first motor 50, the second motor 52 and the third motor 54 according to the pre-recorded track data stored by the storage unit. The rod 42 further achieves the purpose of outputting a pre-recorded track. In addition, the above connection manner can make the motion of the track play module 120 not affected by the motion of the motion platform (ie, the track play module 120 and the motion platform operate independently of each other).

The signal transmitting module 130 is connected to the track play module 120 and maintains a fixed relative position with the platform module 110 for transmitting the position signal according to the pre-recorded track. In more detail, the signal transmitting module 130 is connected to the track play module 120 and continuously sends a position signal in a fixed direction. When the track play module 120 outputs the pre-recorded track, the signal transmitting module 130 is in a three-dimensional space. The position of the signal transmitting module 130 can be changed in the three-dimensional space by the position change of the position signal detected by the three-dimensional signal receiving module 140. The position signal sent by the signal transmitting module 130 can be, but is not limited to, laser light. For example, the position signal emitted by the signal transmitting module 130 can also be infrared or ultrasonic.

The three-dimensional signal receiving module 140 is fixed to the platform module 110 for receiving the position signal transmitted by the signal transmitting module 130 and outputting the sensing signal to the track playing module 120. Since the three-dimensional signal receiving module 140 is fixed to the platform module 110, the platform module 110 is dependent on the three-dimensional signal receiving module 140, and both have the same motion track. In this embodiment, the three-dimensional signal receiving module 140 can include a plurality of two-dimensional sensing units 20 and a five-sided housing 22, and the two-dimensional sensing units 20 are arranged in an array on the five-sided housing 22. The inner peripheral wall 221 and the inner peripheral wall 221 cover the signal transmitting module 130 to receive the position signal emitted by the signal transmitting module 130 when the signal transmitting module 130 rotates according to the pre-recorded track, thereby determining the position change of the signal transmitting module 130 in the three-dimensional space. Each of the two-dimensional sensing units 20 can be, but is not limited to, a laser light sensor, and can be adjusted according to a position signal sent by the actual signal transmitting module 130. For example, when the position signal emitted by the signal transmitting module 130 is infrared, each two-dimensional sensing unit 20 can be an infrared sensor; when the position signal sent by the signal transmitting module 130 is ultrasonic, each A two-dimensional sensing unit 20 can be an ultrasonic sensor. The three-dimensional signal receiving module 140 can output the sensing signal to the processing unit of the track play module 120 by using a wireless or wired manner.

When the pre-recorded trajectory is the same as the motion trajectory, the trajectory mode 120 outputs a fixed signal according to the sensing signal; when the pre-recorded trajectory is different from the motion trajectory, the trajectory modulo 120 group outputs the deviation signal according to the sensing signal. The deviation signal may include a change amount when the relative position between the pre-recorded track and the position of the motion track changes at the same time, and the change amount may be an angle or a displacement.

For example, please refer to "4A" and "4B". "4A" is a schematic diagram of an embodiment of a three-dimensional trajectory verification device when the pre-recorded trajectory is the same as the motion trajectory. "Picture 4B" is A schematic structural diagram of an embodiment of a three-dimensional trajectory verification device when the recorded trajectory and the motion trajectory are different. It should be noted that in order to avoid the complexity of the drawing, the three-dimensional signal receiving module 140 of "4A" and "4B" omits three sides of the five-sided housing.

It can be seen from the above paragraph that the track play module 120 and the motion platform are independently operated, that is, the track play module 120 and the platform module 110 are independently operated independently (the platform module 110 and the motion platform are dependent) ).

In addition, the signal transmitting module 130 and the platform module 110 maintain a fixed relative position. Therefore, the motion coordinates of the platform module 110 and the motion coordinates of the three-dimensional signal receiving module 140 have a fixed relative position, so that the three-dimensional signal receiving module 140 The two-dimensional sensing unit 20a continuously receives the position signal transmitted by the signal transmitting module 130.

While the platform module 110 receives the motion track of the motion platform, the track play module 120 also outputs the pre-recorded track, so that the motion track received by the platform module 110 is the same as the pre-recorded track output by the track play module 120. The two-dimensional sensing unit 20a will continue to receive the position signal (because the signal transmitting module 130 and the platform module 110 maintain a fixed relative position); please refer to "FIG. 4B", when the platform module 110 receives the motion When the trajectory is different from the pre-recorded trajectory, the two-dimensional sensing unit 20b receives the position signal (ie, the signal transmitting module 130 and the platform module 110 do not maintain a fixed relative position, so that different two-dimensional sensing units receive The processing unit of the track play module 120 can receive the relative distance relationship between the two-dimensional sensing unit 20a and the two-dimensional sensing unit 20b and the two-dimensional sensing unit 20a and the two-dimensional sensing unit 20b. The time difference of the position signal calculates the error between the pre-recorded trajectory and the motion trajectory, and outputs a deviation signal. In more detail, please refer to "4B", assuming that the pre-recorded trajectory has an upper elevation angle at time t. _Error , but the platform module 110 does not synchronize the elevation motion (ie, the motion track received by the platform module 110 is different from the pre-recorded track), and the two-dimensional sensing unit 20b receives the position signal. Since the absolute positions of the two-dimensional sensing unit arrays of the three-dimensional signal receiving module 140 are known, the relative distance between the two-dimensional sensing unit 20a and the two-dimensional sensing unit 20b and the two-dimensional sense can be transmitted. The time difference T between the measuring unit 20a and the two-dimensional sensing unit 20b receives the position signal to calculate the position of the current trajectory deviation. In this embodiment, the platform module 110 has an error Z _Error . Where L is the distance between the coordinate origin of the track play module 120 and the two-dimensional sensing unit 20b. However, the above examples are not intended to limit the invention.

In other words, when the motion track received by the platform module 110 is different from the pre-recorded track, the processing unit can transmit the two-dimensional sensing unit that currently receives the position signal and the two-dimensional sensing unit that previously fixed the continuous receiving position signal. The deviation angle between the angle of the deviation and the distance between the two-dimensional sensing unit that receives the position signal 10 and the coordinate origin of the track play module 120 is calculated to obtain a corresponding offset error.

Through the above mechanism, the processing unit can calculate the three-dimensional offset displacement error and the angular error between the platform module 110 and the preset trajectory at each time point. At the same time, it can be inferred that the verification accuracy of the three-dimensional trajectory verification device 100 is positively correlated with the array density of the two-dimensional sensing unit of the three-dimensional signal receiving module 140, and the density of the array of the two-dimensional sensing unit is denser. The verification accuracy of the three-dimensional trajectory verification device 100 will be higher.

Next, please refer to "figure 5", "figure 5" is a flowchart of a method for verifying a three-dimensional trajectory of the three-dimensional trajectory verification device of FIG. 1, the steps of which include: receiving a motion trajectory by using the platform module (step 210); Using the track play module to output a pre-recorded track (step 220); using the signal transmitting module to transmit a position signal according to the pre-recorded track, wherein the signal transmitting module and the platform module maintain a fixed relative position (step 230); using the three-dimensional signal Receiving the module to receive the position signal, and outputting the sensing signal to the track play module (step 240); when the pre-recorded track is the same as the motion track, the track play module outputs a fixed signal according to the sensing signal (step 250); And when the pre-recorded track is different from the motion track, the track play module outputs the deviation signal according to the sensing signal (step 260). Step 210 is performed synchronously with step 220, and other steps have a difference in execution order according to the causal relationship.

For details, please refer to "Figure 6" and "Figure 6" is a detailed flow chart of step 220 of "5th figure". The step of outputting the pre-recorded track by using the track play module (ie, step 220) further includes: using the pre-recorded track data stored by the storage unit (step 310); and using the processing unit to control the mechanism unit according to the pre-recorded track data. The pre-recorded track is output (step 320). The pre-recorded trajectory data includes rollover, pitch, yaw and time, so that the track play-out module outputs the pre-recorded track according to the roll, pitch and yaw control mechanism units of each time.

In summary, it can be seen that the difference between the present invention and the prior art is that the fixed relative position is maintained by the signal transmitting module and the platform module, so that the motion track of the platform module is different from the pre-recorded track of the track play module. The track play module outputs the deviation signal according to the sensing signal, and the technical problem can solve the problems of the prior art.

While the present invention has been described above in the foregoing embodiments, it is not intended to limit the invention, and the invention may be modified and modified without departing from the spirit and scope of the invention. The scope of patent protection shall be subject to the definition of the scope of the patent application attached to this specification.

20, 20a, 20b‧‧‧ two-dimensional sensing unit

22‧‧‧Five-sided shell

40‧‧‧First member

42‧‧‧Second bars

50‧‧‧First motor

52‧‧‧second motor

54‧‧‧third motor

100‧‧‧3D trajectory verification device

110‧‧‧ platform module

120‧‧‧Track Playback Module

130‧‧‧Signal launch module

140‧‧‧3D signal receiving module

221‧‧‧ inner wall

_Error ‧‧‧Upper elevation

L‧‧‧ distance

Step 210‧‧‧ Receive motion trajectory using the platform module

Step 220‧‧‧ Use the track dial module to output the pre-recorded track

Step 230‧‧‧ Using the signal transmitting module to transmit the position signal according to the pre-recorded track, wherein the signal transmitting module and the platform module maintain a fixed relative position

Step 240‧‧‧ Use the 3D signal receiving module to receive the position signal and output the sensing signal to the track dialing module

Step 250‧‧‧ When the pre-recorded track is the same as the motion track, the track play module outputs a fixed signal according to the sensing signal

Step 260‧‧‧ When the pre-recorded track is different from the motion track, the track play module outputs the deviation signal according to the sensing signal

Step 310‧‧‧Use the pre-recorded track data stored in the storage unit

Step 320‧‧‧ Using the processing unit to control the mechanism unit according to the pre-recorded trajectory data to output the pre-recorded trajectory

FIG. 1 is a schematic structural view of an embodiment of a three-dimensional trajectory verification device according to the present invention. FIG. 2 is a schematic structural view of the platform module, the track dialing module and the signal transmitting module of FIG. 1 . FIG. 3A is a schematic perspective view of the three-dimensional signal receiving module of FIG. 1 . FIG. 3B is a schematic view showing another perspective structure of the three-dimensional signal receiving module of FIG. 1. 4A is a schematic structural view of an embodiment of a three-dimensional trajectory verification device when the pre-recorded trajectory is the same as the motion trajectory. FIG. 4B is a structural schematic diagram of an embodiment of a three-dimensional trajectory verification apparatus when the pre-recorded trajectory is different from the motion trajectory. Fig. 5 is a flow chart showing the method of the three-dimensional trajectory verification method of the three-dimensional trajectory verification device of Fig. 1. Figure 6 is a detailed flow chart of step 220 of Figure 5.

20‧‧‧Two-dimensional sensing unit

22‧‧‧Five-sided shell

40‧‧‧First member

42‧‧‧Second bars

50‧‧‧First motor

52‧‧‧second motor

54‧‧‧third motor

100‧‧‧3D trajectory verification device

110‧‧‧ platform module

120‧‧‧Track Playback Module

130‧‧‧Signal launch module

140‧‧‧3D signal receiving module

Claims (12)

A three-dimensional track verification device includes: a platform module for receiving a motion track; a track play module for outputting a pre-recorded track; and a signal transmitting module connected to the track play module; And transmitting a position signal according to the pre-recorded track, wherein the signal transmitting module maintains a fixed relative position with the platform module; and a three-dimensional signal receiving module is fixed on the platform module for receiving the signal The position signal emitted by the transmitting module is outputted to the track play module; wherein when the pre-recorded track is the same as the motion track, the track play module outputs according to the sense signal a fixed signal; when the pre-recorded track is different from the motion track, the track play module outputs a deviation signal according to the sensing signal; wherein the three-dimensional signal receiving module comprises a plurality of two-dimensional sensing units and a A five-sided housing, the two-dimensional sensing units are arranged in an array on the inner peripheral wall of the five-sided housing. The three-dimensional trajectory verification device according to claim 1, wherein the trajectory play-out module comprises a processing unit, a storage unit and a mechanism unit, and the processing unit controls the trajectory data according to a pre-recorded trajectory stored by the storage unit. The mechanism unit outputs the pre-recorded track. The three-dimensional trajectory verification device according to claim 2, wherein the mechanism unit comprises a first motor, a second motor, a third motor, a first rod and a second a rod member, one end of the first rod member is connected to the platform module, the other end is connected to one end of the second rod member, and the other end of the second rod member is connected to the signal transmitting module, and the first motor is disposed in the first rod a joint between the rod and the platform module, the first motor is located between the first rod and the platform module, and the second motor is disposed at a joint of the second rod and the signal emitting module And the second motor and the signal emitting module are located on opposite sides of the second rod, the third motor is disposed at a junction of the second rod and the first rod, and the third motor and the third The two lever members are located on opposite sides of the first lever member, and the track dialing module controls the first by using the first motor, the second motor and the third motor according to the pre-recorded track data by the processing unit The rod and the second rod further output the pre-recorded track. According to the third-party trajectory verification device of claim 2, wherein the pre-recorded trajectory data includes roll, pitch, yaw and time, so that the track play-out module reproduces the trajectory according to the roll, pitch and yaw of each time. . According to the three-dimensional trajectory verification device of claim 1, wherein the platform module is dependent on the three-dimensional signal receiving module. The three-dimensional trajectory verification device according to claim 1, wherein the position signal emitted by the signal transmitting module is laser light, infrared light or ultrasonic wave. A method for verifying a three-dimensional trajectory, comprising: receiving a motion track by using a platform module; outputting a pre-recorded track by using a track play module; and transmitting a position signal according to the pre-recorded track by using a signal transmitting module, wherein The signal transmitting module maintains a fixed relative position with the platform module; Receiving the position signal by using a three-dimensional signal receiving module, and outputting a sensing signal to the track setting module, wherein the three-dimensional signal receiving module comprises a plurality of two-dimensional sensing units and a five-sided housing, The two-dimensional sensing unit is arranged in an array on the inner peripheral wall of the five-sided housing; when the pre-recorded track is the same as the motion track, the track-playing module outputs a fixed signal according to the sensing signal; When the pre-recorded track is different from the motion track, the track play module outputs a deviation signal according to the sensing signal. The method for verifying a three-dimensional trajectory according to the seventh aspect of the patent application, wherein the trajectory play-out module comprises a processing unit, a storage unit and a mechanism unit, and in the step of outputting the pre-recorded trajectory by using the trajectory play-out module, The method further includes: using a pre-recorded track data stored by the storage unit; and using the processing unit to control the mechanism unit according to the pre-recorded track data to output the pre-recorded track. The three-dimensional trajectory verification method according to claim 8 , wherein the mechanism unit comprises a first motor, a second motor, a third motor, a first rod and a second rod, the first rod One end of the platform is connected to the platform module, and the other end is connected to one end of the second rod. The other end of the second rod is connected to the signal transmitting module. The first motor is disposed on the first rod and the platform module. And the first motor is located between the first rod and the platform module, the second motor is disposed at a junction of the second rod and the signal emitting module, and the second motor and the signal The emitting module is located on opposite sides of the second rod, and the third motor is disposed at a joint of the second rod and the first rod and The third motor and the second rod are located on opposite sides of the first rod, and the track dialing module controls the first motor, the second motor, the third motor, the first rod and the The second lever outputs the pre-recorded track. According to the third-party trajectory verification method of claim 8, wherein the pre-recorded trajectory data includes roll, pitch, yaw and time, so that the track play-out module controls the mechanism according to the roll, pitch and yaw of each time. Unit to output the pre-recorded track. The three-dimensional trajectory verification method according to claim 7 of the patent application scope, wherein the platform module is dependent on the three-dimensional signal receiving module. According to the method of claim 7, the three-dimensional trajectory verification method, wherein the position signal emitted by the signal transmitting module is laser light, infrared light or ultrasonic wave.