TWI701535B - Method and system for planning trajectory - Google Patents

Abstract

The disclosure is related to a method and a system for planning trajectory. A simulator and a computing device are provided to perform the following steps. The simulator is used to establish a virtual self-propelled device model and a virtual environment model according to a self-propelled device and an environment. An optimized trajectory is formed in the virtual environment model after the pose data in each unit of time is obtained using a simulator algorithm. A sensor data is generated by a sensor module of the virtual self-propelled device module moving on a path of the virtual environment model. The sensor data is imported to a Simultaneous Localization and Mapping algorithm for establishing a map corresponding to the path. The sensor data is imported to a Cartographer Localization algorithm for forming a reference trajectory. After determining whether or not the reference trajectory approaches the optimized trajectory, a set of parameters provided for the self-propelled device operating on the path can be obtained according to the reference trajectory that approaches the optimized trajectory.

Description

Trajectory planning method and system

This case is about a technology for trajectory planning of self-propelled devices, especially a trajectory planning method and system that uses a simulator to form an optimized trajectory with a simulator algorithm and based on this to modify the parameters of the mapping positioning algorithm.

In recent years, the artificial intelligence (AI) industry has developed, and related products have appeared on the market one after another. One of them is applied to the operation of service robots, including the use of service robots to perform patrols, guidance, and document delivery. One of the core technologies is indoor positioning and navigation. When performing indoor positioning and navigation, it is necessary to perform timely simulation positioning and mapping in advance, such as a simultaneous positioning and map construction (Simultaneous localization and mapping, SLAM) technology, but the effect of SLAM is good or bad Will affect the ability of the robot to run.

In the conventional technology, the SLAM mechanism mainly includes firstly using an external high-end camera to record the robot, and establishing an indoor positioning navigation map of the robot in a specific occasion.

When traditional real-time simulation synchronization positioning and map construction are used to make indoor positioning navigation maps, it is necessary to use multiple high-resolution cameras to record the real trajectory to obtain high-resolution images. Therefore, the related software and hardware equipment is quite expensive and may depend on The size of the venue requires more cameras. And, if you want to change the venue, you need to re-arrange all the cameras.

This case discloses a trajectory planning method and system suitable for a self-propelled device that walks in an environment along a path to form a moving trajectory. The trajectory planning system includes a simulator and a computing device, and executes the following trajectory planning The steps described in the method. The method includes establishing a virtual self-propelled device model and a virtual environment model according to the self-propelled device and the environment in a simulator, wherein the virtual self-propelled device model is provided with a sensing module identical to the self-propelled device; the simulator executes a simulator The algorithm obtains the posture information of each unit time, and constructs an optimized trajectory in the virtual environment model; obtains the sensor data file generated by the sensor module of the virtual self-propelled device model on the path of the virtual environment model movement; The device obtains the sensor data file and the optimized trajectory, and imports the sensor data file into the Synchronous Positioning and Mapping Algorithm (SLAM) to construct a map of the corresponding path, and imports the sensor data file into the mapping and positioning algorithm to obtain each The posture information of a unit of time forms a reference trajectory on the map, and the reference trajectory corresponds to the movement trajectory. The computing device compares the reference trajectory and the optimized trajectory to determine whether the reference trajectory is close to the optimized trajectory, and if it is determined that the reference trajectory is close to the optimized trajectory, the parameters in the mapping positioning algorithm are imported into the self-propelled Device. If it is judged that the reference trajectory is not close to the optimized trajectory, adjust the parameters in the mapping positioning algorithm, recalculate the new reference trajectory with the mapping positioning algorithm, and compare it with the optimized trajectory again, and then judge the new If the reference trajectory is close to the optimized trajectory, repeat the calculation until the reference trajectory approaching the optimized trajectory is obtained, and then import the parameters in the mapping positioning algorithm corresponding to the aforementioned reference trajectory into the self-propelled device, and let the self-propelled device follow This parameter runs on the path.

Further, the sensing data file includes a plurality of sensing data corresponding to time, position, and directionality, and these sensing data are based on the optical radar simulation unit and the inertia quantity of the sensing module in the virtual self-propelled device model. At least one of a measurement simulation unit and an odometer simulation unit.

