Multi-path-point continuous pose synchronous planning method, electronic equipment and storage medium

Abstract

The invention discloses a multi-waypoint continuous pose synchronous planning method, electronic equipment and a storage medium, and the method comprises the steps: firstly obtaining the position and attitude information of at least three waypoints, and determining the position and attitude information corresponding to at least two transition points according to the position and attitude information; then determining the length of a position path between adjacent points in the road point and the transition point and generating a queue, converting the attitude information into a unit quaternion, determining the length of an attitude path and generating a queue; based on the two queues, establishing a position and posture corresponding relation, including a two-dimensional array and a two-dimensional curve; receiving input speed information, and planning a position degree-of-freedom motion rate; and finally, determining the Cartesian space position and attitude corresponding to the current moment according to the current moment, the position degree-of-freedom motion rate and the position attitude corresponding relation. According to the method, the continuity of Cartesian space motion planning is ensured, the calculation complexity is reduced, and the non-synchronization of pose planning is avoided.

Description

Technical Field

[0001] This invention relates to the field of robot trajectory planning technology, and in particular to a multi-pathpoint continuous pose synchronization planning method, electronic device and storage medium. Background Technology

[0002] As the manufacturing industry moves towards intelligence and high precision, robots are being used more widely and deeply on production lines, participating in various key tasks of production. To ensure the production efficiency and delivery quality of robots, they need to operate efficiently and continuously across multiple waypoints, executing complex trajectories with high motion precision within a specified space, while ensuring continuity and stability during movement. In addition to the above execution conditions, for tasks such as those involving complex curves and surfaces, besides ensuring the precise positional accuracy of the tool, the attitude planning of the tool during continuous movement also affects the robot's delivery quality, requiring assurance of the tool's relative attitude relationship with the plane at each moment. Therefore, how to plan the phase consistency of the robot's position and attitude during operation is an important research topic for ensuring work quality. Currently, conventional implementation methods fall into two categories:

[0003] 1. Proportional Synchronization: For point-to-point motion tasks, based on the proportional relationship between position path length and attitude axis angle rotation, the attitude axis angle rotation corresponding to each position at each moment can be obtained by first planning the position path motion rate and then calculating it according to the pose ratio.

[0004] 2. Polynomial Programming: For multi-point motion tasks, when the motion rate is known, the total planning time can be specified when using polynomial programming. Therefore, the motion rate of the multi-point location path is planned first to obtain the running time between each point. Then, high-order polynomial programming is used to calculate the motion rate of the attitude between each point by specifying the time between waypoints, ensuring that the position and attitude planning time are consistent. The attitude description can support multiple methods such as axis angle, Euler angle, and quaternion.

[0005] However, the two implementation methods mentioned above have the following drawbacks:

[0006] 1. Proportional synchronization: Since the attitude between two points is described by axis angle, the attitude between multiple points changes along the axis direction. Therefore, this method only supports point-to-point planning and is difficult to extend to multi-point continuous planning, and cannot guarantee the continuity of motion between multiple points.

[0007] 2. Polynomial programming: Each degree of freedom (such as the three-dimensional coordinates of position x, y, z and the Euler angles, quaternions and other parameters of attitude) is calculated separately. The calculation of motion rate per cycle is large, and the results vary greatly when different speed acceleration constraints are set. It only guarantees the time synchronization when reaching each waypoint. It cannot guarantee the consistency of phase during the motion process. That is, it is difficult to guarantee the synchronization of position and attitude of the end tool at each moment during the motion process and the consistency of motion at different speeds. Summary of the Invention

[0008] This invention provides a multi-path point continuous pose synchronization planning method, electronic device and storage medium. While ensuring the continuity of motion planning in Cartesian space, this method not only reduces the computational complexity of multidimensional solutions, but also avoids the pose planning asynchrony problem caused by multidimensional phase inconsistency.

[0009] According to one aspect of the present invention, a multi-path continuous pose synchronization planning method is provided, comprising:

[0010] Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius;

[0011] Based on waypoint location information and transition point location information, determine the location path length between any two adjacent points among waypoints and transition points, and generate a location path length queue based on the location path length.

[0012] The waypoint attitude information and transition point attitude information are converted into unit quaternions. The attitude path length between adjacent points is determined based on the unit quaternion, and an attitude path length queue is generated based on the attitude path length.

[0013] The position and attitude correspondence is established based on the position path length queue and the attitude path length queue. The position and attitude correspondence includes a two-dimensional array and a two-dimensional curve.

[0014] Receive input velocity information and calculate the positional degree of freedom motion rate based on the velocity information and positional path length;

[0015] Based on the correspondence between the current time, the motion rate of the degree of freedom of the position, and the position and attitude, determine the Cartesian space position and Cartesian space attitude corresponding to the current time.

[0016] According to another aspect of the present invention, a multi-path continuous pose synchronization planning device is provided, comprising:

[0017] Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius;

[0018] Based on waypoint location information and transition point location information, determine the location path length between any two adjacent points among waypoints and transition points, and generate a location path length queue based on the location path length.

