Moving body moving method and device based on path error compensation

Abstract

The invention relates to a moving body moving method and device based on path error compensation, and the method comprises the steps: obtaining set positions in a planned path of a moving body, sequentially issuing the set positions to the moving body, enabling the moving body to move according to the set positions, and describing the planned path as an ordered point set, a connecting line of every two continuous set position points in the ordered point set forms a path section in the planned path; acquiring an actual position to which the moving body moves based on the current set position in real time; determining the path section of the actual position in the planned path, and calculating the deviation degree between the actual position and the path section as an offset error; and correcting the next set position to be issued in the planned path based on the offset error, and issuing the corrected next set position to the moving body, so that the moving body moves based on the corrected next set position. The position of the moving body on the static planning path can be dynamically adjusted, so that errors in actual operation are reduced, and the moving precision and the anti-interference capability of the system are improved.

Description

Technical Field

[0001] The present application relates to the field of motion control technology, and in particular to a method and device for moving a moving body based on path error compensation, and a computing device. Background Art

[0002] In the fields of modern industrial automation, robotics, and autonomous driving, it is crucial to achieve high-precision trajectory tracking. In order to improve the accuracy of trajectory tracking and solve the problem that traditional control methods are difficult to handle nonlinear systems, adaptive dynamic programming (ADP) based on neural networks can be introduced to optimize path error compensation. This method is the same as the Actor-Critic architecture, with three main components working together: the Critic Network, the Action Network, and the Model Network. The Critic Network is used to evaluate the current strategy, that is, to estimate the value (Q value) of taking an action under a given state; the Action Network decides the action to be taken based on the information provided by the evaluation network, and is responsible for generating the best action plan for a given state. The Model Network is used to simulate the dynamic characteristics of the system and predict the system response under different control inputs. Such as Figure 1 As shown, the process of this method is as follows:

[0003] 1. Provide state parameters to the value function (i.e., the critic network of ADP) and the policy (i.e., the action network of ADP) through the environment (i.e., the model network of ADP);

[0004] 2. The policy (i.e., the action network of ADP) receives the current state and selects an action based on the current state.

[0005] 3. The environment receives the action, generates a new state and reward based on the action, and provides the new state and reward to the value function.

[0006] 4. The value function receives the new state (State) and historical rewards (Reward), calculates the value (Q value) of taking an action under the new state based on the new state and historical rewards, and calculates the TD error (Temporal Difference Error) based on the value (Q value) and outputs it. The TD error is used to evaluate the quality of the current strategy.

[0007] 5. The strategy receives TD Error to adjust its strategy parameters to optimize future action selection.

[0008] 6. The environment receives future actions, generates a new state and reward based on the future actions, and provides the next new state and reward to the value function to continue the next round of iteration.

[0009] Although ADP-based methods offer a high degree of flexibility, allowing for efficient learning and adaptation in complex environments, they also face challenges. First, the ADP algorithm involves a large amount of computation. For tasks with high real-time requirements, such as trajectory tracking control of driverless cars, the control system must be able to make decisions in a short period of time. If the learning process of ADP is too slow, it may not be able to meet this requirement. Furthermore, ADP can adapt to environmental changes through continuous learning, but for sudden disturbances, such as emergencies in traffic flow or the appearance of unexpected obstacles in the robot's operating environment, it may not be able to respond in time due to the slow learning process, thus affecting the system's anti-interference ability.

[0010] Therefore, how to provide a solution for dynamically adjusting the moving route that can meet real-time requirements, enhance anti-interference capabilities, and improve moving accuracy without requiring complex calculations is a technical problem that needs to be solved. Summary of the invention

[0011] In view of the above problems in the prior art, the present application provides a moving body movement method and apparatus, a computing device and a storage medium based on path error compensation to achieve real-time dynamic adjustment of the moving position of the moving body.

[0012] To achieve the above-mentioned object, the present application provides, in a first aspect, a moving object moving method based on path error compensation, comprising:

[0013] Acquire a set position in a planned path of the moving body, and send the set position to the moving body in sequence, so that the moving body moves according to the set position, wherein the planned path can be described as an ordered point set, and a line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path;

[0014] Real-time acquisition of the actual position to which the moving body moves based on the currently set position sent;

[0015] Determine the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where the actual position is located as an offset error;

[0016] The next set position to be issued in the planned path is corrected based on the offset error, and the corrected next set position is issued to the moving body, so that the moving body moves based on the corrected next set position.

[0017] From the above, the present application performs compensation processing by obtaining the actual position to which the moving body moves according to the current set position, and by calculating the offset error between the path segment where the actual position is located in the planned path and the actual position. This process does not involve a large amount of calculation and can meet real-time performance. The present application immediately performs the next set position correction after obtaining the offset error, which can enhance the anti-interference performance. Since the set position issued is corrected in real time, the moving body moves according to the corrected trajectory, which can improve the movement accuracy of the moving body.

[0018] As a possible implementation manner of the first aspect, determining the path segment where the actual position is located in the planned path includes:

[0019] Traversing each point within a preset range in the ordered point set of the planned path;

[0020] For every two consecutive points within the range, a first angle and a second angle between the actual position and the path segment formed by the two points are calculated; the angle between the line connecting the actual position and one of the two points and the path segment formed by the two points is the first angle, and the angle between the line connecting the actual position and the other of the two points and the path segment formed by the two points is the second angle;

