TWI708130B - Path tracing system - Google Patents

Abstract

A path tracing system is disclosed in the present invention. The path tracing system includes a positioning module, a path segmentation module, a position vector operation module, a path vector operation module, a path reconstruction module, a tracing vector operation module, and a controlling module. The positioning module is utilized to position a current position. The path segmentation module is utilized to divide a path into a plurality of segments by a plurality of path points. The position vector operation module is utilized to operate a plurality of position vectors. The path vector operation module is utilized to operate a plurality of path vectors. The path reconstruction module is utilized to delete at least one path points by position vectors and path vectors, and reconstruct a remaining path. The controlling module is utilized to control a mobile platform to trace back to the remaining path with moving a shortest distance.

Description

Path tracking system

The present invention relates to a system, in particular to a path tracking system.

With the development of science and technology, mobile platform technologies such as automated guided vehicles and robots have gradually been applied in various fields. Generally speaking, mobile platforms will follow a planned path to reach a destination. When the mobile platform leaves the planned path, most of them will first rotate and reposition, and recalculate and plan a new planned path according to the repositioned position to reach the above destination. However, recalculating the planned path will occupy the computing resources of the mobile platform, and the mobile platform may have other commands to be executed on the original planned path.

In view of the fact that in the prior art, the mobile platform recalculates and plans a new planning path, which will occupy computing resources and may make it impossible to execute the instructions to be executed on the original planning path. One of the main objectives of the present invention is to provide a path tracking system to solve at least one of the problems in the prior art.

In order to solve the problems of the prior art, the necessary technical means adopted by the present invention is to provide a path trajectory tracking system, which is set on a mobile platform and includes a positioning module, a path segmentation module, a position vector operation module, A path vector operation module, a path reconstruction module, a trajectory tracking vector operation module and a control module.

The positioning module is used to position the mobile platform to obtain a current coordinate. The path segmentation module is used to divide a planned path that the mobile platform should follow into a plurality of road sections by using a plurality of path points, and each path point has a path point coordinate. The position vector calculation module is electrically connected to the positioning module and the path segmentation module to receive the current coordinates and the path point coordinates of the path point, and use the current coordinates to calculate a path point coordinate of each path point. The position vector is used to calculate a plurality of the above-mentioned position vectors. The path vector calculation module is electrically connected to the path segmentation module, and is used to define a path vector corresponding to the position vector by using one of the path points and the next neighbor.

The path reconstruction module is electrically connected to the position vector operation module and the path vector operation module, so as to calculate a vector included angle by using the position vector and the path vector corresponding to each other, and the vector included angle is greater than 270 degrees and less than 90 degrees. The corresponding path point is deleted to reconstruct the remaining part of the planned path into a residual path. The trajectory tracking vector calculation module is electrically connected to the path reconstruction module and the positioning module to calculate the shortest distance between the current coordinates and the residual path, and thereby generate a trajectory tracking vector. The control module is electrically connected to the trajectory tracking vector calculation module to generate a control command according to the trajectory tracking vector, and use the control command to control the mobile platform to move along the trajectory tracking vector and trace to the residual path.

On the basis of the above-mentioned necessary technical means, a subsidiary technical means derived from the present invention is to make the path tracing system further include a path generation module, and the path generation module is electrically connected to the path segmentation module for Generate a planned path.

On the basis of the above-mentioned necessary technical means, an auxiliary technical means derived from the present invention is to make the position vector operation module in the path tracking system include a position vector operation unit, and the position vector operation unit reduces the current coordinates respectively Go to the path point coordinates of each path point to calculate the position vector.

On the basis of the above-mentioned necessary technical means, one of the subsidiary technical means derived from the present invention is to make the path vector operation module in the path trajectory tracking system include a path vector operation unit, and the path vector operation unit integrates the above path points Next, a neighbor subtracts that one of the path points to define the path vector.

On the basis of the above-mentioned necessary technical means, an auxiliary technical means derived from the present invention is to make the path reconstruction module in the path trajectory tracking system, including a length calculation unit, a cosine calculation unit and a judgment unit. The length calculation unit is used to calculate the length of each position vector and each path vector. The cosine calculation unit is electrically connected to the length calculation unit, and uses the corresponding one of the position vector and the path vector and the length thereof to calculate a cosine value of the vector included angle. The judging unit is electrically connected to the cosine operation unit to receive the cosine value, and when it is confirmed that the cosine value is greater than 0, it is determined that the vector angle is greater than 270 degrees or less than 90 degrees.