[0019] The waypoint attitude information and transition point attitude information are converted into unit quaternions. The attitude path length between adjacent points is determined based on the unit quaternion, and an attitude path length queue is generated based on the attitude path length.

[0020] The position and attitude correspondence is established based on the position path length queue and the attitude path length queue. The position and attitude correspondence includes a two-dimensional array and a two-dimensional curve.

[0021] Receive input velocity information and calculate the positional degree of freedom motion rate based on the velocity information and positional path length;

[0022] Based on the correspondence between the current time, the motion rate of the degree of freedom of the position, and the position and attitude, determine the Cartesian space position and Cartesian space attitude corresponding to the current time.

[0023] According to another aspect of the present invention, an electronic device is provided, the electronic device comprising:

[0024] At least one processor;

[0025] and memory that is communicatively connected to at least one processor;

[0026] The memory stores a computer program that can be executed by at least one processor, and the computer program is executed by at least one processor so that at least one processor can execute the multi-path point continuous pose synchronization planning method of any embodiment of the present invention.

[0027] According to another aspect of the present invention, a computer-readable storage medium is provided, which stores computer instructions for causing a processor to execute and implement the multi-path point continuous pose synchronization planning method of any embodiment of the present invention.

[0028] The technical solution of this invention first acquires the position and attitude information of at least three waypoints, thereby determining the position and attitude information of at least two transition points. Next, it determines the position path length between adjacent points among the waypoints and transition points and generates a queue. The attitude information is then converted into unit quaternions to determine the attitude path length and generate another queue. Subsequently, a position-attitude correspondence is established based on these two queues, including a two-dimensional array and a two-dimensional curve. Then, input velocity information is received, and the motion rate of the position degrees of freedom is planned. Finally, based on the current time, the motion rate of the position degrees of freedom, and the position-attitude correspondence, the Cartesian space position and attitude corresponding to the current time are determined. This method establishes a position-attitude correspondence based on two queues, enabling the correlation and synchronous planning of position and attitude changes. By planning the motion rate of the position degrees of freedom upon receiving velocity information and determining the Cartesian space position and attitude based on the current time and the position-attitude correspondence, the continuity of motion planning is ensured. The Cartesian space attitude can be obtained based on the position-attitude correspondence, reducing the computational complexity of multi-dimensional solutions and ensuring the phase consistency of position and attitude planning, thus avoiding the problem of asynchronous pose planning.

[0029] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0030] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0031] Figure 1 A flowchart of a multi-path continuous pose synchronization planning method provided in an embodiment of the present invention;

[0032] Figure 2 The fusion transition trajectory diagram of the fitting intermediate segment provided in the embodiment of the present invention;

[0033] Figure 3 A schematic diagram of a unit hypersphere provided in an embodiment of the present invention;

[0034] Figure 4 A flowchart of another multi-path point continuous pose synchronization planning method provided in an embodiment of the present invention;

[0035] Figure 5 This is a schematic diagram of the position and attitude relationship provided in an embodiment of the present invention;

[0036] Figure 6A flowchart of another multi-path continuous pose synchronization planning method provided in an embodiment of the present invention;

[0037] Figure 7 A flowchart of a multi-path continuous pose synchronization planning method provided as an optional embodiment of the present invention;

[0038] Figure 8 This is a schematic diagram of the structure of a multi-path continuous pose synchronization planning device provided in an embodiment of the present invention;

[0039] Figure 9 A schematic diagram of the structure of an electronic device for implementing the multi-path continuous pose synchronization planning method of this invention. Detailed Implementation

[0040] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0041] It should be noted that the terms "first," "second," etc., in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that the embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0042] Figure 1 This is a flowchart illustrating a multi-path continuous pose synchronization planning method provided in an embodiment of the present invention. This embodiment is applicable to robot trajectory planning and solving. The method can be executed by a multi-path continuous pose synchronization planning device, which can be implemented in hardware and / or software. This device can be configured in an electronic device, for example, a robot controller. Figure 1 As shown, the method specifically includes the following steps:

[0043] S110. Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius.

[0044] In this context, waypoints can be key points in robot trajectory planning; waypoint location information can be the 3D coordinates of the key points; waypoint pose information can be the pose of the key points, which can be represented by Euler angles or rotation matrices, without specific restrictions. The point fusion radius refers to a parameter controlling the width of the transition region in trajectory planning, and can be preset. Transition points refer to auxiliary points generated by offsetting the waypoint normal direction.

[0045] Specifically, the location information of three waypoints can be obtained as waypoint location information, and the attitude information can be obtained as waypoint attitude information. Based on the waypoint location information of the three waypoints and the point fusion radius, the two transition points corresponding to the transition area and the location and attitude information of the two transition points can be determined, namely the transition point location information and the transition point attitude information.

[0046] In some embodiments, at least three waypoints include one intermediate point and at least two non-intermediate points. Determining the transition point position information and transition point attitude information of at least two transition points corresponding to the waypoints based on waypoint position information and point fusion radius may include: determining the tangent normal direction vector from each non-intermediate point to the intermediate point based on waypoint position information; and determining the transition point position information and transition point attitude information of two transition points on both sides of the intermediate point based on the tangent normal direction vector and point fusion radius.

