Robot trajectory tracking method, system, medium and device with coordinated posture and orientation

Abstract

The present invention proposes a robot trajectory tracking method, system, medium and equipment with coordinated posture and orientation, and relates to the field of robot navigation technology. The method includes: according to the current coordinate system and the target coordinate system of the robot, obtaining the affine transformation matrix of the robot from the current coordinate system to the target coordinate system; according to the current posture and the target posture of the robot, obtaining the motion relationship between the current posture and the target posture based on the affine transformation matrix; establishing a control law equation according to the motion relationship, and obtaining the control law parameter value according to the stability requirement; obtaining a control decision according to the control law parameter value, and controlling the robot to achieve automatic navigation and reach the target posture. The present invention is based on the Lyapunov stability framework, and through feedback linearization, according to the Vieta theorem, the Hurwitz matrix and the Xie Xukai criterion, the parameter constraint conditions are comprehensively obtained. Under the conditions of these control laws, the mobile robot can stably and effectively track the target waypoint.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation technology, and in particular to a posture and orientation coordinated robot trajectory tracking method, system, medium and equipment. Background Art

[0002] Navigation control is the core issue of mobile robots, which is generally divided into three levels: macro, meso and micro:

[0003] (1) The macro level is manifested as global path planning;

[0004] (2) Meso-level, generally dynamic local path planning;

[0005] (3) Micro level: mainly trajectory tracking.

[0006] Robot global path planning is to plan a series of target positions based on the current position of the robot, such as Figure 1 When the global path planning is completed, a series of trajectories are obtained; then local planning is performed to filter and extract valid waypoints (ViaPoint); finally, a trajectory tracking controller is designed to control the robot to traverse various waypoints with the set posture to achieve trajectory tracking.

[0007] Global path planning is generally based on the shortest path of a directed graph, such as breadth-first search BFS and heuristic search Dijkstra, Algorithms, etc.

[0008] Local path planning mainly includes planning algorithms based on dynamic models, such as the dynamic window method (DWA), artificial potential field method, and planning methods based on random sampling, such as rapid search random tree (RRT), etc. Among them, the DWA controller can be used for differential and omnidirectional models, but not for the Ackerman model; and it only considers speed and acceleration limits, and the trajectory space is small.

[0009] Trajectory tracking is mainly a control problem, such as pure tracking algorithms based on kinematics, model predictive control (MPC) based on dynamics, and data-driven PID regulation. Among them, MPC controllers can be used for models such as differential and Ackerman, which divide the future time period into multiple nodes to predict the state of each node; but there are high requirements for computing power, and there is a risk of control lag due to insufficient bandwidth. The PID controller is single-input and single-output, and an additional observer needs to be designed for state observation requirements, and the control parameter setting needs to rely on experience. Summary of the invention

[0010] In order to solve the above problems, the present invention proposes a robot trajectory tracking method, system, medium and device with coordinated posture and orientation, based on the Lyapunov stability framework, through feedback linearization, according to the Vieta theorem, the Hurwitz matrix and the Xie Xukai criterion, the parameter constraint conditions are comprehensively obtained. Under the conditions of these control laws, the mobile robot can effectively track the target waypoint.

[0011] In order to achieve the above object, the present invention adopts the following technical solution:

[0012] In a first aspect, the present invention provides a posture and orientation coordinated robot trajectory tracking method, comprising:

[0013] According to the current coordinate system and target coordinate system of the robot, the affine transformation matrix of the robot from the current coordinate system to the target coordinate system is obtained. ;

[0014] According to the current posture and target posture of the robot, obtain the affine transformation matrix based on the The kinematic equations and geometric relationship between the current pose and the target pose;

[0015] Establishing a control law equation according to the kinematic equation and the geometric relationship, obtaining the control law parameter values ​​according to the stability requirement; obtaining a control decision according to the control law parameter values;

[0016] The movement of the robot is controlled based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

[0017] Preferably, the affine transformation matrix from the current coordinate system to the target coordinate system is The acquisition process is:

[0018] According to the current robot coordinate system { R}In the map coordinate system{ W}Middle pose ， Obtained from { W}to{ R The affine transformation matrix of ;

[0019] Given the target coordinate system { T}In the map coordinate system{ W}Middle pose , obtained from { W}to{ T The affine transformation matrix of ;

[0020] according to The solution is from the current coordinate system { R} to the target coordinate system { TThe affine transformation matrix of .

[0021] Preferably, the geometric relationship includes the distance from the current posture to the target posture, the azimuth angle of the target posture relative to the current posture, and the attitude angle of the current posture relative to the target posture.

[0022] Preferably, the control law equation is:

[0023]

[0024] in, and is the linear velocity and angular velocity of the robot; , , are control law parameters, Based on the distance between the robot and the target Linear adjustment of line speed; The angular velocity is linearly adjusted based on the azimuth between the robot and the target; The angular velocity is linearly adjusted based on the attitude angle between the robot and the target.

[0025] Preferably, the control law parameter values ​​are obtained according to the stability requirements; specifically:

[0026] The differential equations of the dynamic system are obtained based on the kinematic equations and geometric relations;

[0027] According to the dynamic system differential equation and the control law equation, the dynamic system differential equation under control is obtained;

[0028] Lyapunov's theorem, Hurwitz's criterion and Xie Xukai's criterion are used to solve the control law parameters based on the differential equations of the dynamic system under control according to the stability requirements.

[0029] Preferably, the Lyapunov theorem, the Horwitz criterion and the Xie Xukai criterion are used to solve the control law parameters based on the differential equation of the dynamic system under control according to the stability requirements. The specific process is:

[0030] According to Lyapunov's theorem, the differential equation of the dynamic system under the control is linearized to obtain a linearized equation;

[0031] Obtaining the first characteristic polynomial and the first characteristic equation according to the first-order linearized equation;

[0032] According to the geometric meaning of the characteristic equation, the initial value range of the control law parameters is obtained;

[0033] According to the Horwitz criterion, a second characteristic equation is obtained based on the first characteristic equation; the second characteristic equation is converted into a Horwitz matrix to obtain a quadratic value range of a control law parameter;

[0034] According to Xie Xukai's criterion, the cubic range of control law parameters is obtained based on the second characteristic equation;

[0035] The final value range of the control law parameters is obtained by combining the initial value range, the secondary value range and the tertiary value range of the control law parameters.

[0036] In a second aspect, the present invention provides a posture and orientation coordinated robot trajectory tracking system, comprising:

[0037] The transformation matrix acquisition module is used to obtain the affine transformation matrix of the robot from the current coordinate system to the target coordinate system according to the current coordinate system and the target coordinate system of the robot. ;

[0038] The motion relationship acquisition module is used to obtain the affine transformation matrix based on the current posture and target posture of the robot. The kinematic equations and geometric relationship between the current pose and the target pose;

[0039] A control decision acquisition module is used to establish a control law equation according to the kinematic equation and the geometric relationship, obtain the control law parameter value according to the stability requirement; and obtain a control decision according to the control law parameter value;

[0040] The navigation module is used to control the movement of the robot based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

[0041] In a third aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of a posture and orientation coordinated robot trajectory tracking method described in the first aspect.

[0042] In a fourth aspect, the present invention provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps in the posture and orientation coordinated robot trajectory tracking method described in the first aspect are implemented.

[0043] Compared with the prior art, the present invention has the following beneficial effects:

[0044] The present invention simplifies the algorithm, improves navigation efficiency, and improves the accuracy of robot automatic navigation by constructing a geometric relationship based on attitude angle and azimuth angle, and constraining the robot navigation direction based on the geometric relationship. It does not require iteration and forward prediction, so there is basically no requirement for computing power, which reduces the computing power and cost of model optimization or simplification, and does not require the configuration of additional equipment.

[0045] In local path planning, the present invention can dynamically adjust the control strategy according to the attitude angle and azimuth angle to adapt to different environmental changes and waypoint requirements, so that the robot can maintain stable trajectory tracking performance in complex and changing environments.

[0046] The control method of the present invention is based on clear mathematical principles and clear parameter constraints, making it relatively simple to implement and maintain. At the same time, since it does not need to rely on too many empirical parameters, the difficulty of debugging and optimization is reduced, and the maintainability and scalability of the system are improved. The control method provided by the present invention is applicable to different types of robots such as wheeled and crawler robots.

[0047] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] The accompanying drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their description are used to explain the present invention but do not constitute a limitation of the present invention.

[0049] Figure 1 It is a schematic diagram of global path planning of a robot in the prior art;

[0050] Figure 2 A main flow chart of a posture and orientation coordinated robot trajectory tracking method provided by an embodiment of the present invention;

[0051] Figure 3 A schematic diagram of a current coordinate system and a target coordinate system provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0052] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0053] Embodiment 1