On the basis of the above-mentioned necessary technical means, an auxiliary technical means derived from the present invention is to make the trajectory tracking vector calculation module in the path tracking system include a vector calculation unit and a distance calculation unit. The vector calculation unit uses the position vector and the path vector corresponding to the last one of the deleted path points to calculate an area value. The distance calculation unit is electrically connected to the vector calculation unit, and uses the area value and the length of the path vector to calculate the shortest distance.

On the basis of the above-mentioned necessary technical means, an auxiliary technical means derived from the present invention is to enable the trajectory tracking vector calculation module in the path trajectory tracking system to further include a direction determination unit and a vector generation unit. The direction judging unit judges a tracking direction from the target position to the residual path. The vector generation unit uses the shortest distance and the tracking direction to generate the trajectory tracking vector.

As mentioned above, the path trajectory tracking system provided by the present invention utilizes positioning module, path segmentation module, position vector operation module and path vector operation module to calculate position vector and path vector, and uses path remodeling The group deletes part of the path points and reconstructs the remaining part of the planned path into a residual path, and then controls the mobile platform to trace to the residual path in the shortest distance. Therefore, the present invention can eliminate the need for the mobile platform to relocate and recalculate the new planned path, but directly delete part of the path points in the old planned path, and trace to the residual path with the shortest distance, so that the mobile platform does not need to be repositioned The new planning path is recalculated and the instructions to be executed on the old planning path can also be executed.

The specific embodiments of the present invention will be described in more detail below with reference to the schematic diagram. According to the following description and the scope of patent application, the advantages and features of the present invention will be more clear. It should be noted that the drawings all adopt very simplified forms and all use imprecise proportions, which are only used to conveniently and clearly assist in explaining the purpose of the embodiments of the present invention.

Please refer to the first and second figures together, in which, the first figure is a block diagram showing the path tracking system provided by the preferred embodiment of the present invention; and the second figure is the block diagram showing the preferred embodiment of the present invention The three-dimensional schematic diagram of the path tracking system provided on the mobile platform. As shown in the figure, a path tracking system 1 is installed on a mobile platform 2. The mobile platform 2 is located in a mobile space S. In this embodiment, the mobile platform 2 draws a robot schematic, but it is not limited to this, and can also be an automatic guided vehicle or a mobile vehicle.

The path tracking system 1 includes a positioning module 11, a path segmentation module 12, a position vector calculation module 13, a path vector calculation module 14, a path reconstruction module 15, and a path tracking vector calculation module 16. A control module 17 and a path generation module 18. Among them, the position vector operation module 13 includes a position vector operation unit 131; the path vector operation module 14 includes a path vector operation unit 141; the path reconstruction module 15 includes a length operation unit 151, a cosine operation unit 152 and a judgment Unit 153; the trajectory tracking vector operation module 16 includes a vector operation unit 161, a distance operation unit 162, a direction determination unit 163 and a vector generation unit 164.

Next, please refer to Figures 1 to 7 together. Figure 3 is a schematic diagram showing the path segmentation module provided by the preferred embodiment of the present invention for dividing planned paths; Figure 4 shows the preferred embodiment of the present invention. The position vector calculation module, the path vector calculation module, and the path reconstruction module provided by the embodiment are schematic diagrams of the calculation vector and the angle between the vector; the fifth figure shows the path reconstruction module provided by the preferred embodiment of the present invention to reconstruct the residual path The sixth diagram is a schematic diagram showing the trajectory tracking vector generated by the trajectory tracking vector calculation module provided by the preferred embodiment of the present invention; and the seventh diagram shows the control module control provided by the preferred embodiment of the present invention Schematic diagram of mobile platform tracking. As shown in the figure, the mobile space S where the mobile platform 2 is located corresponds to a map M.

The mobile platform 2 should move along a planned path P, but the mobile platform 2 is not currently on the planned path P. In addition, it should be noted that drawing the planned path P on the actual moving space S is only to clearly illustrate the technical content of the present invention. The planned path P does not actually exist in the actual moving space S, but in the virtual map M.

The positioning module 11 will locate the mobile platform 2 in the mobile space S, and obtain a corresponding current coordinate LC on the map M. The positioning module 11 may be a device or device with a positioning function, such as a GPS positioning device and a Bluetooth positioning device.

The path segmentation module 12 uses a plurality of path points (drawing five path points P1, P2, P3, P4, P5 here) to divide the planned path P into a plurality of road sections (drawing four road sections PP1, PP2, PP3 and PP4), as shown in the third figure. Among them, the way points P1, P2, P3, P4, P5 each have a path point coordinate, and the way points P1, P2, P3, P4, P5 are the mobile platform 2 should follow the order of movement, or the way point P1 can be regarded as Starting point, the way point P5 is regarded as the end point.