[0047] In this context, the middle point refers to the waypoint located in the middle of the three waypoints, while the non-middle points refer to the two waypoints located at either end. This explanation clarifies the distinction between middle and non-middle points when there are three waypoints; a similar distinction can be made for any three waypoints.

[0048] Specifically, when determining the relevant information of two transition points, the tangent normal direction vectors pointing from each non-intermediate point to the intermediate point can be calculated based on the waypoint location information. Then, using the calculated tangent normal direction vectors as a basis, and combined with a pre-set point fusion radius, the transition point location information and attitude information of the two transition points on either side of the intermediate point can be determined. After obtaining the transition point location and attitude information, the tangent normal information can be used to fit the fused transition trajectory of the intermediate segment using a cubic spline curve. By solving for the coefficients of the spline curve, the curve equation can be constructed, thus obtaining a smooth transition trajectory. For example... Figure 2 The figure shown is a fusion transition trajectory diagram of the fitting intermediate segment provided in an embodiment of the present invention.

[0049] S120. Based on waypoint location information and transition point location information, determine the location path length between any two adjacent points among waypoints and transition points, and generate a location path length queue based on the location path length.

[0050] Among them, the location path length refers to the length of the location path between two adjacent points; the location path length queue refers to a queue composed of multiple location path lengths.

[0051] Continue to refer to Figure 2 The path length of each segment in Cartesian space can be calculated piecewise. ,in waypoints Interval length, For fusion segment Spline path length, taking this as an example, can be used to obtain a unidirectional increasing queue of position path lengths. For each location path length, such as waypoints... Length between The path is a straight line, and the distance between two points can be calculated using the formula for distance between two points.

[0052] In some embodiments, determining the location path length between any two adjacent points among waypoints and transition points based on waypoint location information and transition point location information includes: when both adjacent points are transition points, determining the spline path length between the two transition points, and using the spline path length as the location path length.

[0053] Understandably, if two adjacent points are both transition points, meaning the path between them can be a spline curve segment, then its length can be calculated to obtain the spline path length. For example, numerical integration methods, such as Simpson's rule, can be used to discretize the curve into multiple small segments, and then the lengths of all the small segments can be summed.

[0054] S130. Convert waypoint attitude information and transition point attitude information into unit quaternions, determine the attitude path length between adjacent points based on the unit quaternions, and generate an attitude path length queue based on the attitude path length.

[0055] The waypoint attitude information and transition point attitude information can be in various forms such as axis angles and Euler angles.

[0056] Specifically, to generate the attitude path length queue, waypoint attitude information can be represented using unit quaternions. The form is represented as follows. Furthermore, the attitude path length between adjacent points is determined based on the unit quaternion, and an attitude path length queue is generated based on the attitude path length.

[0057] In some embodiments, determining the attitude path length between adjacent points based on unit quaternions includes: converting the unit quaternion into hypersphere points on a unit hypersphere, and determining the distance between the hypersphere points as the attitude path length between adjacent points.

[0058] Understandably, unit quaternions are used to represent rotational attitude in three-dimensional space. These unit quaternions can be transformed onto a unit hypersphere, becoming hypersphere points. The distance between hypersphere points on the unit hypersphere reflects the degree of attitude change at adjacent points. Therefore, by calculating the distance between hypersphere points and using it as the attitude path length between adjacent points, we can measure the distance traveled from one point to another in terms of attitude change.

[0059] For example, using unit quaternions as a descriptive method Quaternions can be converted into points on a four-dimensional unit hypersphere, such as... Figure 3 The diagram shown is a schematic representation of a unit hypersphere provided in an embodiment of the present invention. Figure 3 The waypoints are shown in the middle. , , Corresponding attitude, transition point , Corresponding posture.

[0060] Furthermore, the distance between two adjacent attitudes on the hypersphere is calculated piecewise, which gives the attitude path length. ,in for posture Interval length, For attitude transition and fusion segment Spline path length, taking this as an example, can be used to obtain a unidirectional increasing queue of attitude path lengths. .

[0061] S140. Establish the position and attitude correspondence based on the position path length queue and the attitude path length queue.

[0062] The position-attitude correspondence is used to represent the relationship between position changes and attitude changes. This correspondence includes a two-dimensional array and a two-dimensional curve. The two-dimensional array stores the corresponding data of position and attitude in a discrete manner, while the two-dimensional curve displays the trend of position and attitude changes in a continuous graphical manner.

[0063] S150: Receive the input velocity information and calculate the positional degree of freedom motion rate based on the velocity information and the positional path length.

[0064] In this embodiment of the invention, the speed information can be input by a user. Based on the input speed information and the position path length, the robot's motion rate in the positional degrees of freedom is planned. The speed information represents the overall required speed of the robot's movement. Combined with the position path length, the robot's movement speed on different path segments can be reasonably allocated, ensuring that the robot can move smoothly along the path according to the expected speed requirements.

[0065] S160. Based on the correspondence between the current time, the motion rate of the position degrees of freedom, and the position and attitude, determine the Cartesian space position and Cartesian space attitude corresponding to the current time.