[0054] like Figure 2 As shown, this embodiment discloses a posture and orientation coordinated robot trajectory tracking method, comprising the following steps:

[0055] S1: According to the current coordinate system and target coordinate system of the robot, obtain the affine transformation matrix from the current coordinate system to the target coordinate system of the robot ;

[0056] S2: According to the current posture and target posture of the robot, obtain the affine transformation matrix based on the The kinematic equations and geometric relationship between the current pose and the target pose;

[0057] S3: Establishing a control law equation according to the kinematic equation and the geometric relationship, and obtaining the values ​​of the control law parameters according to the stability requirements; and obtaining a control decision according to the values ​​of the control law parameters;

[0058] S4: Control the movement of the robot based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

[0059] Next, combine Figure 2 , a posture and orientation coordinated robot trajectory tracking method disclosed in this embodiment is described in detail.

[0060] The local navigation of the robot is to run from the current posture to the target posture, and establish the current coordinate system with the current posture of the robot , establish the target coordinate system based on the target pose ,like Figure 3 shown.

[0061] In S1, according to the current coordinate system and target coordinate system of the robot, the affine transformation matrix of the robot from the current coordinate system to the target coordinate system is obtained. ; Specifically include:

[0062] S101: According to the current coordinate system { R}In the map coordinate system{ W}Middle pose ， Obtained from { W}to{ R The affine transformation matrix of ;

[0063] S102: Given a target coordinate system { T}In the map coordinate system{ W}Middle pose , obtained from { W}to{ T The affine transformation matrix of ;

[0064] S103: According to Solve the affine transformation matrix from the current coordinate system {R} to the target coordinate system {T} .

[0065] First, we need to solve the current position of the robot Relative target pose The attitude angle and azimuth .

[0066] Specifically, we need to solve the current posture To target pose The two-dimensional Euclidean transformation matrix of :

[0067] (1)

[0068] (2).

[0069] The specific solution method is to remember the map as the world coordinate system , according to the robot coordinate system In the map coordinate system neutral posture , can be obtained from to The affine transformation matrix ; For a given target coordinate system In the map coordinate system neutral posture , can be obtained from to The affine transformation matrix ; According to the cascade relationship , which can be solved by the robot To the target The Euclidean transformation matrix .

[0070] In S2, according to the current posture and target posture of the robot, the affine transformation matrix is ​​obtained. The kinematic equations and geometric relationship between the current pose and the target pose.

[0071] First, according to the solution , is a 3×3 matrix, where For the matrix No. Line Column elements; attitude angle , azimuth , and the distance between the robot and the target .

[0072] Secondly, realize rigid body motion The specific method is as follows:

[0073] According to the kinematic equation:

[0074] (3)

[0075] in: and For robots The linear velocity and angular velocity, and For robots In the target coordinate system The coordinates of For robots Angular displacement, angular displacement It has been processed by Lie algebra and there is no need to consider the circular loop problem.

[0076] like Figure 3 , according to the geometric relationship:

[0077] (4)

[0078] in: is the distance between the robot and the target, that is Origin and Distance from the origin; For oneself Origin to The vector from the origin and of The angle between the axes, i.e. the azimuth of the target in the robot coordinate system, is positive in the counterclockwise direction according to the right-hand system; for relatively The attitude angle, i.e. to The rotation angle, that is, the two-dimensional rotation matrix The corresponding Euler angles are positive counterclockwise in the right-hand system.

[0079] by is the state vector, combining the kinematic equation (3) and the geometric relationship (4):

[0080] (5)

[0081] as well as

[0082] (6)

[0083] The differential equation of the dynamic system can be obtained as:

[0084] (7)

[0085] It should be understood that it is achievable for those skilled in the art to combine the kinematic equation (3) and the geometric relationship (4) to obtain the dynamic system differential equation (7).

[0086] by To control the vector, linear feedback control is used to set the input control (control law equation) as:

[0087] (8)

[0088] in, , , are the control law parameters:

[0089] 1) Based on the distance between the robot and the target Linear adjustment of line speed, i.e. distance The larger the line speed The bigger;

[0090] 2) The angular velocity is linearly adjusted based on the azimuth angle between the robot and the target, i.e., the azimuth angle The greater the angular velocity The bigger;

