Online trajectory planning attitude interpolation method and system based on Lie group Lie algebra

Abstract

The invention discloses an online trajectory planning attitude interpolation method and system based on Lie group Lie algebra, and the method comprises the steps: obtaining first initial pose information and first target pose information of the tail end of a mechanical arm, each of the first initial pose information and the first target pose information comprises position vector information under a base coordinate system and pose description information obtained by Lie group-Lie algebra conversion; based on the first initial pose information and the first target pose information, on-line trajectory planning is adopted to generate control information of a plurality of first interpolation points of the tail end of the mechanical arm moving from the first initial pose to the first target pose; and according to the control information of the tail end of the mechanical arm at the first interpolation points, the mechanical arm is controlled to move from the first initial pose to the first target pose. The rotation matrix of the tail end of the mechanical arm is converted into posture description in the Lie algebra space, and the position and the posture track which are reached at the same time are generated through online track planning; and when the target pose is changed, re-planning can be carried out according to the parameters at the current moment.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm control, and in particular to an online trajectory planning posture interpolation method and system based on Lie group and Lie algebra. Background Art

[0002] Attitude interpolation is a common technology in the fields of robotics, drones, computer animation, and virtual reality, which aims to achieve a smooth transition between two known attitudes. The axis-angle method, quaternion, rotation matrix, and Lie group algebra (SO3) are usually used to represent attitudes, because these methods can effectively avoid the universal deadlock problem and achieve smooth attitude transition. In some application scenarios, not only a smooth transition of attitude is required, but also the rotation speed needs to be considered. In the online trajectory planning (OTG) of the robot arm, the starting speed is not zero, which will cause the planning to be affected by the starting speed. For example, when the robot arm rotates forward around the z-axis, the rotation direction of the robot arm is changed to rotate counterclockwise around the z-axis. At this time, the attitude of the robot arm needs to be reduced to zero based on the current attitude speed and then continue to rotate counterclockwise around the z-axis from zero. Therefore, the attitude interpolation technology with speed needs to ensure that the attitude interpolation is smooth and continuous to avoid abrupt changes. At present, the common methods of attitude interpolation technology include linear interpolation, spherical linear interpolation, and spherical spline interpolation. The attitude description method usually uses Euler angles, quaternions, SO3, and rotation matrices. However, the above-mentioned method of representing posture also has the following shortcomings: Euler angle posture interpolation is prone to universal deadlock and cannot guarantee the optimal posture interpolation; quaternion posture interpolation usually uses the interpolation of the angle of rotation around the axis when the axis is fixed. When the initial posture speed is not zero or the axis changes after the target posture is changed during the posture change, it is impossible to describe the relationship between the angle and the axis; the rotation matrix represents redundant posture parameters and cannot realize posture interpolation; when the axis-angle method posture interpolation is used, the axis cannot be calculated when the angle is zero or Π. Summary of the invention

[0003] The purpose of an embodiment of the present invention is to provide an online trajectory planning posture interpolation method and system based on Lie group Lie algebra. By converting the rotation matrix of the end of the robot arm into a posture description in the Lie algebra space, it is possible to accurately represent the posture information, provide accurate basic data for subsequent trajectory planning and motion control, and improve the precision of the robot arm posture description; convert the homogeneous transformation matrix into a one-dimensional matrix for describing the position and posture, and use online trajectory planning to generate a position and posture trajectory that is reached simultaneously. When the target posture changes, it can be re-planned with the current parameters, thereby enhancing the flexibility of the robot arm trajectory planning to cope with changes and ensuring the continuity of the motion.

[0004] To solve the above technical problems, a first aspect of an embodiment of the present invention provides an online trajectory planning posture interpolation method based on Lie group Lie algebra, comprising the following steps:

[0005] Acquire first initial posture information and first target posture information of the end of the robotic arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation;

[0006] Based on the first initial posture information and the first target posture information, online trajectory planning is used to generate control information of a plurality of first interpolation points of the end of the manipulator moving from the first initial posture to the first target posture, wherein the control information includes: position information, velocity information and acceleration information;