[0066] Specifically, the process involves obtaining the current time value, then calculating the length of the position path the robot has already traversed at that moment based on the position degree of freedom and the current time. Next, the corresponding attitude path length is found in the position-attitude correspondence. Finally, based on the position path length and attitude path length, combined with the initial waypoint positions and attitude information, the robot's specific position coordinates and attitude angle in Cartesian space at the current moment can be determined. This process can be used to determine the corresponding Cartesian space position and attitude for any given moment, thus achieving multi-point path planning.

[0067] In an optional embodiment, the method further includes: upon receiving a new waypoint, determining at least two new transition points corresponding to the new waypoint; calculating new transition position information and new transition attitude information for the two new transition points; and, based on the new transition position information, new transition attitude information, new waypoint and waypoint position information and waypoint attitude information corresponding to at least three waypoints, re-executing the processes of generating a position path length queue, generating an attitude path length queue, establishing a position attitude correspondence, determining the position degree of freedom motion rate, and determining the Cartesian space position and Cartesian space attitude corresponding to the current moment.

[0068] In some practical scenarios, when a new waypoint is received, in order to ensure that path planning continues to operate reasonably and accurately, it is necessary to generate new transition points related to the new waypoint and obtain their corresponding position and orientation information. Then, the new waypoint and its corresponding new transition point information must be integrated into the existing path planning system, and a series of key steps must be recalculated, including generating a queue of position and orientation path lengths, establishing the correspondence between position and orientation, determining the motion rate of the position degrees of freedom, and finally re-determining the robot's current position and orientation in Cartesian space. This ensures that path planning can adapt to the changes brought about by the new waypoint, enabling the robot to move accurately according to the new path requirements.

[0069] For example, if a new waypoint is added to the trajectory pool, such as Figure 2 middle Points can be recalculated for fusion segments. Update the two-dimensional path queue based on the corresponding position, attitude, and path length, according to the current position and path length. and the updated total path length The motion rate of the positional degrees of freedom is recalculated using a planning method. Proceed to step S130, execute the subsequent steps again, and finally complete the planning. The continuous path planning ends.

[0070] The technical solution of this invention first acquires the position and attitude information of at least three waypoints, thereby determining the position and attitude information of at least two transition points. Next, it determines the position path length between adjacent points among the waypoints and transition points and generates a queue. The attitude information is then converted into unit quaternions to determine the attitude path length and generate another queue. Subsequently, a position-attitude correspondence is established based on these two queues, including a two-dimensional array and a two-dimensional curve. Then, input velocity information is received, and the motion rate of the position degrees of freedom is planned. Finally, based on the current time, the motion rate of the position degrees of freedom, and the position-attitude correspondence, the Cartesian space position and attitude corresponding to the current time are determined. This method establishes a position-attitude correspondence based on two queues, enabling the correlation and synchronous planning of position and attitude changes. By planning the motion rate of the position degrees of freedom upon receiving velocity information and determining the Cartesian space position and attitude based on the current time and the position-attitude correspondence, the continuity of motion planning is ensured. The Cartesian space attitude can be obtained based on the position-attitude correspondence, reducing the computational complexity of multi-dimensional solutions and ensuring the phase consistency of position and attitude planning, thus avoiding the problem of asynchronous pose planning.

[0071] Figure 4 This is a flowchart of another multi-path continuous pose synchronization planning method provided by an embodiment of the present invention. Based on the above embodiment, this embodiment further optimizes the correspondence between position and pose. For non-fused segments, a two-dimensional array can be established, and for fused segments, a two-dimensional curve can be established. Figure 4 As shown, the method specifically includes the following steps:

[0072] S210. Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius.

[0073] S220. Based on waypoint location information and transition point location information, determine the location path length between any two adjacent points among waypoints and transition points, and generate a location path length queue based on the location path length.

[0074] S230. Convert waypoint attitude information and transition point attitude information into unit quaternions, convert unit quaternions into hypersphere points on the unit hypersphere, determine the distance between hypersphere points as the attitude path length between adjacent points, and generate an attitude path length queue based on the attitude path length.

[0075] S240. For any two adjacent points between waypoints and transition points, if the road segment is a non-fusion segment, a two-dimensional array is established with the position path length queue as the horizontal axis and the attitude path length queue as the vertical axis, which serves as the position and attitude correspondence of the non-fusion segment.

[0076] In path planning, the path formed by waypoints and transition points will have segments with different properties. Non-fusion segments refer to those path segments that do not require special smooth transition processing. The position and pose correspondence of non-fusion segments can be represented by a two-dimensional array.

[0077] Specifically, a coordinate system is constructed with the position path length queue as the horizontal axis and the attitude path length queue as the vertical axis. The position path length and attitude path length are stored as elements in a two-dimensional array. For example... Figure 5 The diagram shown is a schematic representation of the position and orientation relationship provided in an embodiment of the present invention.

[0078] S250. If the road segment is a fusion segment, then establish the corresponding two-dimensional curve of the fusion segment based on splines or polynomials, and use the two-dimensional curve as the position and attitude correspondence of the fusion segment.