[0091] 3) The angular velocity is linearly adjusted based on the attitude angle between the robot and the target, that is, the attitude angle The greater the angular velocity The bigger.

[0092] Combining (7) and (8), the differential equation of the dynamic system under control is obtained:

[0093] (9)

[0094] Formula (9) presents a nonlinear system due to the existence of trigonometric functions. Let Satisfy in a small neighborhood and , formula (9) can be linearized into formula (10):

[0095] (10)

[0096] (11)

[0097] (12)

[0098] Among them, formula (11) is the characteristic polynomial, and formula (12) is the corresponding characteristic equation.

[0099] According to Lyapunov's theorem, The solution set is ,system Stability requirements ,Right now All solutions have negative real parts. There are zero characteristic roots in equation (12). According to the basic theorem of stability: A system with zero roots and a minimal polynomial with multiple roots is unstable. A single root system with zero root and a minimal polynomial is stable but not asymptotically stable.

[0100] It can be seen that the linearization of Eq. (10) leads to critical stability. Therefore, in order to meet the stability requirements, it is weakened in the minimum neighborhood. and , linearize system (9) into a linearized equation:

[0101] (13)

[0102] (14)

[0103] (15)

[0104] Among them, formula (14) is the first characteristic polynomial of the linearized equation, and formula (15) is the corresponding first characteristic equation.

[0105] According to (15) The items are:

[0106] (16)

[0107] In formula (15), The geometric meaning of the term is a parabola. According to Vieta's theorem, the same sign requirement Combining (16) we have:

[0108] (17)

[0109] The coordinates of the axis of symmetry of the parabola are , because they have the same sign and are located in the left half plane, that is, the axis of symmetry of the curve is on the left, we need Right now:

[0110] (18)

[0111] Combining (16) to (18), we get the initial range of control law parameters:

[0112] (19)

[0113] According to the Horwitz criterion, a stable system Solution Equivalent to And all the sequential principal minors of the Horwitz matrix are positive, and the first characteristic equation (15) is rearranged into the second characteristic equation:

[0114] (20)

[0115] The Horwitz matrix of formula (20) is:

[0116] (twenty one)

[0117] according to of have:

[0118] (twenty two)

[0119] according to of have:

[0120] (twenty three)

[0121] according to of have

[0122] (twenty four)

[0123] according to have

[0124] (25)

[0125] Combining (22) to (25), we get the quadratic value range of the control law parameters:

[0126] (26)

[0127] According to Xie Xukai's criterion, a stable system Solution The necessary condition is The sufficient condition is . From the second characteristic equation (20) have:

[0128] (27)

[0129] Combining (19), (26) and (27), we get the final range of control law parameters:

[0130] (28)

[0131] For the robot of this embodiment, the exemplary control law parameters satisfying the constraints are preferably (29)

[0132] The control law parameters are substituted into the control law equation to obtain a control decision, and the movement of the robot is controlled based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