In this embodiment, the path segmentation module 12 uses the turning points and the beginning and the end of the planned path P to set path points P1, P2, P3, P4, and P5, but it is not limited thereto. The path segmentation module 12 can also use equal distances to set path points P1, P2, P3, P4, P5, or use equal time intervals to set path points P1, P2, P3, P4, P5, or use planning path P To set the path points P1, P2, P3, P4, P5. In practice, the coordinate information of the path points P1, P2, P3, P4, and P5 is the Cartesian coordinate system.

The position vector calculation module 13 is electrically connected to the positioning module 11 and the path segmentation module 12, and receives the current coordinates LC and the path point coordinates of the path points P1, P2, P3, P4, and P5. The position vector calculation unit 131 subtracts the path point coordinates of the path points P1, P2, P3, P4, and P5 from the current coordinates LC to form five position vectors VL1, VL2, VL3, VL4, and VL5, respectively.

The path vector calculation module 14 is electrically connected to the path segmentation module 12, and receives path point coordinates of the path points P1, P2, P3, P4, and P5. The path vector calculation unit 141 uses one of the path point coordinates of the path points P1, P2, P3, P4, and P5 to define a path vector corresponding to the position vector. The position vector calculation module 13 and the path vector calculation module 14 can be processors, arithmetic units, arithmetic chips, or other devices with arithmetic functions.

In more detail, the path vector calculation unit 141 subtracts the path point coordinates of the path point P1 from the path point coordinates of the path point P2 to form the path vector VR1. The path point coordinates of the path point P1 are one of the aforementioned path point coordinates, and the path point coordinates of the path point P2 are the next adjacent ones of the aforementioned path point coordinates. By analogy, the path vector calculation unit 141 will calculate a total of four path vectors VR1, VR2, VR3, VR4. Because there are no other path points behind the path point P5, only the above four path vectors will be calculated.

The path reconstruction module 15 is electrically connected to the position vector operation module 13 and the path vector operation module 14, and uses the corresponding one of the position vector and the path vector to calculate a vector angle. For example, the path reconstruction module 15 uses the position vector VL1 and the path vector VR1 to calculate the vector angle A1 formed by the position vector VL1 and the path vector VR1. Similarly, the path reconstruction module 15 also calculates the vector angles A2, A3, and A4. The actions of the position vector calculation module 13, the path vector calculation module 14, and the path reconstruction module 15 are as shown in the fourth figure.

Next, the path reconstruction module 15 will determine the vector angles A1, A2, A3, A4, and delete the path points corresponding to the vector angles A1, A2, A3, A4 greater than 270 degrees and less than 90 degrees, so as to delete the planned path P The remaining part is reconstructed into a residual path PR.

In this embodiment, the length calculation unit 151 calculates the length of each vector, including position vectors VL1, VL2, VL3, VL4, VL5 and path vectors VR1, VR2, VR3, VR4. The cosine calculation unit 152 is electrically connected to the length calculation unit 151, and uses the position vectors VL1, VL2, VL3, VL4, VL5, path vectors VR1, VR2, VR3, VR4 and their respective lengths to calculate the vector angles A1, A2 , A3, A4 cosine value. The cosine of the vector angle is the inner product of the two vectors forming the vector angle divided by the product of the length of the two vectors. Next, the judging unit 153 is electrically connected to the cosine calculation unit 152 to receive the cosine value, and when it is confirmed that the cosine value is greater than 0, it is determined that the vector angle is greater than 270 degrees and less than 90 degrees. In other embodiments of the present invention, the angle may also be directly used to determine whether the vector included angle is greater than 270 degrees and less than 90 degrees.

In this embodiment, there are vectored angles A1 and A2 that are greater than 270 degrees and less than 90 degrees. Therefore, the path reconstruction module 15 deletes the path points P1 and P2 corresponding to the vector angles A1 and A2, and reconstructs the remaining part of the planned path P into a residual path PR. The purpose of deleting the waypoints P1 and P2 is to prevent the mobile platform 2 from moving to the waypoints P1 and P2, that is, to prevent the mobile platform 2 from wasting time to go toward the starting point. At this time, the path point P3 can be temporarily regarded as the starting point of the residual path PR.