[0021] When it is determined that both the first angle and the second angle are less than or equal to 90 degrees, the path segment formed by the corresponding two points is the path segment where the actual position is located in the planned path.

[0022] From the above, the present application only requires simple geometric operations, that is, it only needs to calculate the angle between the actual position and the line between each two consecutive points in the traversal range. When the angle satisfies the condition of being less than or equal to 90 degrees, the path segment where the moving body is located can be quickly located, which reduces the amount of calculation and improves the real-time processing capability.

[0023] As a possible implementation manner of the first aspect, the points within the preset range include:

[0024] A preset number of points before and after a previous set position in the planned path are taken as a reference, wherein the previous set position is a set position issued before the current set position.

[0025] From the above, compared to traversing the entire ordered point set to determine the path segment where the actual position is located, the present application uses a localized search strategy to only consider the path segment where the previous set position issued before the current set position is located and a preset number of points before and after it, which can significantly reduce the amount of calculation and speed up the processing speed.

[0026] As a possible implementation manner of the first aspect, the traversal includes:

[0027] According to the distance between the preset number of points and the previous set position in the order of the number of path segments in the planned path, traversal is performed from near to far.

[0028] From the above, the present application starts traversing from a point close to the previous set position, and can give priority to those path segments that are most likely to contain the current actual position, which helps to converge to the correct path segment faster and avoid unnecessary calculations.

[0029] As a possible implementation manner of the first aspect, the degree of deviation between the actual position and the path segment where the actual position is located includes:

[0030] The distance from the actual position to the path segment on which it is located.

[0031] From the above, the present application adopts distance as a measure of the degree of deviation, which reduces the amount of calculation and can significantly improve the processing speed.

[0032] As a possible implementation of the first aspect, the following also includes:

[0033] Obtaining the first time of sending the current set position;

[0034] Determine a second time at which the actual position is acquired;

[0035] Calculate the time difference between the first time and the second time as a delay error;

[0036] The next set position to be sent in the planned path is also corrected based on the delay error.

[0037] From the above, in practical applications, there is often a certain delay between issuing instructions and the moving body responding and reaching the specified position. The present application can effectively compensate for this delay by calculating the delay error and correcting it, so that the system can more accurately predict and adjust the actual position of the moving body.

[0038] As a possible implementation manner of the first aspect, the correcting the next set position to be issued in the planned path includes:

[0039] Set compensation ratio;

[0040] The next set position to be sent is corrected based on the compensation ratio, the offset error and the delay error.

[0041] From the above, the present application can flexibly adjust the influence of the offset error and the delay error according to the actual situation by introducing the compensation ratio, so as to achieve more accurate path correction and ensure that the moving body follows the preset planned path more accurately.

[0042] As a possible implementation manner of the first aspect, the correcting the next set position to be sent based on the compensation ratio, the offset error, and the delay error includes:

[0043] The corrected next set position to be issued = the next set position to be issued in the planned path * (100% - compensation ratio) + offset error * compensation ratio;

[0044] Among them, the next set position to be sent in the planned path is the Nth set position in the planned path, where N=the number of path segments where the actual position of the current set position is located in the planned path + the number of points in the planned path corresponding to the delay error.

[0045] To achieve the above-mentioned object, the second aspect of the present application provides a moving object moving device based on path error compensation, comprising:

[0046] A planned path acquisition module is used to acquire a set position in the planned path of the moving body, and send the set position to the moving body in sequence, so that the moving body moves according to the set position. The planned path can be described as an ordered point set, and the line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path;

[0047] A real-time position acquisition module, used to acquire in real time the actual position to which the moving body moves based on the currently set position sent;

[0048] An offset error determination module is used to determine the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where the actual position is located as an offset error;

[0049] The set position correction module is used to correct the next set position to be issued in the planned path based on the offset error, and issue the corrected next set position to the moving body so that the moving body moves based on the corrected next set position.

[0050] A third aspect of the present application provides a computing device, including:

[0051] processor, and

[0052] A memory having program instructions stored thereon, wherein when the program instructions are executed by the processor, the processor executes any of the methods described in the first aspect above.

[0053] A fourth aspect of the present application provides a computer-readable storage medium having program instructions stored thereon, wherein when the program instructions are executed by a computer, the computer implements any of the methods described in the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 A schematic diagram of the existing ADP method;

[0055] Figure 2 It is a flow chart of a moving body moving method based on path error compensation provided in an embodiment of the present application;

[0056] Figure 3 It is a schematic diagram of a method for calculating a path segment and an offset error of an actual position provided in an embodiment of the present application;

[0057] Figure 4 A schematic diagram of the time delay between a set position and an actual position provided in an embodiment of the present application;

[0058] Figure 5 A schematic diagram of a specific flow chart of a moving object moving method based on path error compensation provided in an embodiment of the present application;

[0059] Figure 6 is a schematic structural diagram of a moving object moving device based on path error compensation provided by an embodiment of the present application;

[0060] Figure 7 It is a structural schematic diagram of a computing device provided in an embodiment of the present application.

[0061] It should be understood that the size and shape of each block diagram in the above structural diagram are for reference only and should not constitute an exclusive interpretation of the embodiment of the present invention. The relative position and inclusion relationship between the blocks presented in the structural diagram are only schematic representations of the structural association between the blocks, and do not limit the physical connection method of the embodiment of the present invention. DETAILED DESCRIPTION