[0133] By combining Lyapunov's theorem, Horwitz's criterion and Xie Xukai's criterion, this embodiment can comprehensively evaluate the stability of the robot tracking system from multiple angles, discover stability problems that may be ignored by a single criterion, and improve the reliability and safety of the tracking control design. At the same time, these criteria are not only applicable to linear systems, but also to various types of systems such as nonlinear systems and time-varying systems, providing a powerful tool for stability analysis of complex robot systems. In addition, the stability criterion provides clear theoretical guidance for control strategy debugging, which helps to quickly locate and solve problems that arise during the debugging process; compared with debugging methods that rely on experience, debugging methods based on stability criteria are more scientific and efficient, which helps to reduce debugging costs and time.

[0134] The existing technologies mostly conduct stability research based on a single criterion, but a single criterion can only be used as a sufficient condition to ensure the stability of tracking control. This condition may not necessarily be true for system stability. That is, if a certain condition is met, the system can be determined to be stable, but this does not mean that the system stability is only determined by this condition. There may be other conditions that make the system stable that are not included. Therefore, in order to more accurately determine the range of conditions for system stability, the present invention uses multiple criteria to narrow the range of control law parameters, so as to achieve the effect of strengthening the sufficient conditions for system stability, that is, to narrow the range of conditions that do not meet stability, while minimizing the over-estimation of the extension of stability conditions, thereby improving the accuracy and practicality of system stability evaluation.

[0135] This specific embodiment can more accurately control the robot's motion trajectory through a control strategy based on attitude angle and azimuth angle, combined with the Lyapunov stability framework and feedback linearization technology, to ensure that the robot can accurately and stably track the target waypoints, and improve the accuracy and reliability of trajectory tracking. At the same time, it integrates a variety of mathematical tools such as Vieta's theorem, the Horwitz matrix, and Xie Xukai's criterion to derive clear parameter constraints, which provides a theoretical basis for the design and optimization of the control law, making the selection of control parameters more scientific and reasonable, and helping to improve the performance and stability of the control system. In addition, while ensuring control accuracy, the present invention has lower requirements on computing resources, which means that under limited computing resources, the present invention can complete control calculations faster, reduce the risk of control lag, and improve the real-time performance of the system.

[0136] Embodiment 2

[0137] This embodiment provides a posture and orientation coordinated robot trajectory tracking system, including:

[0138] The transformation matrix acquisition module is used to obtain the affine transformation matrix of the robot from the current coordinate system to the target coordinate system according to the current coordinate system and the target coordinate system of the robot. ;

[0139] The motion relationship acquisition module is used to obtain the affine transformation matrix based on the current posture and target posture of the robot. The kinematic equations and geometric relationship between the current pose and the target pose;

[0140] A control decision acquisition module is used to establish a control law equation according to the kinematic equation and the geometric relationship, obtain the control law parameter value according to the stability requirement; and obtain a control decision according to the control law parameter value;

[0141] The navigation module is used to control the movement of the robot based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

[0142] Embodiment 3

[0143] This embodiment provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the steps in the posture and orientation coordinated robot trajectory tracking method as described in the first embodiment above are implemented.

[0144] Embodiment 4

[0145] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps in the posture and orientation coordinated robot trajectory tracking method described in the above-mentioned embodiment 1 are implemented.

[0146] The steps or modules involved in the above embodiments 2 to 4 correspond to those in embodiment 1. For the specific implementation, please refer to the relevant description of embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood to include any medium that can store, encode or carry an instruction set for execution by a processor and enable the processor to execute any method in the present invention.

[0147] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot trajectory tracking method with posture and orientation coordination, characterized in that: include: According to the current coordinate system and target coordinate system of the robot, the affine transformation matrix of the robot from the current coordinate system to the target coordinate system is obtained. ; According to the current posture and target posture of the robot, obtain the affine transformation matrix based on the The kinematic equations and geometric relationship between the current pose and the target pose; Establishing a control law equation according to the kinematic equation and the geometric relationship, obtaining the control law parameter values ​​according to the stability requirement; obtaining a control decision according to the control law parameter values; The control law equation is: in, and is the linear velocity and angular velocity of the robot; , , are control law parameters, Based on the distance between the robot and the target Linear adjustment of line speed; is the azimuth between the robot and the target Linear adjustment of angular velocity; is the attitude angle between the robot and the target Linear adjustment of angular velocity; The movement of the robot is controlled based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