[0079] The fusion segment is the region in the path that requires smooth transition processing. Position and attitude changes need to be more continuous and natural to avoid abrupt changes and shocks during motion. Using splines or polynomials to create the corresponding two-dimensional curve for the fusion segment can describe the relationship between position and attitude changes in a mathematically continuous and smooth manner.

[0080] Specifically, select appropriate spline or polynomial functions, such as cubic spline functions or quintic polynomial functions. Using the position path length as the independent variable and the attitude path length as the dependent variable, determine the parameters of the spline or polynomial function through interpolation or fitting using the position and attitude path length data of adjacent points within the fused segment. For example, for cubic spline interpolation, solve a series of linear equations to determine the spline function coefficients for each sub-interval, ultimately obtaining a two-dimensional curve that accurately describes the correspondence between position and attitude in the fused segment.

[0081] S260: Receive the input velocity information, determine the total path length based on the position path length, and use the S-curve planning method to plan the position degree of freedom motion rate based on the maximum velocity, acceleration, jerk and total path length.

[0082] Specifically, the total path length can be calculated. Given the input commands for maximum speed, acceleration, and jerk, an S-curve programming method is used to plan the motion rate of the positional degrees of freedom. .

[0083] S270. Based on the correspondence between the current time, the motion rate of the position degrees of freedom, and the position and attitude, determine the Cartesian space position and Cartesian space attitude corresponding to the current time.

[0084] The technical solution of this invention first acquires the position and attitude information of at least three waypoints, thereby determining the position and attitude information of at least two transition points. Next, it determines the position path length between adjacent points among the waypoints and transition points and generates a queue. The attitude information is then converted into unit quaternions to determine the attitude path length and generate another queue. Subsequently, a position-attitude correspondence is established based on these two queues, including a two-dimensional array and a two-dimensional curve. Then, input velocity information is received, and the motion rate of the position degrees of freedom is planned. Finally, based on the current time, the motion rate of the position degrees of freedom, and the position-attitude correspondence, the Cartesian space position and attitude corresponding to the current time are determined. This method establishes a position-attitude correspondence based on two queues, enabling the correlation and synchronous planning of position and attitude changes. By planning the motion rate of the position degrees of freedom upon receiving velocity information and determining the Cartesian space position and attitude based on the current time and the position-attitude correspondence, the continuity of motion planning is ensured. The Cartesian space attitude can be obtained based on the position-attitude correspondence, reducing the computational complexity of multi-dimensional solutions and ensuring the phase consistency of position and attitude planning, thus avoiding the problem of asynchronous pose planning.

[0085] Figure 6 This is a flowchart illustrating another multi-path point continuous pose synchronization planning method provided by an embodiment of the present invention. Based on the above embodiments, this embodiment further optimizes the process of determining Cartesian space position and Cartesian space attitude. Specific implementation details can be found in the technical solution of this embodiment. Technical terms that are the same as or corresponding to those in the above embodiments will not be repeated here. Figure 6 As shown, the method specifically includes the following steps:

[0086] S300: Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius.

[0087] S310. Based on waypoint location information and transition point location information, determine the location path length between any two adjacent points among waypoints and transition points, and generate a location path length queue based on the location path length.

[0088] S320. Convert waypoint attitude information and transition point attitude information into unit quaternions, determine the attitude path length between adjacent points based on the unit quaternions, and generate an attitude path length queue based on the attitude path length.

[0089] S330. Establish the position and attitude correspondence based on the position path length queue and the attitude path length queue.

[0090] S340 receives the input velocity information and plans the positional degree of freedom motion rate based on the velocity information and the positional path length.

[0091] S350. Determine the path length of the current position based on the current time and the motion rate of the degrees of freedom of the current position, and determine the target position segment corresponding to the current time based on the path length of the current position.

[0092] The current position path length can be understood as the position path of the robot at the current moment. During the robot's movement along the planned path, it is necessary to determine the specific position segment of the path, i.e., the target position segment, of the robot at the current moment based on the current moment and the pre-planned position degree of freedom motion rate.

[0093] Specifically, based on the motion rate of the current position degrees of freedom, the length of the position path traversed by the robot from the start of the motion to the current moment can be calculated and denoted as the current position path length. Then, the position path length queue is traversed, and the current position path length is compared one by one with the position path lengths between adjacent points in the queue. The interval that satisfies the condition that the current position path length lies between the position path lengths of two adjacent points is found and taken as the target position segment for the current moment. For example, based on the current time... The path length to the current location can be calculated. .

[0094] S360. Determine the location path length-location relationship corresponding to the target location segment, and determine the Cartesian space location corresponding to the current moment based on the location path length-location relationship and the current location path length.

[0095] It should be noted that after determining the target position segment at the current moment, it is necessary to further determine the robot's specific position coordinates within that position segment, i.e., its position in Cartesian space.

[0096] Specifically, first determine the position path length-position relationship corresponding to the target position segment. The position path length-position relationship can differ for different position segments. If the target position segment is a non-fused segment, the position and attitude correspondence is represented using a two-dimensional array. In this case, the Cartesian space position at the current moment can be calculated using linear interpolation based on the position coordinates of the two endpoints of the target position segment and the ratio of the current position path length to the endpoint position path length.