[0062] The technical solution provided by the present application is further described below with reference to the accompanying drawings and examples. It should be understood that the system structure and business scenarios provided in the examples of the present application are mainly to illustrate the possible implementation methods of the technical solution of the present application and should not be interpreted as the only limitation on the technical solution of the present application. It is known to those of ordinary skill in the art that with the evolution of the system structure and the emergence of new business scenarios, the technical solution provided by the present application is also applicable to similar technical problems.

[0063] It should be understood that the moving body movement solution based on path error compensation provided in the embodiments of the present application includes a moving body movement method and device based on path error compensation. Since the principles of solving the problems in these technical solutions are the same or similar, some repetitive parts may not be repeated in the introduction of the following specific embodiments, but it should be regarded as that these specific embodiments have been referenced to each other and can be combined with each other.

[0064] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present application. In case of any inconsistency, the meaning described in this specification or the meaning derived from the contents recorded in this specification shall prevail. In addition, the terms used herein are only for the purpose of describing the embodiments of the present application and are not intended to limit the present application.

[0065] The moving body movement scheme based on path error compensation in this application can be applied to devices that require path tracking and position control, for example: applied to the control of manipulators, automatic guided vehicles in warehousing systems, drones, intelligent self-driving vehicles (such as agricultural seeding equipment, etc.), logistics sorting robots, etc. This application can realize real-time judgment of the deviation between the moving body and the planned path, and perform error compensation to adjust the moving route in real time and improve the moving accuracy without complex calculations. The following is an introduction to this application in conjunction with specific embodiments.

[0066] [First embodiment of a moving object moving method based on path error compensation]

[0067] The first embodiment of the present application provides a moving object moving method based on path error compensation, such as Figure 2 As shown, the method includes:

[0068] S210: obtaining a set position in a planned path of the moving body, and sending the set position to the moving body in sequence, so that the moving body moves according to the set position, wherein the planned path can be described as an ordered point set, and a line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path;

[0069] S220: Acquire in real time the actual position to which the moving body moves based on the currently set position sent;

[0070] S230: determining the path segment where the actual position is located in the planned path, and calculating the degree of deviation between the actual position and the path segment where the actual position is located as an offset error;

[0071] S240: Correcting the next set position to be issued in the planned path based on the offset error, and issuing the corrected next set position to the moving body, so that the moving body moves based on the corrected next set position.

[0072] From the above, the present application performs compensation processing by obtaining the actual position to which the moving body moves according to the current set position, and by calculating the offset error between the path segment where the actual position is located in the planned path and the actual position. This process does not involve a large amount of calculation and can meet real-time performance. The present application immediately performs the next set position correction after obtaining the offset error, which can enhance the anti-interference performance. Since the set position issued is corrected in real time, the moving body moves according to the corrected trajectory, which can improve the movement accuracy of the moving body.

[0073] In some embodiments, the planned path of the moving body obtained in S210 may be a pre-generated planned path from the starting point to the destination, and may be generated on a host computer or client side and then sent to the moving body.

[0074] The prior art ADP requires a large amount of high-quality training data, and the calculation also consumes a lot of resources. The present application does not require training data, but only needs to be based on planning data, and its calculation amount is far less than that of ADP.

[0075] In some embodiments, the planned path of the moving body in S210 can be a path determined by at least one of the following path planning methods: for example, a global static planning method, a shortest path method, a method combining specified waypoints, and a full traversal path method (commonly used in sweeping robots, etc.).

[0076] In some embodiments, the planned path of the moving body can be described as a set of ordered discrete points in a Cartesian coordinate system, where each point defines the planned path position (i.e., set position) of the moving body in the Cartesian coordinate system. The set position refers to the specific position that the system expects the moving body (e.g., robot, robotic arm, etc.) to reach. Each set position will be converted into a control instruction and sent to the control system of the moving body (e.g., motor controller). In some embodiments, these ordered point sets can be stored in an array, such as RoadData[TotalNum], where TotalNum represents the total number of set position points of the path. These set position points are arranged in time or logical order and connected in sequence to form a continuous trajectory to guide the movement of the moving body from the starting set position to the end set position. The line connecting every two set position points in the array constitutes a path segment in the planned path. The set position will be sent to the moving body in sequence according to the sending cycle (according to the actual setting).

[0077] In the process of path planning for a moving body, the data used to define its route can be directly obtained from external sources. These data support flexible input modes: they can be input separately from multiple different points (positions or time points) to gradually build complete path information; or all necessary path data can be input from a single point at one time to achieve fast path setting.

[0078] The following is a further example. In the Cartesian coordinate system, each set position point in the ordered point set is represented by three-dimensional coordinates (Px, Py, Pz), where: Px: the coordinate value on the X-axis, Py: the coordinate value on the Y-axis, and Pz: the coordinate value on the Z-axis.

[0079] Among them, each line connecting two consecutive points constitutes a path segment in the planned path, as shown in Table 1:

[0080] Table 1

[0081] Serial number Px(mm) Py(mm) Pz(mm) 1 100 200 300 2 150 250 350 3 200 300 400 4 250 350 450 5 300 400 500

[0082] For example, point 1 to point 2 is a path segment (segment 1), point 2 to point 3 is another path segment (segment 2), and so on. If the position is set to point 1, then the number of segments of the set position is segment 1, and if the position is set to point 2, then the number of segments of the set position is segment 2, that is, the number of segments of a certain position and the corresponding point (number of points) of the position in the planned path are one-to-one corresponding. Each path segment is of equal length (the distance between each adjacent point is the same). The moving body moves at a constant speed.