2. A posture and orientation coordinated robot trajectory tracking method as claimed in claim 1, characterized in that: The affine transformation matrix from the current coordinate system to the target coordinate system The acquisition process is: According to the current robot coordinate system { R }In the map coordinate system{ W }Middle pose ， Obtained from { W }to{ R The affine transformation matrix of ; Given the target coordinate system { T }In the map coordinate system{ W }Middle pose , obtained from { W }to{ T The affine transformation matrix of ; according to The solution is from the current coordinate system { R } to the target coordinate system { T The affine transformation matrix of .

3. A posture and orientation coordinated robot trajectory tracking method as claimed in claim 1, characterized in that: The geometric relationship includes the distance from the current posture to the target posture, the azimuth angle of the target posture relative to the current posture, and the attitude angle of the current posture relative to the target posture.

4. A posture and orientation coordinated robot trajectory tracking method as claimed in claim 1, characterized in that: According to the stability requirement, the control law parameter values ​​are obtained; specifically: The differential equations of the dynamic system are obtained based on the kinematic equations and geometric relations; According to the dynamic system differential equation and control law equation, the dynamic system differential equation under control is obtained; Lyapunov's theorem, Horwitz's criterion and Xie Xukai's criterion are used to solve the control law parameters based on the differential equations of the dynamic system under control according to the stability requirements.

5. A posture and orientation coordinated robot trajectory tracking method as claimed in claim 4, characterized in that: The Lyapunov theorem, the Horvitz criterion and the Xie Xukai criterion are used to solve the control law parameters based on the differential equation of the dynamic system under control according to the stability requirements. The specific process is as follows: According to Lyapunov's theorem, the differential equation of the dynamic system under the control is linearized to obtain a linearized equation; Obtaining the first characteristic polynomial and the first characteristic equation according to the first-order linearized equation; According to the geometric meaning of the characteristic equation, the initial value range of the control law parameters is obtained; According to the Horwitz criterion, a second characteristic equation is obtained based on the first characteristic equation; The second characteristic equation is converted into a Hurwitz matrix to obtain the quadratic value range of the control law parameters; According to Xie Xukai's criterion, the cubic range of control law parameters is obtained based on the second characteristic equation; The final value range of the control law parameters is obtained by combining the initial value range, the secondary value range and the tertiary value range of the control law parameters.

6. A robot trajectory tracking system with posture and orientation coordination, characterized in that: include: The transformation matrix acquisition module is used to obtain the affine transformation matrix of the robot from the current coordinate system to the target coordinate system according to the current coordinate system and the target coordinate system of the robot. ; The motion relationship acquisition module is used to obtain the affine transformation matrix based on the current posture and target posture of the robot. The kinematic equations and geometric relationship between the current pose and the target pose; A control decision acquisition module is used to establish a control law equation according to the kinematic equation and the geometric relationship, obtain the control law parameter value according to the stability requirement; and obtain a control decision according to the control law parameter value; The control law equation is: in, and is the linear velocity and angular velocity of the robot; , , are control law parameters, Based on the distance between the robot and the target Linear adjustment of line speed; is the azimuth between the robot and the target Linear adjustment of angular velocity; is the attitude angle between the robot and the target Linear adjustment of angular velocity; The navigation module is used to control the movement of the robot based on the control decision, so that the robot can achieve automatic navigation and reach the target posture.

7. A posture and orientation coordinated robot trajectory tracking system as claimed in claim 6, characterized in that: The affine transformation matrix from the current coordinate system to the target coordinate system The acquisition process is: According to the current robot coordinate system { R }In the map coordinate system{ W }Middle pose , obtained from { W }to{ R The affine transformation matrix of ; Given the target coordinate system { T }In the map coordinate system{ W }Middle pose , obtained from { W }to{ T The affine transformation matrix of ; according to The solution is from the current coordinate system { R } to the target coordinate system { T The affine transformation matrix of .

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by the processor, the steps in the posture and orientation coordinated robot trajectory tracking method as described in any one of claims 1-5 are implemented.

9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the posture and orientation coordinated robot trajectory tracking method as described in any one of claims 1-5 are implemented.

Images