[0007] According to the control information of the end of the robot arm at the plurality of first interpolation points, the robot arm is controlled to move from the first initial posture to the first target posture.

[0008] Furthermore, the first initial posture of the end of the robotic arm is a stationary state or a non-stationary state.

[0009] Furthermore, the online trajectory planning posture interpolation method based on Lie group Lie algebra also includes:

[0010] During the process of the end of the robotic arm moving from the first initial posture to the first target posture, receiving second target posture information different from the first target posture;

[0011] Obtaining real-time position information of the end of the robotic arm;

[0012] The real-time posture of the end of the robot arm is used as the second initial posture, and the path of the end of the robot arm moving from the second initial posture to the second target posture is re-planned online through Lie group and Lie algebra transformation to obtain control information of a plurality of second interpolation points of the end of the robot arm moving from the real-time posture to the second target posture;

[0013] According to the control information of the end of the robot arm at the plurality of second interpolation points, the robot arm is controlled to move from the second initial posture to the second target posture.

[0014] Further, the obtaining of the first initial posture information and the first target posture information of the end of the robot arm includes:

[0015] Obtaining a rotation matrix of the first initial pose and a rotation matrix of the first target pose respectively;

[0016] According to the following formula, the rotation matrix is ​​transformed by Lie group and Lie algebra to obtain the posture description information in Lie algebra space;

[0017]

[0018] Where R is a 3×3 rotation matrix, tr(R) is the trace of the matrix, a is a 3×1 matrix used to describe the rotation axis, and θ is the angle of rotation around the rotation axis a.

[0019] Furthermore, converting the rotation matrix into posture description information in Lie algebra space according to the following formula includes:

[0020] Obtaining first posture description information and second posture description information in a Lie algebra space based on Lie group and Lie algebra transformation, and a first moving path and a second moving path corresponding to the first posture description information and the second posture description information respectively;

[0021] A moving path corresponding to the posture description information that is closest to the end point of the first initial posture is selected as a preferred moving path, and control information of several first interpolation points of the preferred moving path is generated based on online trajectory planning to control the end of the robotic arm to move from the first initial posture to the first target posture.

[0022] Furthermore, the calculation formula for calculating the posture description information closest to the first initial posture endpoint is as follows:

[0023] φ2=min(||φ1-φ2'2||2,||φ1-φ2″||2);

[0024] φ'2=θa;

[0025] φ2″=(2π-θ)(-a);

[0026] Among them, φ2 is the posture description information corresponding to the preferred moving path, φ1 is the first initial posture, φ′2 is the first posture description information rotated around the rotation axis a by an angle θ, and φ″2 is the second posture description information rotated around the rotation axis a by an angle (2π-θ).

[0027] Further, the selecting a moving path corresponding to the posture description information that is closest to the first initial posture endpoint as the preferred moving path includes:

[0028] A moving path corresponding to a posture description mode with a smaller rotation angle value in the first posture description information and the second posture description information is selected as a preferred moving path.

[0029] Accordingly, a second aspect of an embodiment of the present invention provides an online trajectory planning posture interpolation system based on Lie group and Lie algebra, and a robotic arm is controlled based on the online trajectory planning posture interpolation method based on Lie group and Lie algebra, including:

[0030] An information acquisition module, which is used to acquire first initial posture information and first target posture information of the end of the robot arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation;

[0031] A trajectory planning module, which is used to generate control information of a plurality of first interpolation points of the end of the manipulator moving from the first initial posture to the first target posture by online trajectory planning based on the first initial posture information and the first target posture information, wherein the control information includes: position information, velocity information and acceleration information;

[0032] A motion control module is used to control the robot arm to move from the first initial posture to the first target posture according to the control information of the end of the robot arm at the first interpolation points.

[0033] Accordingly, a third aspect of an embodiment of the present invention provides an electronic device, comprising: at least one processor; and a memory connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the online trajectory planning posture interpolation method based on Lie group Lie algebra.

[0034] Correspondingly, a fourth aspect of an embodiment of the present invention provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the online trajectory planning posture interpolation method based on Lie group and Lie algebra.

[0035] The above technical solution of the embodiment of the present invention has the following beneficial technical effects:

[0036] 1. The rotation matrix at the end of the robot arm is converted into a posture description in the Lie algebra space, which can accurately represent the posture information, provide accurate basic data for subsequent trajectory planning and motion control, and improve the precision of the robot arm posture description;

[0037] 2. The homogeneous transformation matrix is ​​converted into a one-dimensional matrix for describing the position and posture, and the position and posture trajectory that are reached simultaneously are generated by online trajectory planning. When the target posture changes, the current parameters can be used for re-planning, which enhances the flexibility of the robot arm trajectory planning to cope with changes and ensures the continuity of the movement.

[0038] 3. Online trajectory planning uses the current speed and position as the starting values ​​to calculate the contour, and decouples the coupled posture through posture representation, achieving continuous and smooth position and speed during the movement of the robot arm, effectively improving the stability and controllability of the robot arm's movement process. The advantages can be seen from the interpolation effect and speed curve. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 It is a flow chart of an online trajectory planning posture interpolation method based on Lie group Lie algebra provided by an embodiment of the present invention;

[0040] Figure 2a The schematic diagram of the rotation principle of the end of the robot arm provided by the embodiment of the present invention is Figure 1 ;

[0041] Figure 2b This is a second schematic diagram of the rotation principle of the end of the robotic arm provided by an embodiment of the present invention;

[0042] Figure 2c The schematic diagram of the rotation principle of the end of the robot arm provided by the embodiment of the present invention is Figure 3 ;

[0043] Figure 3 is a schematic diagram of the rotation process of the end of the robotic arm provided by an embodiment of the present invention;

[0044] Figure 4 is a schematic diagram of optimizing the rotation posture of the end of a robotic arm provided by an embodiment of the present invention;

[0045] Figure 5 is a schematic diagram of real-time posture adjustment during movement of the end of a robotic arm provided by an embodiment of the present invention;

[0046] Figure 6 yes Figure 5 Schematic diagram of various speeds after real-time posture adjustment;

[0047] Figure 7 It is a module block diagram of an online trajectory planning attitude interpolation system based on Lie group and Lie algebra provided by an embodiment of the present invention.

[0048] Reference numerals:

[0049] 1. Information acquisition module, 2. Trajectory planning module, 3. Motion control module. DETAILED DESCRIPTION

[0050] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention is further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings. It should be understood that these descriptions are only exemplary and are not intended to limit the scope of the present invention. In addition, in the following description, the description of well-known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present invention.

[0051] Please refer to Figure 1 A first aspect of an embodiment of the present invention provides an online trajectory planning posture interpolation method based on Lie group Lie algebra, comprising the following steps:

[0052] Step S100, obtaining first initial posture information and first target posture information of the end of the robot arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation.

[0053] Step S200, based on the first initial posture information and the first target posture information, online trajectory planning is used to generate control information of several first interpolation points of the robot arm moving from the first initial posture to the first target posture, and the control information includes: position information, speed information and acceleration information.

[0054] Step S300, controlling the robot arm to move from a first initial posture to a first target posture according to control information of the end of the robot arm at a plurality of first interpolation points.

[0055] The online trajectory planning posture interpolation method based on Lie group and Lie algebra can accurately obtain the first initial posture information and the first target posture information of the end of the robot arm, where the posture information includes the position vector information in the base coordinate system and the posture description information obtained by the Lie group and Lie algebra transformation. Then, the online trajectory planning is used to generate the control information of several first interpolation points based on the initial and target posture information, including position, velocity and acceleration information. Finally, based on these control information, the robot arm can be accurately controlled to move from the first initial posture to the first target posture, thereby realizing efficient and accurate robot arm posture interpolation and trajectory control, and improving the motion accuracy and stability of the robot arm.

[0056] The above Lie group Lie algebra posture description method is explained as follows:

[0057] (1) Definition of a group: A group consists of a set of elements and an operation that combines the elements (such as addition or multiplication), and is closed, associative, has an identity element and an inverse element.

[0058] (2) Lie group (SO3): Lie group is a special group structure with a smooth manifold, which means it is continuous. A Lie group can be formed by a rotation matrix and a matrix multiplication operator, but the operator only defines multiplication, not addition, which makes it difficult to take limits and derivatives, and it is also difficult to implement interpolation.

[0059] (3) Lie algebra (so3): Lie algebra is often associated with Lie groups. It usually refers to the tangent space of a Lie group near its unit element, describing the local derivative of the Lie group. It can be understood that Lie algebra maps the elements of the Lie group (nonlinear space) to the linear space, making it easy to perform derivative and interpolation operations.

[0060] Mapping relationship:

[0061] The mapping from Lie algebra to Lie group adopts exponential mapping, and the mapping from Lie algebra to Lie group adopts logarithmic mapping. Let the Lie group composed of rotation matrices be SO(3) = {R∈R 3×3 |RR T =I, det(R) = 1}, and its corresponding Lie algebra is so(3) = {φ∈R 3 ,Φ=φ ^ ∈R 3×3},in

[0062]

[0063] Among them, φ is the posture description represented by the corresponding rotation matrix in the Lie group in the Lie algebra space, which can be written as φ = θa, θ is the angle of rotation, and a is the axis of rotation. For example, the rotation matrix corresponding to the base coordinate system rotating 90° along the z axis is The corresponding Lie algebra is described as The specific conversion formula is as follows:

[0064] SO(3) and SO(3) convert into each other:

[0065] The transformation from Lie group to Lie algebra is realized by exponential mapping. Let the element R in SO(3) correspond to the element φ in SO(3). The specific transformation process is as follows:

[0066] so(3)->SO(3):

[0067]

[0068] SO(3)->so(3):

[0069]

[0070] Optionally, the first initial posture of the end of the robot arm may be a stationary state or a non-stationary state in motion.

[0071] In one embodiment of the present invention, please refer to Figure 2a , Figure 2b and Figure 2c , Figure 2a is the posture of the robot in the initial state, Figure 2aIt can be seen that position 1 (corresponding to x1, y1, z1) coincides with the base coordinate system (x0, y0, z0), which is equivalent to saying that the current position of the robot arm (position 1) is at the origin of the posture. Figure 2b It is the posture of position 2 described by Lie algebra (coordinate system x2, y2, z2), that is, the posture of the target position. θ2 is the angle of rotation, and a2 is the axis of rotation. It can be interpreted that the coordinate system x1, y1, z1 can be rotated to the coordinate system 2 along the a2 axis by θ2. Figure 2c Similarly. Figure 2a , Figure 2b and Figure 2c It can be represented as coordinate system 1 at the origin, first moving to coordinate system 2, which is the target posture. During the movement, the target position is changed, and it is necessary to rotate to the new target posture with the current posture.

[0072] In the posture interpolation method, the initial posture of the end of the manipulator in space is expressed as R1 by SO(3), the intermediate posture is R2, and the target posture is R3. The end of the manipulator first rotates from the R1 posture to the R2 posture, and changes its posture to R3 during the rotation process (the end of the manipulator has not reached R2 during the movement, and has a certain rotation speed at this time). The postures expressed by SO(3) are φ1, φ2, and φ3 respectively. For the convenience of calculation, assuming that R1 is a unit matrix, φ1, φ2, and φ3 are expressed as follows:

[0073]

[0074] Please refer to Figure 3 , since the posture description of so(3) is described by the axis and the angle of rotation, the direction of the axis represents the direction of the axis, and the length of the axis is the angle of rotation. Figure 2a , Figure 2b and Figure 2c The rotation method should be as follows Figure 3 Specifically, (1) the initial posture rotates toward the intermediate posture R2; (2) the initial posture rotates toward the intermediate posture R2; (3) the target posture is changed, the speed toward R2 is reduced, and the rotation is toward R3; (4) the rotation continues toward R3; (5) the rotation continues toward R3; (6) the rotation speed reaches R3 and is 0.

[0075] Based on the above content, the online trajectory planning posture interpolation method based on Lie group Lie algebra also includes:

[0076] Step S410, receiving new second target posture information during the process of the end of the robot arm moving from the first initial posture to the first target posture.

[0077] Step S420, obtaining real-time position information of the end of the robotic arm.

[0078] Step S430, taking the real-time posture of the end of the robot arm as the second initial posture, re-planning the online trajectory of the path of the end of the robot arm moving from the second initial posture to the second target posture through Lie group and Lie algebraic transformation, and obtaining the control information of several second interpolation points of the end of the robot arm moving from the real-time posture to the second target posture.

[0079] Step S440, controlling the robot arm to move from the second initial posture to the second target posture according to the control information of the end of the robot arm at a plurality of second interpolation points.

[0080] During the movement of the end of the robot arm from the first initial posture to the first target posture, the new second target posture information can be received, and the real-time posture information can be obtained at the same time. Then, with the help of Lie group and Lie algebra transformation, the real-time posture is used as the second initial posture, and the online trajectory planning of the path to the new target posture is re-performed to obtain the control information of several second interpolation points, and then the robot arm is accurately controlled to move from the second initial posture to the second target posture based on these control information. The robot arm can flexibly respond to the setting of the new target posture during the movement process. By replanning the trajectory in real time, the adaptability, accuracy and responsiveness of the robot arm movement to complex task scenarios are effectively improved, ensuring that it can complete the action more accurately according to the new requirements.

[0081] Specifically, the step S100 of obtaining the first initial posture information and the first target posture information of the end of the robot arm includes:

[0082] Step S110, respectively obtaining a rotation matrix of the first initial posture and a rotation matrix of the first target posture.

[0083] Step S120, transforming the rotation matrix through Lie group Lie algebra according to the following formula to obtain posture description information in Lie algebra space;

[0084]

[0085] Where R is a 3×3 rotation matrix, tr(R) is the trace of the matrix, a is a 3×1 matrix used to describe the rotation axis, and θ is the angle of rotation around the rotation axis a.

[0086] The above implementation method can utilize the characteristics of Lie group and Lie algebra to convert the common posture representation form of rotation matrix into Lie algebra space for posture description, so that the expression of the robot arm posture information is more accurate and convenient for subsequent online trajectory planning, interpolation point control and other operations based on it, thereby laying a good foundation for realizing efficient, accurate and flexible motion control of the robot arm, and improving the precision and scientificity of the control of the robot arm posture.

[0087] For further information, please refer to Figure 4, in step S120, the rotation matrix is ​​converted into the posture description information in the Lie algebra space according to the following formula, including:

[0088] Step S121 , obtaining first posture description information and second posture description information in Lie algebra space, and first moving path and second moving path respectively corresponding to the first posture description information and the second posture description information based on Lie group and Lie algebra transformation.

[0089] Step S122, select the moving path corresponding to the posture description information closest to the end point of the first initial posture as the preferred moving path, generate control information of several first interpolation points of the preferred moving path based on online trajectory planning, and control the end of the robotic arm to move from the first initial posture to the first target posture.

[0090] The Lie group and Lie algebra transformations are used to enrich the methods of posture description and path planning. By reasonably selecting the optimal moving path and generating interpolation point control information, the motion trajectory of the robot arm can be planned more accurately, effectively improving the accuracy, scientificity and flexibility of motion control in the process of the robot arm moving from the initial posture to the target posture, making the robot arm's action execution more in line with expectations and efficient.

[0091] In order to achieve the optimal interpolation trajectory path, it is necessary to optimize the Lie algebra description of the posture. According to the Lie algebra description of the posture, φ = θa represents a rotation of θ around the axis a in a fixed axis coordinate system. Therefore, when the axes are opposite and the rotation angles differ by 2π-θ, the posture represented is the same. The mathematical description is:

[0092] φ=θa=(2π-θ)(-a);

[0093] Assume that the current posture of the robot arm in space is φ1=[x1,y1,z,1] and the target posture is φ′2=[x2,y2,z2]=[x'2,y'2,z'2]=φ″2. In order to achieve the optimal interpolation trajectory, φ2 needs to be reselected. The evaluation criterion is the vector closest to the end point of φ1 vector. The calculation formula is as follows:

[0094] φ2=min(||φ1-φ′2||2,||φ1-φ″2||2);

[0095] Among them, φ2 is the posture description information corresponding to the preferred moving path, φ1 is the first initial posture, φ2' is the first posture description information, and φ2" is the second posture description information.

[0096] Further, selecting a moving path corresponding to the posture description information that is closest to the first initial posture endpoint as the preferred moving path includes: selecting a moving path corresponding to a posture description method with a smaller rotation angle value in the first posture description information and the second posture description information as the preferred moving path.

[0097] The above-mentioned selection of the preferred path based on the rotation angle value can determine the motion trajectory of the robot arm from the first initial posture to the first target posture in a relatively more direct and concise path planning approach. This method of selecting the preferred movement path based on the rotation angle value helps to simplify the complexity of path planning, making the generated preferred movement path more reasonable and efficient, and then when the interpolation point control information is subsequently generated based on online trajectory planning to control the movement of the robot arm, the robot arm can complete the movement from the initial posture to the target posture in a more precise, stable and expected manner, effectively improving the accuracy and overall performance of the robot arm motion control.

[0098] from Figure 4 It can be seen that interpolation method A is better than interpolation method B. A has fewer interpolation points and the corresponding posture has the smallest rotation angle, which can achieve the optimal posture during the interpolation process.

[0099] Below, taking a specific implementation of an embodiment of the present invention as an example, the above-mentioned posture planning method is described in detail:

[0100] 1. Given the initial position of the end of the robot and target pose Where P i =[X i ,Y i ,Z i ] T is the position vector, R i is the rotation matrix, using the following formula:

[0101]

[0102] Transformed into the posture description method φ in Lie algebra space i =[x i ,y i ,z i ] where R is a 3×3 rotation matrix, tr(R) is the trace of the matrix, a is a 3×1 matrix used to describe the axis of rotation, and θ is the angle of rotation around the axis a. For example, the rotation matrix corresponding to a 90° rotation of the base coordinate system along the z axis is The corresponding Lie algebra is described as

[0103] 2. Convert the homogeneous transformation matrix describing the end position of the robot arm into a one-dimensional matrix ψ that can be used to describe the position and posture information i =[P i ,φ i], the current position and posture information is ψ0 = [X0, Y0, Z0, x0, y0, z0], the target position and posture information is ψ1 = [X1, Y1, Z1, x1, y1, z1], and the current speed information is The acceleration information is Setting the target speed and acceleration to 0 can stop the robot arm.

[0104] Furthermore, online trajectory planning is used to generate simultaneous position and attitude trajectories.

[0105] 3. When the target posture of the robot arm changes during movement, the posture, velocity and acceleration at the current moment are used as the posture, velocity and acceleration of the starting position, and the contour is calculated again using online trajectory planning to let the robot arm execute the planned trajectory.

[0106] 4. Since the online trajectory planning uses the current speed and position as the starting value for planning when calculating the contour, the continuity of posture and speed during movement can be guaranteed. Since the position is not coupled, it can be planned separately, but the posture is coupled, so it is necessary to use the above-mentioned posture representation method to decouple it, so that the position and speed of the robot arm motion planning can be continuous and smooth. Assuming that the starting posture of the robot arm is T0, the target posture is T1, and the target posture is changed to T2 during the movement, the posture interpolation can be achieved using the above-mentioned posture description method. Figure 5 The effect shown.

[0107] 5. After the online trajectory planning of position and attitude, the velocity curve of position and attitude can be obtained, such as Figure 6 shown.

[0108] First, the above formula is used to convert the rotation matrix of the end of the robot into a posture description method in the Lie algebra space, which can more accurately describe the posture information. For example, the case where the base coordinate system rotates 90° along the z-axis in the example clearly shows its effectiveness. Secondly, the homogeneous transformation matrix is ​​converted into a one-dimensional matrix to describe information such as position and posture, and the robot can be stopped by setting the target speed and acceleration to 0, and the online trajectory planning can generate the position and posture trajectory that are reached at the same time. Furthermore, when the target posture changes during the movement of the robot, the current moment related parameters are used as the starting point for re-planning to ensure the continuity of the movement. More importantly, the online trajectory planning is used to calculate the contour with the current moment speed and position as the starting value, and the posture with coupling is decoupled by combining the posture representation method, so that the position and speed in the robot motion planning are continuous and smooth, which can be reflected in the posture interpolation effect and the obtained speed curve, etc., which improves the accuracy, flexibility, stability and controllability of the robot posture planning as a whole.

[0109] Accordingly, please refer to Figure 7 A second aspect of an embodiment of the present invention provides an online trajectory planning posture interpolation system based on Lie group and Lie algebra, and an online trajectory planning posture interpolation method based on Lie group and Lie algebra controls a robotic arm, including:

[0110] An information acquisition module 1 is used to acquire first initial posture information and first target posture information of the end of the robot arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation;

[0111] A trajectory planning module 2 is used to generate control information of a plurality of first interpolation points of the end of the manipulator moving from the first initial posture to the first target posture by using online trajectory planning based on the first initial posture information and the first target posture information, wherein the control information includes: position information, velocity information and acceleration information;

[0112] The motion control module 3 is used to control the robot arm to move from a first initial posture to a first target posture according to the control information of the end of the robot arm at a plurality of first interpolation points.

[0113] Accordingly, a third aspect of an embodiment of the present invention provides an electronic device, comprising: at least one processor; and a memory connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes an online trajectory planning posture interpolation method based on Lie group Lie algebra.

[0114] Accordingly, a fourth aspect of an embodiment of the present invention provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement an online trajectory planning posture interpolation method based on Lie group and Lie algebra.

[0115] The embodiment of the present invention aims to protect an online trajectory planning posture interpolation method and system based on Lie group and Lie algebra, wherein the method includes the following steps: obtaining the first initial posture information and the first target posture information of the end of the robot arm, the first initial posture information and the first target posture information both include: the position vector information in the base coordinate system and the posture description information obtained by the Lie group and Lie algebra transformation; based on the first initial posture information and the first target posture information, using online trajectory planning to generate the control information of several first interpolation points of the end of the robot arm moving from the first initial posture to the first target posture, the control information includes: position information, speed information and acceleration information; according to the control information of the end of the robot arm at several first interpolation points, control the robot arm to move from the first initial posture to the first target posture. The above technical scheme has the following effects:

[0116] 1. The rotation matrix at the end of the robot arm is converted into a posture description in the Lie algebra space, which can accurately represent the posture information, provide accurate basic data for subsequent trajectory planning and motion control, and improve the precision of the robot arm posture description;

[0117] 2. The homogeneous transformation matrix is ​​converted into a one-dimensional matrix for describing the position and posture, and the position and posture trajectory that are reached simultaneously are generated by online trajectory planning. When the target posture changes, the current parameters can be used for re-planning, which enhances the flexibility of the robot arm trajectory planning to cope with changes and ensures the continuity of the movement.

[0118] 3. Online trajectory planning uses the current speed and position as the starting values ​​to calculate the contour, and decouples the coupled posture through posture representation, achieving continuous and smooth position and speed during the movement of the robot arm, effectively improving the stability and controllability of the robot arm's movement process. The advantages can be seen from the interpolation effect and speed curve.

[0119] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0120] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0121] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0122] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0123] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. An online trajectory planning posture interpolation method based on Lie group Lie algebra, characterized in that: The steps include: Acquire first initial posture information and first target posture information of the end of the robotic arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation; Based on the first initial posture information and the first target posture information, online trajectory planning is used to generate control information of a plurality of first interpolation points of the end of the manipulator moving from the first initial posture to the first target posture, wherein the control information includes: position information, velocity information and acceleration information; According to the control information of the end of the robot arm at the plurality of first interpolation points, the robot arm is controlled to move from the first initial posture to the first target posture.

2. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 1 is characterized in that: The first initial posture of the end of the robotic arm is a stationary state or a non-stationary state.

3. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 2 is characterized in that: Also includes: During the process of the end of the robotic arm moving from the first initial posture to the first target posture, receiving second target posture information different from the first target posture; Obtaining real-time position information of the end of the robotic arm; The real-time posture of the end of the robot arm is used as the second initial posture, and the path of the end of the robot arm moving from the second initial posture to the second target posture is re-planned online through Lie group and Lie algebra transformation to obtain control information of a plurality of second interpolation points of the end of the robot arm moving from the real-time posture to the second target posture; According to the control information of the end of the robot arm at the plurality of second interpolation points, the robot arm is controlled to move from the second initial posture to the second target posture.

4. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 1 is characterized in that: The obtaining of the first initial posture information and the first target posture information of the end of the robotic arm includes: Obtaining a rotation matrix of the first initial pose and a rotation matrix of the first target pose respectively; According to the following formula, the rotation matrix is ​​transformed by Lie group and Lie algebra to obtain the posture description information in Lie algebra space; Where R is a 3×3 rotation matrix, tr(R) is the trace of the matrix, a is a 3×1 matrix used to describe the rotation axis, and θ is the angle of rotation around the rotation axis a.

5. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 4 is characterized in that: The step of converting the rotation matrix into posture description information in Lie algebra space according to the following formula includes: Obtaining first posture description information and second posture description information in a Lie algebra space based on Lie group and Lie algebra transformation, and a first moving path and a second moving path corresponding to the first posture description information and the second posture description information respectively; A moving path corresponding to the posture description information that is closest to the end point of the first initial posture is selected as a preferred moving path, and control information of several first interpolation points of the preferred moving path is generated based on online trajectory planning to control the end of the robotic arm to move from the first initial posture to the first target posture.

6. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 5 is characterized in that: The calculation formula for calculating the posture description information closest to the first initial posture endpoint is as follows: φ2=min(||φ1-φ′2||2,||φ1-φ″2||2); φ'2=θa; φ″2=(2π-θ)(-a); Among them, φ2 is the posture description information corresponding to the preferred moving path, φ1 is the first initial posture, φ′2 is the first posture description information rotated around the rotation axis a by an angle θ, and φ″2 is the second posture description information rotated around the rotation axis a by an angle (2π-θ).

7. The online trajectory planning posture interpolation method based on Lie group Lie algebra according to claim 5 is characterized in that: The step of selecting a moving path corresponding to the posture description information which is closest to the first initial posture endpoint as the preferred moving path includes: A moving path corresponding to a posture description mode with a smaller rotation angle value in the first posture description information and the second posture description information is selected as a preferred moving path.

8. An online trajectory planning attitude interpolation system based on Lie group Lie algebra, characterized in that: The robot arm is controlled based on the online trajectory planning posture interpolation method based on Lie group Lie algebra as described in any one of claims 1 to 7, comprising: An information acquisition module, which is used to acquire first initial posture information and first target posture information of the end of the robot arm, wherein the first initial posture information and the first target posture information both include: position vector information in a base coordinate system and posture description information obtained by Lie group and Lie algebra transformation; A trajectory planning module, which is used to generate control information of a plurality of first interpolation points of the end of the manipulator moving from the first initial posture to the first target posture by online trajectory planning based on the first initial posture information and the first target posture information, wherein the control information includes: position information, velocity information and acceleration information; A motion control module is used to control the robot arm to move from the first initial posture to the first target posture according to the control information of the end of the robot arm at the first interpolation points.

9. An electronic device, characterized in that: include: at least one processor; And a memory connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor executes the online trajectory planning posture interpolation method based on Lie group Lie algebra as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: Computer instructions are stored thereon, and when the instructions are executed by a processor, an online trajectory planning posture interpolation method based on Lie group Lie algebra as described in any one of claims 1-7 is implemented.

Images