[0083] In some embodiments, the control system of the moving body adjusts the speed and direction of the moving body according to the instruction containing the set position so that it reaches the set position as accurately as possible. However, in the actual operation process, due to the influence of various factors (such as mechanical wear, external interference, etc.), the moving body may not fall accurately on the set position of the planned path after movement, and may deviate from the set position. When the set position is issued to the moving body, the moving body can immediately move to the actual position based on the set position, that is, there is no time difference between issuing the set position and the moving body moving based on the set position. Based on this, in S220, the present application needs to obtain the actual position to which the moving body moves based on the issued current set position at the current time. The acquisition method can be external detection or self-reporting by the moving body.

[0084] In some embodiments, in S230, since the moving body deviates from the set position after movement, the subsequent set position needs to be corrected so that the moving body can accurately move to the set position based on the corrected set position. After the actual position is determined, it is necessary to calculate the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where it is located as the offset error.

[0085] In some embodiments, in S230, determining the path segment where the actual position is located in the planned path includes:

[0086] Traversing each point within a preset range in the ordered point set of the planned path;

[0087] For every two consecutive points within the range, a first angle and a second angle between the actual position and the path segment formed by the two points are calculated; the angle between the line connecting the actual position and one of the two points and the path segment formed by the two points is the first angle, and the angle between the line connecting the actual position and the other of the two points and the path segment formed by the two points is the second angle;

[0088] When it is determined that both the first angle and the second angle are less than or equal to 90 degrees, the path segment formed by the corresponding two points is the path segment where the actual position is located in the planned path.

[0089] Specifically, when determining the path segment where the actual position is located in the planned path, it is necessary to first set the traversal range, which can be a global range, that is, the entire ordered point set. It can also be each point within a preset range in the ordered point set of the planned path, which can reduce unnecessary global searches and improve search efficiency.

[0090] The path segment where the actual position is located in the planned path corresponds to the number of points in the planned path.

[0091] Specifically, each time two consecutive points are traversed, the first angle and the second angle between the actual position and the line connecting the two points are calculated, such as Figure 3 As shown, the actual position is Pact. When it is in the area between two points Pleft and Pright, the two angles theta between it and the line between the two points are calculated, where the angle between the line connecting the actual position Pact and Pleft and the path segment formed by the two points (Pleft and Pright) is the first angle theta, and the angle between the line connecting the actual position Pact and Pright and the path segment formed by the two points (Pleft and Pright) is the second angle theta. When the two angles are both less than or equal to 90°, the path segment where the actual position is located in the planned path is found.

[0092] In this way, the present application only requires simple geometric operations, that is, it only needs to calculate the angle between the actual position and the line between each two consecutive points in the traversal range. When the angle satisfies the condition of being less than or equal to 90 degrees, the path segment where the moving body is located can be quickly located, reducing the amount of calculation and improving real-time processing capabilities.

[0093] In some embodiments, each point within the preset range includes: a preset number of points before and after the previous set position in the planned path, based on the number of path segments in the planned path, wherein the previous set position is the set position that was previously issued before the current set position.

[0094] Specifically, each point within the preset range can be a point with a preset amount of points before and after the previous set position of the current set position in the planned path. This setting is in line with the actual situation because the moving body usually does not suddenly jump to a place far away from the last set position.

[0095] In some embodiments, the traversal includes: traversing from near to far according to the distance between the preset number of points and the order of the number of path segments in which the previous set position is located in the planned path.

[0096] Specifically, when the traversal range is determined to be the points within a preset range, the specific traversal is performed from near to far according to the distance of the preset number of points from the order of the number of path segments in which the previous set position is located in the planned path. This can give priority to those path segments that are most likely to contain the actual position, help to converge to the correct path segment more quickly, avoid unnecessary calculations, and solve the problem of slow convergence or even failure of the ADP algorithm to converge to the global optimal solution due to inappropriate initial conditions or parameter settings, especially when it is used in practice, it is easy to have non-convergence problems.

[0097] The following example illustrates the traversal within a preset range and the process of determining path segments:

[0098] Assume there is a planned path, which consists of 10 points, marked as P1 to P10. These points are divided into several segments, each containing 2 points, so there are 9 segments in total (S1: P1-P2, S2: P2-P3, S3: P3-P4, S4: P4-P5, S5: P5-P6, S6: P6-P7, S7: P7-P8, S8: P8-P9, S9: P9-P10). The moving body needs to move based on the current set position P6, which is located at the junction between S5 (P5-P6) and S6 (P6-P7), and its actual position of movement is P6'. The path segment number of the previous set position P5 of the current set position P6 in the planned path is S5: P5-P6, then the points within the preset range are all points in the segment before and after the path segment S5 where the previous set position P5 is located, such as all points from S4 to S7, that is, P4, P5, P6, P7.

[0099] According to the principle of "from near to far", the traversal order should be as follows: first consider the two point pairs closest to the path segment S5 where the previous set position P5 is located, that is, P5-P6 (S5). Then consider the point pairs in the path segment adjacent to S5, first P4-P5 (S4), and then P6-P7 (S6).

[0100] Next, for each two consecutive points (eg, P4-P5, P5-P6, P6-P7), the first angle and the second angle between the actual position P6' and the line between the two points are calculated, and it is checked whether these angles are less than or equal to 90 degrees.

[0101] For P5-P6(S5):

[0102] Calculate θ1(P5-P6-P6') and θ2(P6-P6'-P5), assuming θ1=75° and θ2=95°, the condition is not satisfied (because θ2>90°).