[0097] If the target location segment is a fusion segment, the aforementioned steps established a two-dimensional curve corresponding to the fusion segment as a position and attitude correspondence based on splines or polynomials. At this point, it is necessary to calculate the corresponding Cartesian space position by substituting the current position path length into the function according to the spline or polynomial function expression. .

[0098] S370. Determine the current attitude path length based on the current position path length and the correspondence between position and attitude, and determine the target attitude segment corresponding to the current moment based on the current attitude path length.

[0099] It should be understood that the robot's posture is constantly changing during movement and is related to changes in position. After determining the target position segment at the current moment, it is necessary to further determine the corresponding posture segment in order to subsequently determine the robot's accurate posture at that moment.

[0100] Specifically, based on the current position path length determined in S350 and the position-attitude correspondence established in S330, the attitude path length corresponding to the current position path length is found. Then, the attitude path length queue generated in S320 is traversed, and the attitude path length is compared one by one with the attitude path lengths between adjacent points in the queue. The interval that satisfies the condition that the attitude path length lies between the attitude path lengths of two adjacent points is found, and the interval corresponding to this interval is the target attitude segment at the current moment.

[0101] S380. Determine the attitude path length-attitude relationship corresponding to the target attitude segment, and determine the Cartesian space attitude corresponding to the current moment based on the attitude path length-attitude relationship and the current attitude path length.

[0102] Specifically, the attitude path length-attitude relationship corresponding to the target attitude segment can be determined. Substituting the current attitude path length into the attitude path length-attitude relationship corresponding to the target attitude segment yields the Cartesian space attitude at the current moment.

[0103] For example, the path length at the current location is known. The current attitude path length can be obtained based on the two-dimensional correspondence between position and attitude path length. Then, based on the correspondence between the attitude path length and the attitude, the current Cartesian space attitude quaternion is calculated. .like Figure 7 The diagram shown is a flowchart of a multi-path point continuous pose synchronization planning method provided by an optional embodiment of the present invention.

[0104] The embodiments of the present invention include at least the following technical effects:

[0105] 1) This invention improves upon point-to-point planning by calculating the two-dimensional path long queues for position and attitude degrees of freedom, respectively, thereby obtaining a unique position-attitude correspondence at each moment. Based on this, it calculates continuous, compliant Cartesian space point planning at each moment with a strict phase relationship, and this phase relationship is not affected by motion parameters such as velocity and acceleration.

[0106] 2) Compared with the two-point proportional synchronization method, the present invention avoids the time loss caused by frequent acceleration and deceleration during the task, the running curve is more stable, the running efficiency and stability are improved, and it supports online updating of the subsequent waypoint queue.

[0107] 3) Compared with polynomial fitting, the path controllability of the present invention is higher, the position and attitude have a strict phase relationship that is not affected by motion parameters, and only a single degree of freedom of position is required for motion rate planning, which reduces computational complexity.

[0108] Figure 8 This is a schematic diagram of a multi-path continuous pose synchronization planning device provided in an embodiment of the present invention. Figure 8 As shown, the device includes:

[0109] The attitude information determination module 410 is used to acquire waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine the transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and the point fusion radius.

[0110] The location queue generation module 420 is used to determine the location path length between any two adjacent points among waypoints and transition points based on waypoint location information and transition point location information, and to generate a location path length queue based on the location path length.

[0111] The attitude queue generation module 430 is used to convert waypoint attitude information and transition point attitude information into unit quaternions, determine the attitude path length between adjacent points based on the unit quaternions, and generate an attitude path length queue based on the attitude path length.

[0112] The correspondence establishment module 440 is used to establish position and attitude correspondence based on the position path length queue and the attitude path length queue, wherein the position and attitude correspondence includes a two-dimensional array and a two-dimensional curve;

[0113] The degree-of-freedom motion rate determination module 450 is used to receive input velocity information and plan the position degree-of-freedom motion rate based on the velocity information and the position path length;

[0114] The current pose determination module 460 is used to determine the Cartesian space position and Cartesian space pose corresponding to the current moment based on the current time, the motion rate of the position degrees of freedom and the correspondence between position and pose.

[0115] In some alternative embodiments, the at least three waypoints include one intermediate point and at least two non-intermediate points, and the attitude information determination module 410 is specifically used for:

[0116] Determine the tangent direction vector from each non-intermediate point to the intermediate point based on waypoint location information;

[0117] The transition point position information and transition point attitude information of the two transition points on both sides of the intermediate point are determined based on the tangent normal direction vector and the point fusion radius.

[0118] In some alternative embodiments, the location queue generation module 420 is specifically used for:

[0119] When two adjacent points are both transition points, determine the spline path length between the two transition points and use the spline path length as the location path length.

[0120] In some alternative embodiments, the attitude queue generation module 430 is specifically used for:

[0121] Convert the unit quaternion into a hypersphere point on the unit hypersphere, and determine the distance between the hypersphere points as the attitude path length between adjacent points.

[0122] In some optional embodiments, the correspondence establishment module 440 is specifically used for:

[0123] For any two adjacent points between waypoints and transition points, if the road segment is a non-fusion segment, a two-dimensional array is established with the position path length queue as the horizontal axis and the attitude path length queue as the vertical axis, which serves as the position and attitude correspondence of the non-fusion segment.