The trajectory tracking vector calculation module 16 is electrically connected to the path reconstruction module 15 and the positioning module 11, and calculates the shortest distance Hm between the current coordinate LC and the residual path PR to generate a trajectory tracking vector DT. In other words, although the path point P3 can be temporarily regarded as the starting point of the residual path PR, in order to enable the mobile platform 2 to track the residual path PR as soon as possible, the trajectory tracking vector calculation module 16 will calculate the current coordinate LC The shortest distance Hm from the residual path PR, so that the mobile platform 2 can track the residual path PR with the shortest distance Hm, that is, the shortest time.

In this embodiment, the vector operation unit 161 uses the position vector and the path vector corresponding to the last one of the deleted path points to perform operations, here are the position vector VL2 and the path vector VR2 corresponding to the path point P2. The vector calculation unit 161 calculates the outer product of the position vector VL2 and the path vector VR2 and adds the absolute value to calculate an area value. From a geometric point of view, it is the area value of the parallelogram enclosed by the position vector VL2 and the path vector VR2. Next, the distance calculation unit 162 divides the area value by the length of the path vector VR2 to calculate the shortest distance Hm, that is, the height of the parallelogram with the path vector VR2 as the base. The height and base of the parallelogram are perpendicular to each other, so the calculated height is the shortest distance Hm.

The direction determining unit 163 determines a tracking direction D from the current coordinate LC toward the path vector VR2. Finally, the vector generating unit 164 uses the tracking direction D and the shortest distance Hm to generate a trajectory tracking vector DT, as shown in the fifth and sixth figures.

The control module 17 is electrically connected to the trajectory tracking vector calculation module 16, and generates a control command according to the trajectory tracking vector DT. Then, control commands are used to control the mobile platform 2 to track the residual path PR of the planned path P along the trajectory tracking vector DT with the shortest distance Hm. In the seventh figure, a dashed box is drawn to indicate the mobile platform before movement, and it is marked as mobile platform 2'. The control module 17 may be a device or device with a control function, such as a controller or a microcontroller.

It should be noted that there is another shortest distance Hm1 between the current coordinate LC and the planned path P, that is, the distance between the current coordinate LC and the road section PP1. Among them, the value of the shortest distance Hm1 will be smaller than the shortest distance Hm. However, the present invention does not directly control the movement of the mobile platform 2 along the shortest distance Hm1.

The present invention deletes unnecessary path points P1 and P2 first, and then calculates the shortest distance Hm between the current coordinate LC and the residual path PR. First, it can prevent the mobile platform 2 from going back, and second, it can be determined that the mobile platform 2 traces to the residual path PR along the shortest distance Hm. It can be clearly seen from the fifth figure that if the mobile platform 2 is directly controlled to move along the shortest distance Hm1, part of the road section PP1 and part of the road section PP2 need to be traveled more.

In summary, the path trajectory tracking system provided by the present invention can delete unnecessary path points when the mobile platform departs from the planned path to prevent the mobile platform from walking toward the starting point, and reconstruct the residual path and make the mobile platform take the shortest distance Trace to the residual path. Compared with the prior art, the mobile platform needs to be rotated and repositioned, and a new planned path needs to be re-planned. The present invention does not need to be re-positioned or re-planned a new path. Instead, the path points in the planned path that are biased toward the starting point are deleted, and Tracing the shortest path to the residual path after deleting the path point can achieve the effect of tracing to the residual path faster. In addition, if there are other command actions to be executed on the original planned path, after the present invention controls the mobile platform to trace back to the residual path, the mobile platform can also continue to execute the aforementioned command actions to be executed.

Through the detailed description of the preferred embodiments above, it is hoped that the characteristics and spirit of the present invention can be described more clearly, and the scope of the present invention is not limited by the preferred embodiments disclosed above. On the contrary, its purpose is to cover various changes and equivalent arrangements within the scope of the patent application for the present invention.

1: Path tracking system

11: Positioning module

12: Path segmentation module

13: Position vector operation module

131: Position vector operation unit

14: Path vector operation module

141: Path Vector Operation Unit

15: Path reconstruction module

151: Length calculation unit

152: Cosine Operation Unit

153: Judgment Unit

16: trajectory tracking vector operation module

161: Vector operation unit

162: Distance calculation unit

163: Direction Judgment Unit

164: Vector generation unit

17: Control module

18: Path generation module

2, 2’: mobile platform

A1, A2, A3, A4: vector angle

D: Tracking direction

DT: Trajectory tracking vector

Hm, Hm1: shortest distance

LC: current coordinates

M: Map

P: planning path

PP1, PP2, PP3, PP4: road section

PR: Residual path

P1, P2, P3, P4, P5: waypoint

S: mobile space

VL1, VL2, VL3, VL4, VL5: position vector

VR1, VR2, VR3, VR4: path vector