[0103] For P4-P5(S4):

[0104] Calculate θ1(P4-P5-P6') and θ2(P5-P6'-P4), assuming θ1=45° and θ2=125°, then the condition is not satisfied (because θ2>90°).

[0105] For P6-P7(S6):

[0106] Calculate θ1(P6-P7-P6') and θ2(P7-P6'-P6), assuming θ1=85° and θ2=80°, then the condition is satisfied (because θ1 and θ2 are both less than or equal to 90°).

[0107] The segment number where the actual position P6' is located is found, that is, S6, the 6th segment.

[0108] In some embodiments, in S230, the degree of deviation between the actual position and the path segment where the actual position is located includes: the distance from the actual position to the path segment where the actual position is located.

[0109] Specifically, Figure 3 As shown, after determining the path segment where the actual position is located, the three-dimensional coordinates of the points Pleft and Pright on the left and right sides of the actual position Pact can be taken out from the ordered point set, and then the angle theta between PrightPact and PrightPleft can be calculated through trigonometric functions, thereby obtaining the distance PathOffset of the line connecting Pact and PleftPright, which is the degree of deviation between the actual position and the path segment where it is located.

[0110] For example: the tenth set position point is 1.0, and the eleventh set position point is 1.5. At this time, the actual position collected is 1.249 captured at 10.5ms; the set position corresponding to 10.5ms should be 1.25 (under ideal conditions with enough and dense points, the path between the two points can be regarded as a line segment, that is, the idea of ​​turning a circle into a straight line). If the 10.5ms captured point 1.249 is directly compared with the point (that is, the tenth point is 1.0), the error is 0.249, while the actual error with the path (between the tenth and eleventh points) is 0.001 (that is, the set position corresponding to 10.5ms should be 1.25 and 1.249). If 0.249 is used directly for compensation, it will increase the overall deviation, so the distance from the path segment is used as the degree of deviation.

[0111] The present application aims to reduce the error on the path, hoping to make the actual path gradually close to the planned path in the continuous adjustment. Therefore, the present application determines the path segment where the actual position is located, and determines the distance from the actual position to the path segment where it is located as the degree of deviation.

[0112] In some embodiments, in S240, when the set position is issued to the moving body, the moving body can immediately move to the actual position based on the set position, that is, there is no time difference between issuing the set position and the moving body moving based on the set position. After the moving body determines the offset error based on the actual position of the current set position at the current time, the next set position of the current set position at the current time will be corrected and issued, so that the moving body moves based on the corrected set position to achieve the correction of the motion path. For example, if the current set position at the current time is P6, the next set position is P7, and P7 is corrected and issued.

[0113] In some embodiments, the above steps S210 and S230 may be implemented by a function module GetPointNum, which may be located in a moving body (such as a robotic arm). Table 2 shows the input and output parameters included in the function module GetPointNum. The function module may be implemented in software or hardware. When implemented in a physical device (i.e., hardware), the input and output correspond to the input pins and output pins of the device:

[0114] Table 2

[0115]

[0116] Among them, using the GetPointNum block, first write the data (i.e., set positions) in the planned path point set RoadData in sequence. Accumulate from zero through IngNum, input the description value in Cartesian space corresponding to the IngNum-th point in the static planning data through CartPos (this example is the control of the robot arm, and the original set position point is described in the axis space coordinate, so there is a conversion to Cartesian space coordinates), and Exe1 continuously sets the rising edge to write the data. For each data input, Exe1 needs to set a rising edge. After the data is written, GetDataOver becomes true. In this way, step S210 is executed.

[0117] After the output pin GetDataOver becomes true, the input pins ActPos (actual position, which corresponds to the actual position obtained from step S220), FindRange (traversal range) and Precision input parameters (i.e. actual position, traversal range) are updated each time the actual position is input, and the traversal range can be adjusted according to demand. Set the initial value of PointNumNow to 0, and then assign the GetPointNum (path segment) obtained in the previous execution to PointNumNow for each execution. In this way, each search is performed within the specified range of the last set position, reducing the amount of calculation and resource consumption. After the input parameter configuration is completed, set Exe2 to true, that is, start searching for the number of path segments where the input actual position ActPos is located in the planned path, and output the number of path segments where the actual position ActPos is located in the planned path through GetPointNum. At the same time, the error pathOffset between the actual position and the set position is also output. After completion, the Done signal is set to true, and the next search can be started. After finding the required number of segments, the search loop is exited to reduce resource consumption; if the segment is not found within the search range, the function block GetPointNum reports an error, which is output by Error and ErrorID, and then the search range FindRange can be expanded and searched again. In this way, step S230 is executed.

[0118] In some other embodiments, it also includes:

[0119] Obtaining the first time of sending the current set position;

[0120] Determine a second time at which the actual position is acquired;

[0121] Calculate the time difference between the first time and the second time as a delay error;

[0122] The next set position to be sent in the planned path is also corrected based on the delay error.

[0123] Specifically, due to the different performance of different controllers, there will be a time difference between the control instruction containing the set position being issued and taking effect, that is, there is a time difference between issuing the current set position and the movement of the moving body based on the current set position, that is, there is a time difference between issuing the current set position and obtaining the corresponding actual position. Figure 4 As shown, the first set position is sent at the 1st second, and the moving body moves based on the first set position after the 4th second, with a time difference of 3.xs. Since x is difficult to determine, it can be considered that the time difference is 3s.