[0124] If the road segment is a fusion segment, then a two-dimensional curve corresponding to the fusion segment is established based on splines or polynomials, and the two-dimensional curve is used as the position and attitude correspondence of the fusion segment.

[0125] In some optional embodiments, the velocity information includes maximum velocity, acceleration, and jerk; the degree-of-freedom motion rate determination module 450 is specifically used for:

[0126] The total path length is determined based on the position path length. Based on the maximum velocity, acceleration, jerk and total path length, the motion rate of the position degrees of freedom is planned using the S-curve programming method.

[0127] In some optional embodiments, the current pose determination module 460 is specifically used for:

[0128] The path length at the current position is determined based on the current time and the motion rate of the degrees of freedom at the current position, and the target position segment corresponding to the current time is determined based on the path length at the current position.

[0129] Determine the position path length-position relationship corresponding to the target position segment, and determine the Cartesian space position at the current moment based on the position path length-position relationship and the current position path length;

[0130] The current attitude path length is determined based on the correspondence between the current position path length and the position attitude, and the target attitude segment corresponding to the current moment is determined based on the current attitude path length.

[0131] Determine the attitude path length-attitude relationship corresponding to the target attitude segment, and determine the Cartesian space attitude corresponding to the current moment based on the attitude path length-attitude relationship and the current attitude path length.

[0132] In some alternative embodiments, the apparatus further includes a waypoint addition module for:

[0133] Upon receiving a new waypoint, determine at least two new transition points corresponding to the new waypoint;

[0134] Calculate the new transition position information and new transition attitude information of the two newly added transition points, and based on the new transition position information, new transition attitude information, new waypoints and waypoint position information and waypoint attitude information corresponding to at least three waypoints, re-execute the process of generating the position path length queue, generating the attitude path length queue, establishing the position attitude correspondence, determining the position degree of freedom motion rate, and determining the Cartesian space position and Cartesian space attitude corresponding to the current moment.

[0135] The multi-path continuous pose synchronization planning device provided in the embodiments of the present invention can execute the multi-path continuous pose synchronization planning method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the method.

[0136] Figure 9 This is a schematic diagram of an electronic device for implementing the multi-path continuous pose synchronization planning method of this invention. The electronic device is intended to represent various forms of digital computers, such as laptops, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframes, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices (e.g., helmets, glasses, watches), and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0137] like Figure 9As shown, the electronic device 10 includes at least one processor 11 and a memory, such as a read-only memory (ROM) 12 or a random access memory (RAM) 13, communicatively connected to the at least one processor 11. The memory stores computer programs executable by the at least one processor. The processor 11 can perform various appropriate actions and processes based on the computer program stored in the ROM 12 or loaded from storage unit 18 into the RAM 13. The RAM 13 can also store various programs and data required for the operation of the electronic device 10. The processor 11, ROM 12, and RAM 13 are interconnected via a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0138] Multiple components in electronic device 10 are connected to I / O interface 15, including: input unit 16, such as keyboard, mouse, etc.; output unit 17, such as various types of displays, speakers, etc.; storage unit 18, such as disk, optical disk, etc.; and communication unit 19, such as network card, modem, wireless transceiver, etc. Communication unit 19 allows electronic device 10 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0139] Processor 11 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, digital signal processors (DSPs), and any suitable processor, controller, microcontroller, etc. Processor 11 performs the various methods and processes described above, such as multi-waypoint continuous pose synchronization planning methods.

[0140] In some embodiments, the multipath point continuous pose synchronization planning method can be implemented as a computer program tangibly contained in a computer-readable storage medium, such as storage unit 18. In some embodiments, part or all of the computer program can be loaded and / or installed on electronic device 10 via ROM 12 and / or communication unit 19. When the computer program is loaded into RAM 13 and executed by processor 11, one or more steps of the multipath point continuous pose synchronization planning method described above can be performed. Alternatively, in other embodiments, processor 11 can be configured to perform the multipath point continuous pose synchronization planning method by any other suitable means (e.g., by means of firmware).

[0141] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0142] Computer programs used to implement the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, such that when executed by the processor, the computer programs cause the functions / operations specified in the flowcharts and / or block diagrams to be performed. The computer programs may be executed entirely on a machine, partially on a machine, or as a standalone software package, partially on a machine and partially on a remote machine, or entirely on a remote machine or server.

[0143] In the context of this invention, a computer-readable storage medium can be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, apparatus, or device. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination thereof. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof.

[0144] To provide interaction with a user, the systems and techniques described herein can be implemented on an electronic device having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the electronic device. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0145] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as data servers), or middleware components (e.g., application servers), or frontend components (e.g., user computers with graphical user interfaces or web browsers through which users can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., communication networks). Examples of communication networks include local area networks (LANs), wide area networks (WANs), blockchain networks, and the Internet.

[0146] A computing system can include clients and servers. Clients and servers are generally located far apart and typically interact through communication networks. The client-server relationship is created by computer programs running on the respective computers and having a client-server relationship with each other. The server can be a cloud server, also known as a cloud computing server or cloud host, which is a hosting product within the cloud computing service system to address the shortcomings of traditional physical hosts and VPS services, such as high management difficulty and weak business scalability.