Further, the method of comparing the reference trajectory with the optimized trajectory is realized by a difference distribution method. In this difference distribution method, the attitude information error value of the reference trajectory relative to the optimized trajectory in each unit time is calculated , Obtain multiple sets of attitude information error values in multiple unit times, and obtain the distribution ratio of the complex error range of attitude information error values.

Furthermore, the distribution ratio of the minimum error range of the error range is taken out, and can be compared with a preset distribution ratio to determine whether the reference trajectory is close to the optimized trajectory.

Further, in the trajectory planning method, the parameters in the mapping and positioning algorithm are adjusted based on a low delay principle.

The trajectory planning method and system proposed in this case is to build the same environment and self-propelled device in the simulator as in the real environment, and adjust the parameters in the simulated environment, including the first collection of the virtual self-propelled device model in the simulator in the virtual environment The sensing data file generated by the sensing module on the path of the model movement becomes the data of the trajectory planning, and the simulator's own simulator algorithm is used to obtain the optimized trajectory detection. The reference trajectory calculated by the mapping positioning algorithm is used. In order to judge whether the reference trajectory is close to the optimized trajectory, and a large amount of data can be obtained in a short time in the simulated environment, it can replace the cost of setting up high-end cameras and other equipment in the conventional technology, and can Avoiding multiple troubles of changing factors in the real environment can effectively converge the experimental results.

In order to further understand the technology, methods and effects of the present invention to achieve the established objectives, please refer to the following detailed descriptions and drawings about the present invention. I believe that the objectives, features and characteristics of the present invention can be thoroughly and concretely obtained. It is understood that, however, the accompanying drawings are only provided for reference and illustration, and are not intended to limit the present invention.

10‧‧‧Self-propelled device

101‧‧‧Optical radar

102‧‧‧Inertial measurement unit

103‧‧‧Odometer

20‧‧‧Simulator

21‧‧‧Sensing Module

201‧‧‧Optical radar simulation unit

202‧‧‧Inertia measurement simulation unit

203‧‧‧Odometer simulation unit

204‧‧‧Virtual Environment Model

205‧‧‧Virtual self-propelled device model

206‧‧‧Simulator algorithm

22‧‧‧Computer

221‧‧‧Memory Unit

222‧‧‧Processing unit

223‧‧‧Synchronous positioning and map construction algorithm

224‧‧‧Mapping positioning algorithm

40‧‧‧Optimized trajectory

401‧‧‧Reference track

t0,t1,t2,…tn‧‧‧Time

d0,d1,d2,…dn‧‧‧distance error

s0,s1,s2,sn‧‧‧optimized position

p0,p1,p2,pn‧‧‧reference position

Steps S301~S319‧‧‧Steps of the trajectory planning process

Steps S501~S509‧‧‧The steps of the difference distribution method flow

Figure 1 shows a schematic diagram of an embodiment of the trajectory planning system architecture; Figure 2 shows an embodiment diagram of the flow of the trajectory planning method; Figure 3 shows the error value of the attitude information of the optimized trajectory and the reference trajectory in each unit time in the trajectory planning method Schematic diagram of an embodiment; FIG. 4 shows an embodiment diagram of a process of executing the difference distribution method in the trajectory planning method.

This case proposes a trajectory planning method and system. The trajectory planning system is suitable for a self-propelled device 10 as shown in Fig. 1. The self-propelled device 10 walks along a path in an environment to form a moving trajectory. The trajectory planning system includes a simulator 20 And a computing device 22. The simulator 20 is used to establish a virtual self-propelled device model 205 and a virtual environment model 204 according to the actual self-propelled device 10 and the environment, wherein the virtual self-propelled device model 205 is provided with the same sensing module as the self-propelled device 10 21 to obtain a sensing data file generated by the sensing module 21 on a path where the virtual self-propelled device model 205 moves on the virtual environment model 204. The sensing data file includes plural sensing data corresponding to time, location, and directionality.