[0124] Corresponding to the ordered point set of the planned path, the delay error is measured in units of points on the path, that is, the number of points in the path planning data corresponding to the time difference between sending the current set position and receiving the actual position. For example, if there is a difference of 3 position points between sending the first set position and obtaining the actual position, the delay error is 3 position points.

[0125] From the above, in practical applications, there is often a certain delay between issuing instructions and the moving body responding and reaching the specified position. The present application can effectively compensate for this delay by calculating the delay error and correcting it, so that the system can more accurately predict and adjust the actual position of the moving body.

[0126] In some embodiments, the correcting the next set position in the planned path includes:

[0127] Set compensation ratio;

[0128] The next set position to be sent is corrected based on the compensation ratio, the offset error and the delay error.

[0129] In some embodiments, the correcting the next set position to be sent based on the compensation ratio, the offset error and the delay error includes:

[0130] The corrected next set position to be issued = the next set position to be issued in the planned path * (100% - compensation ratio) + offset error * compensation ratio;

[0131] Among them, the next set position to be sent in the planned path is the Nth set position in the planned path, where N=the number of path segments where the actual position of the current set position is located in the planned path + the number of points in the planned path corresponding to the delay error.

[0132] Specifically, the compensation ratio can be adjusted according to actual conditions, and it is recommended not to exceed 5%.

[0133] Due to the delay in the control system, when the N1th actual position point (ActPos N1) is obtained, there will be a delay of nT in the unit of operation cycle T, where n is a positive integer, and the control instruction will actually take effect after nT cycles. Therefore, if the next set position point is calculated and issued at this time, the next set position to be issued actually corresponds to the future (N1+n)th position point, and the error PathOffset recorded previously is added to this point as the new set position.

[0134] by Figure 4 The figure is taken as an example to illustrate which setting point is the next setting position point to be issued when there is an experimental deviation.

[0135] After the four set positions are issued, the moving body starts to move based on the first set position, that is, the set position SetPos[1] is issued in the 1st second, the moving body starts to move based on the set position SetPos[1] in the 4th second, and feeds back the actual position ActPos[1]. The set position SetPos[2] is issued in the 2nd second, the moving body starts to move based on the set position SetPos[2] in the 5th second, and feeds back the actual position ActPos[2]. The set position SetPos[3] is issued in the 3rd second, the moving body starts to move based on the set position SetPos[3] in the 6th second, and feeds back the actual position ActPos[3]. The set position SetPos[4] is issued in the 4th second, the moving body starts to move based on the set position SetPos[4] in the 7th second, and feeds back the actual position ActPos[4]. That is, there is a delay deviation of 3 task cycles between issuing the set position and feeding back the actual position.

[0136] Based on the actual position ActPos[4], it is determined that the number of path segments where ActPos[4] is located in the planned path is 4, so the next set position to be issued is SetPos[4+3=7], that is, the corrected SetPos[7] is issued in the 5th second (corrected SetPos[7]=initial SetPos[7]*(100%-compensation ratio)+PathOffset*compensation ratio), so that the moving body directly skips SetPos[5] and SetPos[6], and moves based on the corrected SetPos[7], thus eliminating the delay.

[0137] From the above, by introducing the compensation ratio, this application can flexibly adjust the influence of the offset error and the delay error according to the actual situation, so as to achieve more accurate path correction and ensure that the moving body follows the preset planned path more accurately. It can effectively reduce the influence of the internal delay of the control system and improve the movement accuracy of the robot.

[0138] [A specific implementation method of a moving object moving method based on path error compensation]

[0139] The second embodiment of the present application provides a moving body moving method based on path error compensation. In this embodiment, it is applied to the movement of a robot, such as Figure 5 As shown, the specific steps of this method are as follows:

[0140] S300: Acquire a planned path of the moving body, and send the set positions SetPos in the planned path to the manipulator in sequence, so that the manipulator moves according to the current set positions in the planned path;

[0141] S305: Acquire in real time the actual position to which the robot moves based on the issued current set position;

[0142] S310: Obtain the first time when the current set position is sent, determine the second time when the actual position is obtained, calculate the delay error between the first time and the second time, correspond to the ordered point set of the planned path, and express it in the delay error point number Nlate;

[0143] S315: taking the number of path segments of the previous set position of the current set position in the planned path as a reference, traversing a preset number of points before and after the previous set position as a traversal range to determine the path segment of the planned path where the actual position is located;

[0144] Specifically, according to the distance of the preset number of points from the previous set position in the order of the number of path segments in the planned path, the traversal is performed from near to far. For every two consecutive points Pleft and Pright in the range, the three-dimensional coordinates of the points Pleft and Pright on the left and right sides of the actual position Pact are taken out, and then the angle theta1 between PrightPact and PrightPleft and the angle theta2 between PleftPact and PleftPright can be calculated by trigonometric functions (such as Figure 3 As shown), when theta1 and theta2 are both less than or equal to 90 degrees, the path segment formed by the corresponding two points Pleft and Pright is the path segment GetPointNum1 where the actual position Pact is located in the planned path;

[0145] S320: Calculate the distance from the actual position Pact to the path segment where it is located as the offset error, such as Figure 3 As shown, the three-dimensional coordinates of the points Pleft and Pright on the left and right sides of the actual position Pact are taken from the ordered point set of the planned path, and then the angle theta1 between PrightPact and PrightPleft can be calculated through trigonometric functions, so as to find the distance PathOffset between the line connecting Pact and PleftPright, which is the degree of deviation between the actual position and the path segment it is located in.