[0147] It should be understood that the various forms of processes shown above can be used, with steps reordered, added, or deleted. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution of this invention can be achieved, and this is not limited herein.

[0148] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A multi-path continuous pose synchronization planning method, characterized in that, include: Obtain waypoint location information and waypoint attitude information corresponding to at least three waypoints, and determine transition point location information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint location information and point fusion radius; Based on the waypoint location information and the transition point location information, determine the location path length between any two adjacent points among the waypoints and transition points, and generate a location path length queue based on the location path length. The waypoint attitude information and the transition point attitude information are converted into unit quaternions. The attitude path length between adjacent points is determined based on the unit quaternions, and an attitude path length queue is generated based on the attitude path length. A position-attitude correspondence is established based on the position path length queue and the attitude path length queue, wherein the position-attitude correspondence includes a two-dimensional array and a two-dimensional curve; Receive input velocity information, and plan the positional degree of freedom motion rate based on the velocity information and the positional path length; Based on the current moment, the motion rate of the stated position degrees of freedom, and the correspondence between the stated position and attitude, determine the Cartesian space position and Cartesian space attitude corresponding to the current moment.

2. The method according to claim 1, characterized in that, At least three waypoints include one intermediate point and at least two non-intermediate points. The step of determining the transition point position information and transition point attitude information of at least two transition points corresponding to the waypoints based on the waypoint position information and the point fusion radius includes: Based on the waypoint location information, determine the tangent normal direction vector from each of the non-intermediate points to the intermediate point; Based on the tangent normal direction vector and the point fusion radius, the transition point position information and transition point attitude information of the two transition points on both sides of the intermediate point are determined.

3. The method according to claim 1, characterized in that, The step of determining the positional path length between any two adjacent points among the waypoints and transition points based on the waypoint location information and the transition point location information includes: When two adjacent points are both transition points, the spline path length between the two transition points is determined, and the spline path length is used as the position path length.

4. The method according to claim 1, characterized in that, Determining the attitude path length between adjacent points based on the unit quaternion includes: The unit quaternion is converted into a hypersphere point on the unit hypersphere, and the distance between the hypersphere points is determined as the attitude path length between the adjacent points.

5. The method according to claim 1, characterized in that, The step of establishing a position-attitude correspondence based on the position path length queue and the attitude path length queue includes: For any two adjacent points between the waypoints and transition points, if the road segment is a non-fusion segment, then a two-dimensional array is established with the position path length queue as the horizontal axis and the attitude path length queue as the vertical axis, as the position and attitude correspondence of the non-fusion segment; If the road segment is a fusion segment, then the two-dimensional curve corresponding to the fusion segment is established based on splines or polynomials, and the two-dimensional curve is used as the position and attitude correspondence of the fusion segment.

6. The method according to claim 1, characterized in that, The velocity information includes maximum velocity, acceleration, and jerk; the calculation of the positional degree of freedom motion rate based on the velocity information and the positional path length includes: The total path length is determined based on the position path length. Based on the maximum speed, the acceleration, the jerk, and the total path length, the motion rate of the position degrees of freedom is planned using the S-curve planning method.

7. The method according to claim 1, characterized in that, The step of determining the Cartesian space position and Cartesian space attitude corresponding to the current time based on the correspondence between the current time, the motion rate of the position degrees of freedom, and the position attitude includes: The path length at the current position is determined based on the current time and the motion rate of the current position degrees of freedom, and the target position segment corresponding to the current time is determined based on the path length at the current position; Determine the location path length-location relationship corresponding to the target location segment, and determine the Cartesian space location corresponding to the current time based on the location path length-location relationship and the current location path length; The current attitude path length is determined based on the correspondence between the current position path length and the position attitude, and the target attitude segment corresponding to the current moment is determined based on the current attitude path length. Determine the attitude path length-attitude relationship corresponding to the target attitude segment, and determine the Cartesian space attitude corresponding to the current moment based on the attitude path length-attitude relationship and the current attitude path length.

8. The method according to claim 1, characterized in that, The method further includes: Upon receiving a new waypoint, at least two new transition points corresponding to the new waypoint are determined; Calculate the new transition position information and new transition attitude information of the two newly added transition points, and based on the new transition position information, the new transition attitude information, the new waypoints and the waypoint position information and waypoint attitude information corresponding to at least three waypoints, re-execute the process of generating a position path length queue, generating an attitude path length queue, establishing a position attitude correspondence, determining the position degree of freedom motion rate, and determining the Cartesian space position and Cartesian space attitude corresponding to the current moment.

9. An electronic device, characterized in that, The electronic device includes: At least one processor; and a memory communicatively connected to the at least one processor; The memory stores a computer program that can be executed by the at least one processor, and the computer program is executed by the at least one processor to enable the at least one processor to perform the multi-way point continuous pose synchronization planning method according to any one of claims 1-8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions that cause a processor to execute the multi-path point continuous pose synchronization planning method according to any one of claims 1-8.

Images