The simulator 20 can be a gazebo program. The sensing module of the self-propelled device 10 includes at least an optical radar (Light Detection and Ranging, LiDAR) 101, an inertial measurement unit (IMU) 102, and an odometer (Odometer). 103, and the sensing module 21 of the virtual self-propelled device model 205 corresponds to the sensing module of the self-propelled device 10. The sensing module 21 includes an optical radar simulation unit 201, an inertia measurement simulation unit 202, and an odometer simulation unit 203. The aforementioned multiple sensing data are generated based on at least one of an optical radar simulation unit 201, an inertia measurement simulation unit 202, and an odometer simulation unit 203 of the sensing module 21 in the virtual self-propelled device model 205 .

Among them, the optical radar 101 and the optical radar simulation unit 201 are sensors that use light to detect distances, and use pulsed light to scan the distance between the self-propelled device 10 or the virtual self-propelled device model 205 on a path. According to the received light wave data, three-dimensional information of the surrounding environment or virtual environment model 204 can be constructed, and the purpose of positioning the autonomous device 10 or the virtual autonomous device model 205 can be achieved.

The inertia measurement unit 102 and the inertia measurement simulation unit 202 are inertial navigators that can sense the posture of the self-propelled device 10 or the virtual self-propelled device model 205 to obtain posture information of the self-propelled device 10 or the virtual self-propelled device model 205 ( pose), where the recorded data are, for example, the angular velocity and acceleration of the autonomous device 10 or the virtual autonomous device model 205 in three axial directions.

The odometer 103 and the odometer simulation unit 203 can sense the cruise mileage of the self-propelled device or the virtual self-propelled device model 205. If it is applied to the wheel-driven self-propelled device 10 or the virtual self-propelled device model 205, the odometer 103 and The odometer simulation unit 203 calculates the mileage of the self-propelled device 10 or the virtual self-propelled device model 205 based on data such as the circumference and rotation speed of the wheel axle, and can also obtain direction data.

The simulator 20 is provided with a simulator algorithm 206 implemented by a computer program, such as an algorithm of the gazebo program itself. The simulator algorithm 206 obtains each of the virtual self-propelled device model 205 in the virtual environment model 204 The posture information per unit time is used to form an optimized trajectory, and the sensing data file generated by the sensing module 21 on a path where the virtual self-propelled device model 205 moves on the virtual environment model 204 is obtained. The sensing data file includes a plurality of sensing data corresponding to time, position, and directionality. According to the embodiment shown in FIG. 1, these sensing data are generated by the optical radar simulation unit 201 and the inertia measurement in the sensing module 21. It is generated by at least one of the simulation unit 202 and the odometer simulation unit 203.