[0146] S325: Set the compensation ratio Percent1, and correct the next set position [GetPointNum1+Nlate] based on the compensation ratio Percent1, the offset error pathOffset, and the delay error point number Nlate:

[0147] The corrected next set position to be sent [GetPointNum1+Nlate]=the next set position on the original planned path [GetPointNum1+Nlate]*(100%-Percent1)+PathOffset1*Percent1.

[0148] Wherein, GetPointNum1+Nlate is the number of path segments where the actual position of the current set position is located in the planned path + the number of points in the planned path corresponding to the delay error.

[0149] S330: Sending the corrected next set position, so that the robot arm moves according to the corrected next set position.

[0150] S305-S325 are executed cyclically with the next set position as the current set position until the moving body finishes moving.

[0151] [Embodiment of the moving device of the present application based on path error compensation]

[0152] like Figure 6 As shown, the embodiment of the present application provides a moving body moving device based on path error compensation, and the moving body moving device based on path error compensation can be used to implement the moving body moving method based on path error compensation in the above embodiment, such as Figure 6 As shown, the moving object moving device based on path error compensation has a planned path acquisition module 610, a real-time position acquisition module 620, an offset error determination module 630, and a set position correction module 640.

[0153] A planned path acquisition module is used to acquire a set position in the planned path of the moving body, and send the set position to the moving body in sequence, so that the moving body moves according to the set position. The planned path can be described as an ordered point set, and the line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path;

[0154] A real-time position acquisition module, used to acquire in real time the actual position to which the moving body moves based on the currently set position sent;

[0155] An offset error determination module is used to determine the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where the actual position is located as an offset error;

[0156] The set position correction module is used to correct the next set position to be issued in the planned path based on the offset error, and issue the corrected next set position to the moving body so that the moving body moves based on the corrected next set position.

[0157] For details, please refer to the detailed description in the first embodiment of the method, which will not be repeated here.

[0158] [Embodiment of the computing device of the present application]

[0159] Figure 7 900 is a schematic structural diagram of a computing device 900 provided in an embodiment of the present application. The computing device can be used as a moving body moving device based on path error compensation to execute the optional embodiments of the moving body moving method based on path error compensation. The computing device can be a terminal or a chip or chip system inside the terminal. Figure 7 As shown, the computing device 900 includes: a processor 910 , a memory 920 , and a communication interface 930 .

[0160] It should be understood that Figure 7The communication interface 930 in the computing device 900 shown may be used to communicate with other devices, and may specifically include one or more transceiver circuits or interface circuits.

[0161] The processor 910 may be connected to a memory 920. The memory 920 may be used to store the program code and data. Therefore, the memory 920 may be a storage unit inside the processor 910, or an external storage unit independent of the processor 910, or a component including a storage unit inside the processor 910 and an external storage unit independent of the processor 910.

[0162] Optionally, the computing device 900 may further include a bus. The memory 920 and the communication interface 930 may be connected to the processor 910 via the bus. The bus may be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 A line without an arrow is used to represent the bus, but this does not mean that there is only one bus or one type of bus.

[0163] It should be understood that in the embodiment of the present application, the processor 910 may adopt a central processing unit (CPU). The processor may also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc. Alternatively, the processor 910 may adopt one or more integrated circuits to execute relevant programs to implement the technical solutions provided in the embodiment of the present application.

[0164] The memory 920 may include a read-only memory and a random access memory, and provides instructions and data to the processor 910. A portion of the processor 910 may also include a nonvolatile random access memory. For example, the processor 910 may also store information on the device type.

[0165] When the computing device 900 is running, the processor 910 executes the computer-executable instructions in the memory 920 to perform any operation step of the above method and any optional embodiment thereof.

[0166] It should be understood that the computing device 900 according to the embodiment of the present application can correspond to the corresponding subjects in the methods according to the embodiments of the present application, and the above-mentioned and other operations and / or functions of each module in the computing device 900 are respectively for implementing the corresponding processes of each method of the present embodiment. For the sake of brevity, they will not be repeated here.

[0167] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

[0168] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0169] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0170] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0171] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0172] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0173] An embodiment of the present application also provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, it is used to execute the above method, which includes at least one of the solutions described in the above embodiments.