The first figure shows a block diagram of the path tracking system provided by the preferred embodiment of the present invention; The second figure is a three-dimensional schematic diagram showing the path tracking system provided by the preferred embodiment of the present invention installed on a mobile platform; The third figure is a schematic diagram showing the division of planned paths by the path segmentation module provided by the preferred embodiment of the present invention; The fourth figure is a schematic diagram showing the position vector calculation module, the path vector calculation module, and the path reconstruction module provided by the preferred embodiment of the present invention; The fifth figure is a schematic diagram showing the reconstruction of the residual path by the path reconstruction module provided by the preferred embodiment of the present invention; The sixth figure is a schematic diagram showing the trajectory tracking vector generated by the trajectory tracking vector calculation module provided by the preferred embodiment of the present invention; and The seventh figure is a schematic diagram showing the control module provided by the preferred embodiment of the present invention to control the tracking of the mobile platform.

1: Path tracking system

11: Positioning module

12: Path segmentation module

13: Position vector operation module

131: Position vector operation unit

14: Path vector operation module

141: Path Vector Operation Unit

15: Path reconstruction module

151: Length calculation unit

152: Cosine Operation Unit

153: Judgment Unit

16: trajectory tracking vector operation module

161: Vector operation unit

162: Distance calculation unit

163: Direction Judgment Unit

164: Vector generation unit

17: Control module

18: Path generation module

Claims (7)

A path trajectory tracking system is set on a mobile platform and includes: a positioning module for positioning the mobile platform to obtain a current coordinate; a path segmentation module for moving the mobile platform using a plurality of path points A planned path that the platform should follow is divided into a plurality of road sections, and each of the path points has a path point coordinate; a position vector operation module is electrically connected to the positioning module and the path segmentation module, For receiving the current coordinates and the path point coordinates of the path points, and using the current coordinates to calculate a position vector with the path point coordinates of each of the path points, thereby calculating a plurality of the above-mentioned position vectors; The path vector calculation module is electrically connected to the path segmentation module to use one of the path points and the next neighbor to define a path vector corresponding to the position vector, thereby defining A plurality of the above-mentioned path vectors; a path reconstruction module that electrically connects the position vector operation module and the path vector operation module, so as to use each of the position vectors and the corresponding one of the path vectors Calculate a vector included angle to calculate a plurality of the above-mentioned vector included angles, delete the path points corresponding to the vector included angles greater than 270 degrees and less than 90 degrees, to reconstruct the remaining part of the planned path into a residual path; The tracking vector calculation module is electrically connected to the path reconstruction module and the positioning module to calculate the current coordinates and the residual path A shortest distance between them is used to generate a trajectory tracking vector; and a control module is electrically connected to the trajectory tracking vector calculation module, so as to generate a control command based on the trajectory tracking vector, and use the control command to control The mobile platform moves along the trajectory tracking vector and traces to the residual path. For example, the path trajectory tracking system described in claim 1 further includes a path generation module, and the path generation module is electrically connected to the path segmentation module to generate the planned path. For example, the path trajectory tracking system described in item 1 of the scope of patent application, wherein the position vector operation module includes a position vector operation unit, and the position vector operation unit subtracts each of the path points from the current coordinates. The coordinates of the path points are used to calculate the position vectors. The path trajectory tracking system described in claim 1, wherein the path vector operation module includes a path vector operation unit, and the path vector operation unit calculates the next neighbor among the path points Subtract the one of the path points to define the path vector. For the path trajectory tracking system described in claim 1, wherein the path reconstruction module includes: a length calculation unit for calculating the length of each of the position vectors and each of the path vectors; A cosine arithmetic unit is electrically connected to the length arithmetic unit, and calculates a cosine value of the angle between the position vectors and the path vectors and their lengths; and a judgment unit, The cosine arithmetic unit is connected linearly to receive the cosine value, and when it is confirmed that the cosine value is greater than 0, it is determined that the vector angle is greater than 270 degrees or less than 90 degrees. The path trajectory tracking system described in item 5 of the scope of patent application, wherein the trajectory tracking vector operation module includes: a vector operation unit that uses the position vector and path vector corresponding to the last of the deleted path point An area value is calculated; and a distance calculation unit is electrically connected to the vector calculation unit, and calculates the shortest distance by using the area value and the length of the path vector. For example, the path trajectory tracking system described in item 6 of the scope of patent application, wherein the trajectory tracking vector calculation module further includes: a direction determining unit that determines a tracking direction from the target position to the residual path; and a The vector generating unit uses the shortest distance and the tracking direction to generate the trajectory tracking vector.

Images