After that, these sensing data files and optimized trajectories will be transmitted to the computing device 22. For example, the simulator can be transmitted through a wired or wireless network, or a direct connection (such as USB) can be used to optimize the sensing data files. The trajectory is transmitted to the computing device 22, where the memory unit 221 stores one or more sensor data files, and the processing unit 222 executes the subsequent trajectory planning method, including importing the sensor data files into a synchronous positioning and map construction algorithm (Simultaneous Localization and mapping (SLAM) 223 is used to construct a map corresponding to the path, and to import the sensed data file into a cartographer localization algorithm (Cartographer Localization) 224 to obtain the posture information of each unit of time, and form a reference trajectory on the map. The trajectory corresponds to the movement trajectory of the self-propelled device in the real environment.

The calculation device 22 compares the reference trajectory and the optimized trajectory to determine whether the reference trajectory is close to the optimized trajectory. If it is determined that the reference trajectory is close to the optimized trajectory, it will be close to the reference trajectory of the optimized trajectory. The mapping positioning algorithm parameters are imported into the real self-propelled device 10. If it is determined that the reference trajectory is not close to the optimized trajectory, the parameters in the mapping positioning algorithm are adjusted.

For the trajectory planning method running in the simulator 20 and the computing device 22, please refer to the flowchart of the embodiment shown in FIG. 2, and please refer to FIG. 1 together.

As shown in step S301 in FIG. 2, the simulator 20 first establishes a virtual self-propelled device model 205 and a virtual environment model 204 according to the self-propelled device and the environment to be run, wherein the virtual self-propelled device model 205 is provided with the self-propelled device 10 The same sensing module 21, in step S303, the simulator 20 obtains the posture information generated per unit time according to a simulator algorithm, and constructs an optimized trajectory in the virtual environment model 204, and obtains in step S305 The virtual self-propelled device model 205 senses the sensing data file generated by the module 21 on the moving path of the virtual environment model 204.

Then, in step S307, after the computing device 22 in the system obtains the sensing data file generated by the simulator 20, the processing unit 222 imports the sensing data file into the synchronous positioning and map construction algorithm 223 to construct a map corresponding to the above path .

In step S309, the processing unit 222 in the computing device 22 imports the sensing data file into the mapping positioning algorithm 224. In step S311, the posture information per unit time can be calculated and obtained. The posture information includes time, position, and directionality. And other information, and constitute a reference trajectory on the map constructed above, and the reference trajectory corresponds to the movement trajectory of the self-propelled device 10 on the path.

In step S313, the optimized trajectory constructed by the simulator is compared with the reference trajectory, and the purpose is to obtain the distribution characteristics of the two trajectories. Then, in step S315, it is determined whether the reference trajectory is close to the optimized trajectory. The method of judging is for example a difference distribution method.

At this time, the system can set a comparison threshold to determine whether the reference trajectory is approaching the optimized trajectory according to the error value between the reference trajectory and the optimized trajectory. In the above-mentioned difference distribution method, there is a preset The distribution ratio is used as the basis for detecting whether the reference trajectory approaches the optimized trajectory.

If it is determined that the reference trajectory has not reached the comparison threshold approaching the optimized trajectory, that is, the reference trajectory has not approached the optimized trajectory, in step S317, the parameters in the mapping positioning algorithm in step S309 are adjusted. Return to S309, import the aforementioned sensing data file and use the new reference trajectory calculated by the mapping positioning algorithm after adjusting the parameters (step S311), and compare the new reference trajectory and the optimized trajectory again until the reference trajectory reaches The comparison threshold for approaching the optimized trajectory, that is, the reference trajectory is approaching the optimized trajectory. It is worth mentioning that when adjusting the parameters of the mapping positioning algorithm, the parameters in the mapping positioning algorithm can be adjusted with a low-latency principle. In this embodiment, the parameters of the mapping positioning algorithm can be, for example, the weight of the sensing data output by the optical radar simulation unit 201, the inertia measurement simulation unit 202, or the odometer simulation unit 203, and the virtual self-propelled device model The friction coefficient of 205, or different noises in the virtual environment model 204 interfere with their corresponding weights, but not limited to the above situation.

If it is judged that the reference trajectory has approached the optimized trajectory, in step S319, the parameters in the mapping positioning algorithm suitable for the self-propelled device 10 are obtained, and the above-mentioned parameters can be imported into the self-propelled device 10 to make the self-propelled device 10 10 can obtain the movement trajectory corresponding to the aforementioned reference trajectory approaching the optimized trajectory when actually walking on the path according to the received parameters.

Therefore, with the trajectory planning method disclosed in this embodiment, the parameters in the mapping positioning algorithm can be adjusted repeatedly in the computing device 22 to determine whether the reference trajectory is approaching the optimized trajectory, and the parameters of the self-propelled device 10 are related to the mapping Therefore, the reference trajectory closest to the optimized trajectory and its corresponding parameters are imported into the self-propelled device 10, so that the self-propelled device 10 can obtain the corresponding parameters when actually walking on the path according to the received parameters. This is the movement trajectory of the reference trajectory which is close to the optimized trajectory.

At this time, the optimized trajectory obtained above can be drawn on the same plane coordinates with multiple reference trajectories, as shown in Figure 3, the optimized trajectory 40 and the reference trajectory 401 on the same path, and the trajectory planning method is compared from them. The reference trajectory 401 and its parameters approaching the optimized trajectory 40 to a certain degree.

In the embodiment of the process shown in FIG. 2, the difference distribution method can be used as a comparison method for judging whether the reference trajectory is close to the optimized trajectory, and the comparison method can be referred to as shown in FIG. 3. It is a schematic diagram of an embodiment of optimizing the posture information error value of the trajectory and the reference trajectory in each unit time in the trajectory planning method, and the flowchart of the embodiment of the process of executing the difference distribution method in the trajectory planning method in FIG. 4.

In the two trajectories shown in Figure 3, there is an optimized trajectory 40 and a reference trajectory 401 drawn based on positioning information. In this difference distribution method, the reference trajectory relative to the optimized trajectory in each unit time is calculated. The error value of attitude information. This example shows that the reference position p0, p1, p2,...pn of the reference trajectory 401 at each time t0, t1, t2,...tn is compared to the same on the optimized trajectory 40 The optimized position of time s0, s1, s2,...sn, the distance error d0, d1, d2,...dn of each time can be obtained. According to step S501 in Fig. 4, the relative trajectory can be calculated in the same way. For the error value of the attitude information of the optimized trajectory per unit time, that is, the position and direction difference in each unit time, in step S503, when the system divides the error value of each attitude information into multiple error ranges, it can The error range of each attitude information error value is obtained. According to this, in step S505, the distribution ratio of the complex error range of the attitude information error value can be obtained. These are the two trajectories (ie, the reference trajectory and the optimized trajectory) The difference distribution.

In order to obtain the reference trajectory approaching the optimized trajectory, the system may be provided with a preset distribution ratio. The difference distribution method is to compare the distribution ratio of the error range formed between the reference trajectory obtained each time and the optimized trajectory. The preset distribution ratio, as in step S507, is used as a basis for detecting whether the reference trajectory has approached the optimized trajectory.

In step S509, the trajectory planning method running in the computing device 22 will determine whether the distribution ratio of the minimum error range in the aforementioned error range is greater than the preset distribution ratio, if not, that is, the current minimum error range distribution ratio is less than the preset distribution ratio. , Which means that the current reference trajectory has not yet approached the optimized trajectory, that is, return to the step of Figure 2, such as S317, adjust the parameters in the mapping positioning algorithm, and then in step S309, import the aforementioned sensing data file and adjust The mapping and positioning algorithm calculation after the parameters is used to obtain the attitude information of each unit time again to obtain a new reference trajectory (step S311), and then re-execute the steps in the difference distribution method process described in FIG. 4, including calculation Obtain the attitude information error value per unit time of the new reference trajectory relative to the optimized trajectory (step S501), obtain the error range of each attitude information error value (step S503), and obtain the complex number of the attitude information error value At least one of the distribution ratio of the error range (step S505) and the distribution ratio of the extracted error range are compared to the preset distribution ratio (step S507) to determine whether the new reference trajectory approaches the optimized trajectory (step S509).

If in the judgment of step S509, when the distribution ratio of the minimum error range is greater than the preset distribution ratio, it means that the reference trajectory has approached the optimized trajectory, that is, step S319 in Figure 2 shows that the mapping positioning suitable for the self-propelled device 10 The parameters in the algorithm, and the above parameters can be imported into the self-propelled device 10, so that the self-propelled device 10 can obtain the movement trajectory corresponding to the aforementioned reference trajectory approaching the optimized trajectory when the path moves according to the received parameters .

For example, a distance can be taken as an example to obtain the attitude information error value of each unit time, that is, to calculate the error distance between two point coordinate positions of two trajectories in the same unit time, a plurality of distance error values can be formed. Such as d1, d2, d3,...dn. The system can set multiple error ranges, such as D0 (1 m), D1 (0.1 m), D2 (0.05 m) and D3 (0.01 m), and calculate the position error of the two curves at each time point. How much is the ratio within 0.01 meter (D3), what is the ratio within 0.05 meter (D2), what is the ratio within 0.1 meter (D1), and how much is the error within 1 meter (D0) Ratio, these ratio values show the curve characteristics of the corresponding reference trajectory, from which the reference trajectory closest to the optimized trajectory can be evaluated.

In one embodiment, the system may only consider the minimum error range (D3 in this example) to compare the preset distribution ratio as a basis for judging whether the reference trajectory is close to the optimized trajectory. If the distribution ratio of the minimum error range is less than the preset distribution ratio, it means that you need to continue to adjust the parameters in the mapping and positioning algorithm, and then import the sensing data file into the mapping and positioning algorithm with the adjusted parameters to recalculate to obtain the per unit time Attitude information, obtain a new reference trajectory, and re-judge whether the new reference trajectory is close to the optimized trajectory; if the distribution ratio in the new minimum error range is greater than the preset distribution ratio, it means that the new reference trajectory has approached the most To optimize the trajectory, the parameters in the mapping positioning algorithm with the adjusted parameters are imported into the self-propelled device 10.

Repeating the above process in this way, the trajectory planning method can effectively reduce the error between the reference trajectory and the optimized trajectory, which helps to improve the positioning and navigation of the self-propelled device.

To sum up, in the real world, it is very difficult to adjust and verify the algorithm parameters during positioning and mapping. It also requires the purchase of expensive equipment to obtain an optimized trajectory. The embodiment of the trajectory planning method and system disclosed above is to create a virtual model based on the self-propelled device and the environment in the simulator, and the parameters can be adjusted in the simulated environment, including the construction of the optimized trajectory and the inclusion in the simulator The sensed data becomes the data for trajectory planning. Based on this, the map can be constructed and the optimized trajectory and reference trajectory can be obtained, and then it can be judged whether the reference trajectory is close to the optimized trajectory. The self-propelled device 10 can quickly obtain the parameter adjustment direction in the computing device 22. In this way, the method and system for trajectory planning using the simulator can obtain a large amount of data in a short time, can replace the cost of setting up the environment in the conventional technology, and can avoid the multiple problems of changing factors in the real environment. Can effectively converge the experimental results.

The above are only preferred and feasible embodiments of the present invention, and all equivalent changes and modifications made in accordance with the scope of the patent application of the present invention shall fall within the scope of the present invention.

20‧‧‧Simulator

21‧‧‧Sensing Module

201‧‧‧Optical radar simulation unit

202‧‧‧Inertia measurement simulation unit

203‧‧‧Odometer simulation unit

204‧‧‧Virtual Environment Model

205‧‧‧Virtual self-propelled device model

206‧‧‧Simulator algorithm

22‧‧‧Computer

221‧‧‧Memory Unit

222‧‧‧Processing unit

224‧‧‧Mapping positioning algorithm

223‧‧‧Synchronous positioning and map construction algorithm

10‧‧‧Self-propelled device

101‧‧‧Optical radar

102‧‧‧Inertial measurement unit

103‧‧‧Odometer

Claims (16)

A trajectory planning method is suitable for a self-propelled device, the self-propelled device walks on an environment along a path to form a moving trajectory. The trajectory planning method includes: in a simulator, a virtual device is established based on the self-propelled device and the environment. A self-propelled device model and a virtual environment model, wherein the virtual self-propelled device model is provided with a sensing module that is the same as the self-propelled device; the simulator obtains attitude information per unit time according to a simulator algorithm , And construct an optimized trajectory in the virtual environment model; obtain a sensing data file generated by the sensing module on the path where the virtual self-propelled device model moves on the virtual environment model; and the sensing data file Import a synchronous positioning and map construction algorithm to construct a map corresponding to the path; import the sensing data file into a mapping positioning algorithm to obtain attitude information per unit time, and construct a reference trajectory on the map, The reference trajectory corresponds to the movement trajectory; the reference trajectory is compared with the optimized trajectory to determine whether the reference trajectory is close to the optimized trajectory; and if not, the parameters in the mapping positioning algorithm are adjusted ; If yes, import the parameters in the mapping positioning algorithm into the self-propelled device. The trajectory planning method according to claim 1, wherein the sensing data file includes a plurality of sensing data corresponding to time, location, and directionality. The trajectory planning method according to claim 2, wherein the sensing data are simulated based on an optical radar simulation unit, an inertia measurement simulation unit and an odometer of the sensor module in the virtual self-propelled device model Produced by at least one of the units. The trajectory planning method according to claim 1, wherein a difference distribution method is used to compare the reference trajectory and the optimized trajectory. The trajectory planning method according to claim 4, wherein, in the difference distribution method, an attitude information error value per unit time of the reference trajectory relative to the optimized trajectory is calculated to obtain the attitude information error value The distribution ratio of the complex error range. The trajectory planning method according to claim 5, wherein the distribution ratio of a minimum error range of the error range is taken out and compared with a preset distribution ratio to determine whether the reference trajectory is close to the optimized trajectory. The trajectory planning method according to claim 6, wherein if the distribution ratio of the minimum error range is less than the preset distribution ratio, the parameters in the mapping positioning algorithm are adjusted, and the sensing data file is imported into the adjusted parameters The mapping and positioning algorithm used to obtain the attitude information of each unit time again to obtain a new reference trajectory. After comparing the new reference trajectory and the optimized trajectory, a new minimum error range is obtained. Distribution ratio; when the distribution ratio in the new minimum error range is greater than the preset distribution ratio, the parameters in the mapping positioning algorithm with adjusted parameters are imported into the self-propelled device. The trajectory planning method according to claim 7, wherein the parameters in the mapping and positioning algorithm are adjusted according to a principle of low delay. A trajectory planning system is suitable for a self-propelled device. The self-propelled device walks along a path in an environment to form a moving trajectory. The trajectory planning system includes: a simulator, which establishes a virtual system based on the self-propelled device and the environment. The self-propelled device model and a virtual environment model obtain posture information of each unit time according to a simulator algorithm, and form an optimized trajectory in the virtual environment model, wherein the virtual self-propelled device model is set with the self-propelled A sensing module with the same device is used to obtain a sensing data file generated by the sensing module on the path along which the virtual self-propelled device model moves on the virtual environment model; and a computing device to obtain the sensing For the data file and the optimized trajectory, perform the following steps: import the sensing data file into a synchronous positioning and map construction algorithm to construct a map corresponding to the path; import the sensing data file into a mapping positioning algorithm , Obtain the attitude information of each unit time, and construct a reference trajectory on the map, the reference trajectory corresponding to the movement trajectory; compare the reference trajectory with the optimized trajectory to determine whether the reference trajectory approaches the Optimize the trajectory; and if not, adjust the parameters in the mapping positioning algorithm; if so, import the parameters in the mapping positioning algorithm into the self-propelled device. The trajectory planning system according to claim 9, wherein the sensing data file includes a plurality of sensing data corresponding to time, position and direction. The trajectory planning system according to claim 10, wherein the sensing data are simulated based on an optical radar simulation unit, an inertia measurement simulation unit, and an odometer of the sensor module in the virtual self-propelled device model Produced by at least one of the units. The trajectory planning system according to claim 9, wherein the computing device compares the reference trajectory and the optimized trajectory by a difference distribution method. The trajectory planning system according to claim 12, wherein, in the difference distribution method, an attitude information error value per unit time of the reference trajectory relative to the optimized trajectory is calculated to obtain the attitude information error value The distribution ratio of the complex error range. The trajectory planning system according to claim 13, wherein the distribution ratio of a minimum error range of the error range is taken out and compared with a preset distribution ratio to determine whether the reference trajectory is close to the optimized trajectory. The trajectory planning system according to claim 14, wherein if the distribution ratio of the minimum error range is less than the preset distribution ratio, the parameters in the mapping positioning algorithm are adjusted, and the sensing data file is imported into the adjusted parameters The mapping and positioning algorithm used to obtain the attitude information of each unit time again to obtain a new reference trajectory. After comparing the new reference trajectory and the optimized trajectory, a new minimum error range is obtained. Distribution ratio; when the distribution ratio in the new minimum error range is greater than the preset distribution ratio, the parameters in the mapping positioning algorithm with adjusted parameters are imported into the self-propelled device. The trajectory planning system according to claim 15, wherein the calculation device adjusts the parameters in the mapping and positioning algorithm according to a low-latency principle.

Images