[0174] The computer storage medium of the embodiment of the present application can adopt any combination of one or more computer-readable media. Computer-readable media can be computer-readable signal media or computer-readable storage media. Computer-readable storage media can be, for example, but not limited to, electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices or devices, or any combination of the above. More specific examples (non-exhaustive lists) of computer-readable storage media include: electrical connections with one or more wires, portable computer disks, hard disks, random access memories (RAM), read-only memories (ROM), erasable programmable read-only memories (EPROM or flash memory), optical fibers, portable compact disk read-only memories (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the above. In this document, computer-readable storage media can be any tangible medium containing or storing programs, which can be used by instruction execution systems, devices or devices or used in combination with them.

[0175] Computer-readable signal media may include data signals propagated in baseband or as part of a carrier wave, which carry computer-readable program code. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. Computer-readable signal media may also be any computer-readable medium other than a computer-readable storage medium, which may send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device.

[0176] The program code embodied on the computer readable medium may be transmitted using any appropriate medium, including but not limited to wireless, wireline, optical fiber cable, RF, etc., or any suitable combination of the foregoing.

[0177] Computer program code for performing the operation of the present application can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages, such as Java, Smalltalk, C++, and conventional procedural programming languages, such as "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computer (e.g., using an Internet service provider to connect through the Internet).

[0178] In addition, the words "first, second, third, etc." or module A, module B, module C and other similar terms in the specification and claims are only used to distinguish similar objects and do not represent a specific ordering of the objects. It can be understood that the specific order or sequence can be interchanged where permitted so that the embodiments of the present application described herein can be implemented in an order other than that illustrated or described herein.

[0179] In the above description, the numbers representing the steps, such as S110, S120, etc., do not necessarily mean that the steps will be executed in this manner. If permitted, the order of the steps can be interchanged or they can be executed simultaneously.

[0180] The term "comprising" as used in the description and claims should not be interpreted as being limited to what is listed thereafter; it does not exclude other elements or steps. Therefore, it should be interpreted as specifying the presence of the features, integers, steps or components mentioned, but does not exclude the presence or addition of one or more other features, integers, steps or components and groups thereof. Therefore, the expression "a device comprising means A and B" should not be limited to a device consisting of components A and B only.

[0181] References to "one embodiment" or "an embodiment" in this specification mean that a particular feature, structure, or characteristic described in conjunction with the embodiment is included in at least one embodiment of the present application. Therefore, the phrases "in one embodiment" or "in an embodiment" appearing in various places in this specification do not necessarily refer to the same embodiment, but may refer to the same embodiment. In addition, in one or more embodiments, the particular features, structures, or characteristics can be combined in any appropriate manner, as would be apparent to one of ordinary skill in the art from this disclosure.

[0182] Note that the above are only preferred embodiments of the present application and the technical principles used. Those skilled in the art will understand that the present application is not limited to the specific embodiments described herein, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of protection of the present application. Therefore, although the present application is described in more detail through the above embodiments, the present application is not limited to the above embodiments, and may also include more other equivalent embodiments without departing from the concept of the present application, all of which belong to the scope of protection of the present application.

Claims

1. A moving object moving method based on path error compensation, characterized in that: include: Acquire a set position in a planned path of the moving body, and send the set position to the moving body in sequence, so that the moving body moves according to the set position, wherein the planned path can be described as an ordered point set, and a line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path; Real-time acquisition of the actual position to which the moving body moves based on the currently set position sent; Determine the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where the actual position is located as an offset error; The next set position to be issued in the planned path is corrected based on the offset error, and the corrected next set position is issued to the moving body, so that the moving body moves based on the corrected next set position.

2. The method according to claim 1, characterized in that The determining of the path segment where the actual position is located in the planned path includes: Traversing each point within a preset range in the ordered point set of the planned path; For every two consecutive points within the range, a first angle and a second angle between the actual position and the path segment formed by the two points are calculated; the angle between the line connecting the actual position and one of the two points and the path segment formed by the two points is the first angle, and the angle between the line connecting the actual position and the other of the two points and the path segment formed by the two points is the second angle; When it is determined that both the first angle and the second angle are less than or equal to 90 degrees, the path segment formed by the corresponding two points is the path segment where the actual position is located in the planned path.

3. The method according to claim 2, characterized in that The points within the preset range include: A preset number of points before and after a previous set position in the planned path are taken as a reference, wherein the previous set position is a set position issued before the current set position.

4. The method according to claim 3, characterized in that The traversal includes: According to the distance between the preset number of points and the previous set position in the order of the number of path segments in the planned path, traversal is performed from near to far.

5. The method according to claim 1, characterized in that The degree of deviation between the actual position and the path segment where the actual position is located includes: The distance from the actual position to the path segment on which it is located.

6. The method according to claim 1, characterized in that Also includes: Obtaining the first time of sending the current set position; Determine a second time at which the actual position is acquired; Calculate the time difference between the first time and the second time as a delay error; The next set position to be sent in the planned path is also corrected based on the delay error.

7. The method according to claim 6, characterized in that The correcting of the next set position to be issued in the planned path includes: Set compensation ratio; The next set position to be sent is corrected based on the compensation ratio, the offset error and the delay error.

8. The method according to claim 7, characterized in that The correcting the next set position to be sent based on the compensation ratio, the offset error and the delay error includes: The corrected next set position to be issued = the next set position to be issued in the planned path * (100% - compensation ratio) + offset error * compensation ratio; Among them, the next set position to be sent in the planned path is the Nth set position in the planned path, where N=the number of path segments where the actual position of the current set position is located in the planned path + the number of points in the planned path corresponding to the delay error.

9. A moving object moving device based on path error compensation, characterized in that: include: A planned path acquisition module is used to acquire a set position in the planned path of the moving body, and send the set position to the moving body in sequence, so that the moving body moves according to the set position. The planned path can be described as an ordered point set, and the line connecting every two consecutive set position points in the ordered point set constitutes a path segment in the planned path; A real-time position acquisition module, used to acquire in real time the actual position to which the moving body moves based on the currently set position sent; An offset error determination module is used to determine the path segment where the actual position is located in the planned path, and calculate the degree of deviation between the actual position and the path segment where the actual position is located as an offset error; The set position correction module is used to correct the next set position to be issued in the planned path based on the offset error, and issue the corrected next set position to the moving body so that the moving body moves based on the corrected next set position.

10. A computing device, characterized in that include: processor, and A memory having program instructions stored thereon, wherein when the program instructions are executed by the processor, the processor is caused to execute the method according to any one of claims 1 to 8.

Images