Robot control method and related equipment

Abstract

The invention discloses a robot control method and related equipment, and relates to the technical field of robots, and the method comprises the steps: obtaining initial pose data and initial motion observation data of a robot in an augmented quaternion form; based on the initial pose data and the initial motion observation data, establishing a generalized least square model with spherical constraint; solving the generalized least square model by adopting a Riemann alternating direction multiplier method to obtain target pose data of the robot; and performing attitude control on the robot based on the target pose data. According to the method, the augmented quaternion is adopted for representation, the generalized least square model of spherical constraint is established, and the Riemannian alternating direction multiplier method is adopted for solving, so that the accuracy, the solving efficiency and the control precision of robot pose description are remarkably improved.

Description

Technical Field

[0001] The present application relates to the field of robotics technology, and more specifically, to a robot control method and related equipment. Background Art

[0002] With the rapid development of robotics technology, accurate and efficient robot control methods have become the key to improving robot performance. Traditional robot control methods mostly rely on representation methods such as Euler angles or rotation matrices to describe the robot's position and motion state, but these methods have limitations such as large amount of calculation and difficulty in handling singular points when dealing with complex rotation and posture optimization problems. Especially when robots face dynamic environments and complex tasks, the accuracy and efficiency of traditional methods often cannot meet the needs of practical applications.

[0003] In the related art, for the posture estimation and control of the robot, the constraint relationship between the quaternion representation of the rotation and the actual motion observation data of the robot is usually not fully considered, resulting in the control strategy lacking sufficient accuracy and robustness. In addition, traditional posture optimization models often have high computational complexity and are difficult to effectively apply in scenarios with high real-time requirements. This not only increases the response time of the control system, but may also sacrifice the optimization effect due to the limitation of computing resources. Traditional methods often lead to low computational efficiency and fail to give full play to the advantages of multi-core processors or distributed computing resources because they fail to effectively reduce the algorithm's dependence on the scale of the problem. In other words, there are technical problems in the related art such as high computational complexity for robot posture control and low posture optimization effect. Summary of the invention

[0004] A series of simplified concepts are introduced in the summary of the invention, which will be further described in detail in the detailed description. The summary of the invention of this application does not mean to attempt to define the key features and essential technical features of the technical solution claimed for protection, nor does it mean to attempt to determine the scope of protection of the technical solution claimed for protection.

[0005] The robot control method and related devices provided in the present application can significantly improve the accuracy of the robot posture description, solution efficiency and control accuracy by adopting augmented quaternion representation, establishing a generalized least squares model of spherical constraints and solving it using the Riemann alternating direction multiplier method.

[0006] In a first aspect, the present application provides a robot control method, comprising: obtaining initial pose data and initial motion observation data of the robot in the form of augmented quaternions; establishing a generalized least squares model with spherical constraints based on the initial pose data and the initial motion observation data; solving the generalized least squares model using the Riemann alternating direction multiplier method to obtain target pose data of the robot; and performing posture control on the robot based on the target pose data.

[0007] In a feasible implementation manner, the initial posture data includes a first quaternion and a first vector, wherein the first quaternion is a quaternion representing a rotation process of the robot, and the first vector is a vector representing a displacement process of the robot; and establishing a generalized least squares model with a spherical constraint based on the initial posture data and the initial motion observation data includes: establishing the generalized least squares model based on the assumption that the first quaternion satisfies the spherical constraint, the rotation noise data in the first quaternion obeys a von Mises-Fisher distribution, and the displacement noise data in the first vector obeys a normal distribution.

[0008] In a feasible implementation, the generalized least squares model is:

[0009]

[0010] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, For the The conjugate of is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative rotation quaternion from the i-th frame to the j-th frame in the initial motion observation data, ∑1 is the first weighting matrix describing the displacement noise data, ∑2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0011] In a feasible implementation, the generalized least squares model is:

[0012]

[0013] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, for The conjugate of for The conjugation of For The second quaternion has the same meaning as For The second quaternion has the same meaning as is the first quaternion of the i-th frame, is the first quaternion of the jth frame, is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative motion quaternion from the i-th frame to the j-th frame in the initial motion observation data, Σ1 is the first weighting matrix describing the displacement noise data, Σ2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0014] In a feasible implementation, the Riemann alternating direction multiplier method is used to solve the generalized least squares model to obtain the target posture data of the robot, including: linearizing the generalized least squares model to obtain a linearized augmented Lagrangian function; adding adjacent terms to the linearized augmented Lagrangian function to obtain an optimized sub-function; and solving the optimized sub-function using the Riemann alternating direction multiplier method to obtain the target posture data of the robot.

[0015] In a feasible implementation manner, the optimization sub-function is:

[0016]

[0017] In the formula, is the first quaternion vector obtained at the k+1th iteration, is the second quaternion vector obtained at the k+1th iteration, tk+1 is the displacement vector obtained at the k+1th iteration, λk+1 is the dual variable obtained at the k+1th iteration, is the second quaternion vector obtained at the kth iteration, is the first quaternion vector obtained at the k-th iteration, tk is the first vector obtained at the k-th iteration, λk is the dual variable obtained at the k-th iteration, is the second quaternion vector, is the initial first quaternion vector, is the initial first vector, represents the augmented Lagrangian function corresponding to the generalized least squares model, is the linearization function of the augmented Lagrangian function, β represents the penalty parameter, and H1, H2 and H3 are three different proximity matrices.

[0018] In a feasible implementation manner, the optimization sub-function is:

[0019]

[0020] In the formula, is the second quaternion of the i-th frame obtained at the k+1-th iteration, is the second quaternion of the i-th frame obtained at the k-th iteration, is the second quaternion vector of all frames obtained in the k+1th iteration, is the second quaternion vector of all frames obtained at the kth iteration, is the first quaternion of the i-th frame obtained at the k-th iteration, is the first quaternion of the i-th frame obtained at the k+1-th iteration, is the first quaternion vector of all frames obtained at the kth iteration, is the first quaternion vector of all frames obtained in the k+1th iteration, t k+1 is the first vector of all frames obtained in the k+1th iteration, t k is the first vector of all frames obtained in the kth iteration, is the component of the dual variable corresponding to the i-th frame obtained in the k-th iteration. H1, H2 and H3 are three different adjacent matrices. 1,i and H 2,i H1 and H2 are the block matrices corresponding to the i-th frame, G 1,ij and G 2,ij are two different intermediate process matrices associated with edge (i, j), ∑1 is the first weighted matrix describing the displacement noise data, ∑2 is the second weighted matrix describing the rotation noise data; Q is a block matrix composed of 3×3 small matrices, each row of Q corresponds to a directed edge in the directed graph, I4 is a 4th-order unit matrix, I m is an m-order identity matrix, where m is the number of directed edges in the directed graph; is the set of all directed edges starting from the i-th frame; f represents the first function g represents the second functional Represents the first functional form with respect to the variable Find the partial derivative, The value of ; Represents the second functional form with respect to the variable Find the partial derivative, The value of ; is the intermediate quaternion in the iterative process, for The imaginary three-dimensional vector of .

[0021] In the second aspect, the present application also provides a robot control device, including: a data acquisition unit, used to acquire the initial pose data and initial motion observation data of the robot in the form of augmented quaternions; a model building unit, used to establish a generalized least squares model with spherical constraints based on the initial pose data and the initial motion observation data; a model solving unit, used to solve the generalized least squares model using the Riemann alternating direction multiplier method to obtain the target pose data of the robot; and a robot control unit, used to perform posture control on the robot based on the target pose data.

[0022] In a third aspect, the present application further provides an electronic device, comprising: a memory and a processor, wherein the processor is configured to implement the steps of the robot control method described in the first aspect when executing a computer program stored in the memory.

[0023] In a fourth aspect, the present application further provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the robot control method described in the first aspect.

[0024] In a fifth aspect, the present application also provides a computer program product, including a computer program or computer executable instructions, which, when executed by a processor, implements the robot control method provided in an embodiment of the present application.

[0025] In summary, the present application uses augmented quaternions to describe the robot's initial posture data and initial motion observation data. Compared with the traditional Euler angle or rotation matrix representation method, augmented quaternions can more accurately describe the three-dimensional rotation of the rigid body, avoiding the gimbal lock problem and the singular point problem, thereby improving the accuracy and stability of the posture description; establishing a generalized least squares model with spherical constraints can fully consider the physical constraints and the accuracy of the observation data during the robot's motion, making the model closer to the actual motion situation. The introduction of spherical constraints helps to keep the rotating quaternion representation on the unit sphere during the solution process, further improving the rationality of the model and the accuracy of the solution; using the Riemannian Alternating Direction Method ofMultipliers,RADMM) to solve the generalized least squares model. This method combines the advantages of Riemann optimization and the alternating direction multipliers method, and can significantly improve the solution efficiency while maintaining the convergence of the algorithm. In addition, the RADMM algorithm directly performs iterative optimization on the Riemann manifold, avoiding the tedious process of converting quaternions into other representations and then optimizing them in traditional methods, thereby improving the accuracy and robustness of the solution; the robot is controlled based on the target posture data obtained by the solution. Due to the use of efficient solution methods and accurate posture descriptions, the robot's posture can be adjusted in real time and accurately to meet the control requirements in complex task scenarios. In summary, the robot control method provided in the present application significantly improves the accuracy of the robot's posture description, solution efficiency and control accuracy by adopting augmented quaternion representation, establishing a generalized least squares model with spherical constraints, and using the Riemann alternating direction multipliers method for solution. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present specification. Also, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0027] Figure 1 A schematic diagram of a flow chart of a robot control method provided in an embodiment of the present application;

[0028] Figure 2 A schematic diagram of the structure of a robot control device provided in an embodiment of the present application;

[0029] Figure 3 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0030] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of this application and the above-mentioned drawings are used to distinguish similar objects, rather than to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments described herein can be executed in an order different from that shown in the figures or described in words. In addition, the terms "is" and "has" and any variations thereof involved in this application are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0031] The term "module" or "unit" in this application refers to a computer program or a part of a computer program that has a predetermined function and works together with other related parts to achieve a predetermined goal, and can be implemented in whole or in part by using software, hardware (such as processing circuits or memories) or a combination thereof. Similarly, a processor (or multiple processors or memories) can be used to implement one or more modules or units. In addition, each module or unit can be part of an overall module or unit that includes the function of the module or unit.

[0032] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. In the following description, it is related to "some embodiments", which describes a subset of all possible embodiments, but it can be understood that "some embodiments" can be the same subset or different subsets of all possible embodiments, and can be combined with each other without conflict.

[0033] See also Figure 1 , Figure 1 1 is a flow chart of a robot control method provided in an embodiment of the present application. The method may specifically include the following steps 101 to 104:

[0034] Step 101, obtaining the initial posture data and initial motion observation data of the robot in the form of augmented quaternions;

[0035] Specifically, quaternion is an extension of complex number, which consists of a real part and three imaginary parts; augmented quaternion is a combination of the original quaternion and a three-dimensional vector, which is transformed into a seven-dimensional vector to uniformly represent the rotation and displacement information, and facilitate mathematical operations and model building. The "robot" in the embodiment of the present application refers to a mechanical device with motion ability and capable of performing specific tasks, usually equipped with sensors and actuators, which can sense the environment and perform corresponding actions. The initial posture data is the position and posture information of the robot at the initial moment, which is expressed in the form of augmented quaternions in the embodiment of the present application; the initial motion observation data is the observation information obtained during the robot's motion, such as the relative translation and relative rotation between adjacent frames.

[0036] In some examples, the pose graph optimization problem can be viewed as a directed graph Each vertex Represents the position of the robot in the i-th frame, using augmented quaternion Indicated by; each directed edge (i, j)∈ε represents the relative motion of the robot between the i-th frame and the j-th frame, The set of augmented quaternions can be written as in is a set of unit quaternions, which is also a unit sphere in four-dimensional space. The unit quaternion q can represent rotation, and the three-dimensional vector t can represent displacement.

[0037] Through the implementation of step 101, the augmented quaternion form is used to describe the robot's initial posture data and initial motion observation data. Compared with the traditional Euler angle or rotation matrix representation method, the augmented quaternion can more accurately describe the three-dimensional rotation of the rigid body, avoiding the gimbal lock problem and the singular point problem, thereby improving the accuracy and stability of the posture description; and the augmented quaternion is used to represent the posture, only 7 elements need to be stored, while the related technology usually uses a special Euclidean group to represent the posture, which requires the storage of 12 elements. Therefore, the embodiment of the present application requires lower space complexity and lower storage cost during the implementation process.

[0038] Step 102, establishing a generalized least squares model with spherical constraints based on the initial posture data and the initial motion observation data;

[0039] Specifically, the generalized least squares model with spherical constraints comprehensively considers the complexity of robot pose estimation and the spherical constraints of quaternions, and solves the robot's target pose data by minimizing the sum of squared errors.

[0040] By implementing step 102, a generalized least squares model with spherical constraints is established, which can fully consider the physical constraints during the robot's motion and the accuracy of the observed data, making the model closer to the actual motion situation. The introduction of spherical constraints helps to keep the rotating quaternion representation on the unit sphere during the solution process, further improving the rationality of the model and the accuracy of the solution.

[0041] Step 103, using the Riemann alternating direction multiplier method to solve the generalized least squares model to obtain the target posture data of the robot;

[0042] Specifically, the alternating direction multiplier method is an iterative algorithm for solving convex optimization problems. It can decompose complex optimization problems into multiple simple sub-problems and gradually approach the optimal solution through iteration. The Riemann alternating direction multiplier method applies the alternating direction multiplier method to optimization problems on Riemann manifolds. In the embodiment of the present application, it is used to solve the generalized least squares model, that is, the quaternion optimization problem with spherical constraints. The target posture data refers to the posture information obtained by the robot after the posture graph is optimized, which is expressed in the form of augmented quaternions and contains information about the robot's rotation and translation.

[0043] Through the implementation of step 103, the Riemannian Alternating Direction Method of Multipliers (RADMM) is used to solve the generalized least squares model. This method combines the advantages of Riemann optimization and the alternating direction multiplier method, and can significantly improve the solution efficiency while maintaining the convergence of the algorithm. In addition, the RADMM algorithm directly performs iterative optimization on the Riemann manifold, avoiding the tedious process of converting quaternions into other representations and then optimizing them in traditional methods. Under the neighboring linearization technology, it can not only ensure that the subproblem has a closed-form solution, but also realize parallel computing, thereby improving the solution efficiency.

[0044] Step 104, performing posture control on the robot based on the target posture data;

[0045] Specifically, the target posture data obtained by the solution can be applied to the robot's control system to adjust the robot's actual posture, which usually involves generating robot-executable instructions or signals based on the target posture data, thereby achieving precise control of the robot's posture.

[0046] Through the implementation of step 104, the posture of the robot is controlled based on the target posture data obtained by the solution. Due to the use of an efficient solution method and accurate posture description, the posture of the robot can be adjusted in real time and accurately to meet the control requirements in complex task scenarios.

[0047] In summary, the embodiments of the present application use augmented quaternions to describe the robot's initial posture data and initial motion observation data. Compared with the traditional Euler angle or rotation matrix representation method, the augmented quaternion can more accurately describe the three-dimensional rotation of the rigid body, avoiding the gimbal lock problem and the singular point problem, thereby improving the accuracy and stability of the posture description; establishing a generalized least squares model with spherical constraints can fully consider the physical constraints and the accuracy of the observation data during the robot's motion, making the model closer to the actual motion situation. The introduction of spherical constraints helps to keep the rotating quaternion representation on the unit sphere during the solution process, further improving the rationality of the model and the accuracy of the solution; using the Riemannian Alternating Direction Method ofMultipliers,RADMM) to solve the generalized least squares model. This method combines the advantages of Riemann optimization and the alternating direction multipliers method, and can significantly improve the solution efficiency while maintaining the convergence of the algorithm. In addition, the RADMM algorithm directly performs iterative optimization on the Riemann manifold, avoiding the tedious process of converting quaternions into other representations and then optimizing them in traditional methods, thereby improving the accuracy and robustness of the solution; the robot is controlled based on the target posture data obtained by the solution. Due to the use of efficient solution methods and accurate posture descriptions, the robot's posture can be adjusted in real time and accurately to meet the control requirements in complex task scenarios. In summary, the robot control method provided in the embodiment of the present application significantly improves the accuracy of the robot's posture description, solution efficiency and control accuracy by adopting augmented quaternion representation, establishing a generalized least squares model of spherical constraints, and using the Riemann alternating direction multipliers method for solution.

[0048] In some embodiments, the aforementioned initial pose data may include a first quaternion and a first vector, wherein the aforementioned first quaternion is a quaternion representing the rotation process of the robot, and the first vector is a vector representing the displacement process of the robot; based on the initial pose data and the initial motion observation data, a generalized least squares model with a spherical constraint is established, which may include: establishing a generalized least squares model based on the assumption that the first quaternion satisfies the spherical constraint, the rotation noise data in the first quaternion obeys the von Mises-Fisher distribution, and the displacement noise data in the first vector obeys the normal distribution.

[0049] Specifically, the first quaternion is a quaternion used to represent the rotation process of the robot; the first vector refers to a three-dimensional vector representing the displacement process of the robot. This vector is used to describe the position of the robot in space, that is, its translation on the three spatial coordinate axes; the von Mises-Fisher distribution is a probability distribution defined on the unit sphere, which is used to describe the data distribution on the sphere; the normal distribution, also known as the Gaussian distribution, is a continuous probability distribution whose probability density function is symmetric about the mean and is bell-shaped.

[0050] Through the implementation of the above embodiment, assuming that the first quaternion satisfies the spherical constraint, the rotation noise data obeys the vonMises-Fisher distribution, and the displacement noise data obeys the normal distribution, the established generalized least squares model is more in line with the actual situation, and the accuracy and robustness of the model are improved.

[0051] In some embodiments, the generalized least squares model is:

[0052]

[0053] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, for The conjugate of is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative rotation quaternion from the i-th frame to the j-th frame in the initial motion observation data, ∑1 is the first weighting matrix describing the displacement noise data, ∑2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0054] Through the implementation of the above embodiments, a specific mathematical expression of the generalized least squares model is given, which comprehensively considers the rotation and displacement process of the robot, as well as the corresponding noise data, and further improves the robustness of posture estimation.

[0055] In some embodiments, the generalized least squares model is:

[0056]

[0057] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, for The conjugate of for The conjugate of For The second quaternion with the same meaning, For The second quaternion with the same meaning, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative motion quaternion from the i-th frame to the j-th frame in the initial motion observation data, ∑1 is the first weighting matrix describing the displacement noise data, ∑2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0058] In some embodiments, the aforementioned step 103 may include: linearizing the generalized least squares model to obtain a linearized augmented Lagrangian function; adding adjacent terms to the linearized augmented Lagrangian function to obtain an optimized sub-function; and solving the optimized sub-function using the Riemann alternating direction multiplier method to obtain the target posture data of the robot.

[0059] Specifically, linearization is a technique used to transform nonlinear problems into linear problems for the purpose of analysis and solution. Specifically, linearization is to perform a first-order Taylor expansion on the generalized least squares model, ignoring higher-order terms, thereby obtaining a linear approximation. Proximal term refers to an additional penalty term used to keep the iterative solution within a certain range, or to guide the solution toward a specific solution. The purpose of linearization is to ensure that the subproblem has a closed-form solution, that is, it can be expressed by an explicit expression in the process of solving the minimum point. Adding proximal terms can enhance the stability of the algorithm, prevent the solution from jumping too much during the iteration process, and also help to deal with constrained optimization problems.

[0060] Through the implementation of the above embodiment, the Riemann alternating direction multiplier method is used to solve the generalized least squares model, which ensures the convergence and efficiency of the algorithm.

[0061] In some embodiments, the optimization subfunction is:

[0062]

[0063] In the formula, is the first quaternion vector obtained at the k+1th iteration, is the second quaternion vector obtained at the k+1th iteration, t k+1 is the displacement vector obtained in the k+1th iteration, λ k+1 is the dual variable obtained in the k+1th iteration, is the second quaternion vector obtained at the kth iteration, is the first quaternion vector obtained at the kth iteration, t k is the first vector obtained in the kth iteration, λ k is the dual variable obtained at the kth iteration, is the second quaternion vector, is the initial first quaternion vector, is the initial first vector, represents the augmented Lagrangian function corresponding to the generalized least squares model, is the linearized function of the augmented Lagrangian function, β represents the penalty parameter, and H1, H2 and H3 are three different proximity matrices.

[0064] Specifically, and It is the adjacent term added to the sub-problem (optimization sub-function) to ensure that the k+1th iteration is not too far away from the kth iteration point, ensuring the stability of the algorithm and having sufficient descent, where, The calculation formula is as follows:

[0065]

[0066] In some embodiments, the optimization subfunction is:

[0067]

[0068] In the formula, is the second quaternion of the i-th frame obtained at the k+1-th iteration, is the second quaternion of the i-th frame obtained at the k-th iteration, is the second quaternion vector of all frames obtained in the k+1th iteration, is the second quaternion vector of all frames obtained at the kth iteration, is the first quaternion of the i-th frame obtained at the k-th iteration, is the first quaternion of the i-th frame obtained at the k+1-th iteration, is the first quaternion vector of all frames obtained at the kth iteration, is the first quaternion vector of all frames obtained in the k+1th iteration, t k+1 is the first vector of all frames obtained in the k+1th iteration, t k is the first vector of all frames obtained in the kth iteration, is the component of the dual variable corresponding to the i-th frame obtained in the k-th iteration. H1, H2 and H3 are three different adjacent matrices. 1,i and H 2,i H1 and H2 are the block matrices corresponding to the i-th frame, G 1,ij and G 2,ij are two different intermediate process matrices associated with edge (i, j), ∑1 is the first weighted matrix describing the displacement noise data, and ∑2 is the second weighted matrix describing the rotation noise data; Q is a block matrix composed of 3×3 small matrices, each row of Q corresponds to a directed edge in a directed graph, I4 is a 4th-order unit matrix, I m is the m-order identity matrix, where m is the number of directed edges in the directed graph; is the set of all directed edges starting from the i-th frame; f represents the first function g represents the second functional Represents the first functional form with respect to the variable Find the partial derivative, The value of ; Represents the second functional form with respect to the variable Find the partial derivative, The value of ; is the intermediate quaternion in the iterative process, for The imaginary three-dimensional vector of .

[0069] Specifically, The i-th column of any row of Q is -I3, and the j-th column is I3. Where W(·) and M(·) are two different operators that expand quaternions into 4th-order matrices.

[0070] Through the implementation of the above embodiment, different proximity matrices and intermediate process matrices are introduced, and tools such as posture transformation matrices and partial derivatives are used, so that the iterative solution process can be explicitly calculated, which is more stable and efficient. At the same time, the correlation and constraints between different frames are also considered, further improving the accuracy and reliability of the solution.

[0071] Furthermore, as an implementation of the aforementioned method embodiment, the present application also provides a robot control device for implementing the aforementioned method embodiment. The device embodiment corresponds to the aforementioned method embodiment. For ease of reading, the present robot control device embodiment will no longer repeat the details of the aforementioned method embodiment one by one, but it should be clear that the device in the embodiment of the present application can correspond to and implement all the contents of the aforementioned method embodiment. Figure 2 As shown, the robot control device 20 includes: a data acquisition unit 201, a model building unit 202, a model solving unit 203 and a robot control unit 204, wherein the data acquisition unit 201 is used to obtain the initial posture data and initial motion observation data of the robot in the form of augmented quaternions; the model building unit 202 is used to establish a generalized least squares model with spherical constraints based on the initial posture data and the initial motion observation data; the model solving unit 203 is used to solve the generalized least squares model using the Riemann alternating direction multiplier method to obtain the target posture data of the robot; the robot control unit 204 is used to perform posture control on the robot based on the target posture data.

[0072] In some embodiments, the initial posture data includes a first quaternion and a first vector, wherein the first quaternion is a quaternion representing the rotation process of the robot, and the first vector is a vector representing the displacement process of the robot; the model building unit 202 is also used to establish a generalized least squares model based on the assumption that the first quaternion satisfies the spherical constraint, the rotation noise data in the first quaternion obeys the von Mises-Fisher distribution, and the displacement noise data in the first vector obeys the normal distribution.

[0073] In some embodiments, the generalized least squares model is:

[0074]

[0075] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, for The conjugate of is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative rotation quaternion from the i-th frame to the j-th frame in the initial motion observation data, ∑1 is the first weighting matrix describing the displacement noise data, ∑2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0076] In some embodiments, the generalized least squares model is:

[0077]

[0078] Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, for The conjugate of for The conjugate of For The second quaternion with the same meaning, For The second quaternion with the same meaning, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative motion quaternion from the i-th frame to the j-th frame in the initial motion observation data, ∑1 is the first weighting matrix describing the displacement noise data, ∑2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

[0079] In some embodiments, the model solving unit 203 is also used to linearize the generalized least squares model to obtain a linearized augmented Lagrangian function; add adjacent terms to the linearized augmented Lagrangian function to obtain an optimized sub-function; use the Riemann alternating direction multiplier method to solve the optimized sub-function to obtain the target posture data of the robot.

[0080] In some embodiments, the optimization subfunction is:

[0081]

[0082] In the formula, is the first quaternion vector obtained at the k+1th iteration, is the second quaternion vector obtained at the k+1th iteration, t k+1 is the displacement vector obtained in the k+1th iteration, λ k+1 is the dual variable obtained in the k+1th iteration, is the second quaternion vector obtained at the kth iteration, is the first quaternion vector obtained at the kth iteration, t k is the first vector obtained in the kth iteration, λ k is the dual variable obtained at the kth iteration, is the second quaternion vector, is the initial first quaternion vector, is the initial first vector, represents the augmented Lagrangian function corresponding to the generalized least squares model, is the linearized function of the augmented Lagrangian function, β represents the penalty parameter, and H1, H2 and H3 are three different proximity matrices.

[0083] In some embodiments, the optimization subfunction is:

[0084]

[0085] In the formula, is the second quaternion of the i-th frame obtained at the k+1-th iteration, is the second quaternion of the i-th frame obtained at the k-th iteration, is the second quaternion vector of all frames obtained in the k+1th iteration, is the second quaternion vector of all frames obtained at the kth iteration, is the first quaternion of the i-th frame obtained at the k-th iteration, is the first quaternion of the i-th frame obtained at the k+1-th iteration, is the first quaternion vector of all frames obtained at the kth iteration, is the first quaternion vector of all frames obtained in the k+1th iteration, t k+1 is the first vector of all frames obtained in the k+1th iteration, t k is the first vector of all frames obtained in the kth iteration, is the component of the dual variable corresponding to the i-th frame obtained in the k-th iteration. H1, H2 and H3 are three different adjacent matrices. 1,i and H 2,i H1 and H2 are the block matrices corresponding to the i-th frame, G 1,ij and G 2,ijare two different intermediate process matrices associated with edge (i, j), ∑1 is the first weighted matrix describing the displacement noise data, and ∑2 is the second weighted matrix describing the rotation noise data; Q is a block matrix composed of 3×3 small matrices, each row of Q corresponds to a directed edge in a directed graph, I4 is a 4th-order unit matrix, I m is the m-order identity matrix, where m is the number of directed edges in the directed graph; is the set of all directed edges starting from the i-th frame; f represents the first function g represents the second functional Represents the first functional form with respect to the variable Find the partial derivative, The value of ; Represents the second functional form with respect to the variable Find the partial derivative, The value of ; is the intermediate quaternion in the iterative process, for The imaginary three-dimensional vector of G 1,ij and G 2,ij They are two different pose transformation matrices respectively.

[0086] The present application also provides a computer-readable storage medium, which stores computer-executable instructions or computer programs. When the computer-executable instructions or computer programs are executed by a processor, the processor will be caused to execute any step of the robot control method provided in the present application.

[0087] In some embodiments, the computer-readable storage medium may be a memory such as RAM, read-only memory (ROM), flash memory, magnetic surface memory, optical disk, or compact disc read-only memory (CD-ROM); or it may be various devices including one or any combination of the above memories.

[0088] In some embodiments, computer executable instructions may be in the form of a program, software, software module, script or code, written in any form of programming language (including compiled or interpreted languages, or declarative or procedural languages), and may be deployed in any form, including as a stand-alone program or as a module, component, subroutine or other unit suitable for use in a computing environment.

[0089] In some embodiments, computer-executable instructions may, but need not, correspond to a file in a file system, may be stored as part of a file storing other programs or data, for example, in one or more scripts in a HyperText Markup Language (HTML) document, in a single file dedicated to the program in question, or in multiple coordinated files (e.g., files storing one or more modules, subroutines, or code portions).

[0090] In some embodiments, computer executable instructions may be deployed to be executed on one electronic device, or on multiple electronic devices located at one site, or on multiple electronic devices distributed at multiple sites and interconnected by a communication network.

[0091] like Figure 3 As shown, the present application also provides an electronic device 30, including a memory 310, a processor 320, and a computer program 311 stored in the memory 310 and executable on the processor, and when the processor 320 executes the computer program 311, any step of the above-mentioned robot control method is implemented.

[0092] The present application also provides a computer program product, which includes a computer program or a computer executable instruction, and the computer program or the computer executable instruction is stored in a computer readable storage medium. The processor of the electronic device reads the computer program or the computer executable instruction from the computer readable storage medium, and the processor executes the computer program or the computer executable instruction, so that the electronic device performs any step of the robot control method described above in the present application.

[0093] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A robot control method, characterized in that: include: Obtain the robot's initial position data and initial motion observation data in the form of augmented quaternions; Based on the initial posture data and the initial motion observation data, a generalized least squares model with spherical constraints is established; The generalized least squares model is solved by using the Riemann alternating direction multiplier method to obtain the target posture data of the robot; The robot is posture controlled based on the target posture data.

2. The robot control method according to claim 1, characterized in that: The initial position and posture data includes a first quaternion and a first vector, wherein the first quaternion is a quaternion representing a rotation process of the robot, and the first vector is a vector representing a displacement process of the robot; The generalized least squares model with spherical constraints is established based on the initial posture data and the initial motion observation data, including: The generalized least squares model is established based on the assumption that the first quaternion satisfies the spherical constraint, the rotation noise data in the first quaternion obeys the vonMises-Fisher distribution, and the displacement noise data in the first vector obeys the normal distribution.

3. The robot control method according to claim 2, characterized in that: The generalized least squares model is: Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, is the first quaternion of the i-th frame, is the first quaternion of the jth frame, For the The conjugate of is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative rotation quaternion from the i-th frame to the j-th frame in the initial motion observation data, Σ1 is the first weighting matrix describing the displacement noise data, Σ2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

4. The robot control method according to claim 2, characterized in that: The generalized least squares model is: Where ε is the set of all edges of the directed graph composed of the initial pose data and the initial motion observation data, is through t i A four-dimensional vector formed by adding a 0 value dimension to it, t i is the first vector of the i-th frame, is through t j A four-dimensional vector formed by adding a 0 value dimension to it, t j is the first vector of the jth frame, for The conjugate of for The conjugation of For The second quaternion has the same meaning as For The second quaternion has the same meaning as is the first quaternion of the i-th frame, is the first quaternion of the jth frame, is the relative translation vector from the i-th frame to the j-th frame in the initial motion observation data, is the relative motion quaternion from the i-th frame to the j-th frame in the initial motion observation data, Σ1 is the first weighting matrix describing the displacement noise data, Σ2 is the second weighting matrix describing the rotation noise data, is a spherical constraint.

5. The robot control method according to claim 4, characterized in that: The Riemann alternating direction multiplier method is used to solve the generalized least squares model to obtain the target posture data of the robot, including: Linearizing the generalized least squares model to obtain a linearized augmented Lagrangian function; Adding adjacent terms to the linearized augmented Lagrangian function to obtain an optimized subfunction; The optimization subfunction is solved by using the Riemann alternating direction multiplier method to obtain the target posture data of the robot.

6. The robot control method according to claim 5, characterized in that: The optimization sub-function is: In the formula, is the first quaternion vector obtained at the k+1th iteration, is the second quaternion vector obtained at the k+1th iteration, t k+1 is the displacement vector obtained in the k+1th iteration, λ k+1 is the dual variable obtained in the k+1th iteration, is the second quaternion vector obtained at the kth iteration, is the first quaternion vector obtained at the kth iteration, t k is the first vector obtained in the kth iteration, λ k is the dual variable obtained at the kth iteration, is the second quaternion vector, is the initial first quaternion vector, t is the initial first vector, represents the augmented Lagrangian function corresponding to the generalized least squares model, is the linearization function of the augmented Lagrangian function, β represents the penalty parameter, and H1, H2 and H3 are three different proximity matrices.

7. The robot control method according to claim 5, characterized in that: The optimization sub-function is: In the formula, is the second quaternion of the i-th frame obtained at the k+1-th iteration, is the second quaternion of the i-th frame obtained at the k-th iteration, is the second quaternion vector of all frames obtained in the k+1th iteration, is the second quaternion vector of all frames obtained at the kth iteration, is the first quaternion of the i-th frame obtained at the k-th iteration, is the first quaternion of the i-th frame obtained at the k+1-th iteration, is the first quaternion vector of all frames obtained at the kth iteration, is the first quaternion vector of all frames obtained in the k+1th iteration, t k+1 is the first vector of all frames obtained in the k+1th iteration, t k is the first vector of all frames obtained in the kth iteration, is the component of the dual variable corresponding to the i-th frame obtained in the k-th iteration. H1, H2 and H3 are three different adjacent matrices. 1,i and H 2,i H1 and H2 are the block matrices corresponding to the i-th frame, G 1,ij and G 2,ij are two different intermediate process matrices associated with edge (i, j), ∑1 is the first weighted matrix describing the displacement noise data, ∑2 is the second weighted matrix describing the rotation noise data; Q is a block matrix composed of 3×3 small matrices, each row of Q corresponds to a directed edge in the directed graph, I4 is a 4th-order unit matrix, I m is an m-order identity matrix, where m is the number of directed edges in the directed graph; is the set of all directed edges starting from the i-th frame; f represents the first function g represents the second functional Represents the first functional form with respect to the variable Find the partial derivative, The value of ; Represents the second functional form with respect to the variable Find the partial derivative, The value of ; is the intermediate quaternion in the iterative process, for The imaginary three-dimensional vector of .

8. A robot control device, characterized in that: include: A data acquisition unit, used to acquire initial position data and initial motion observation data of the robot in the form of augmented quaternions; A model building unit, used for building a generalized least squares model with spherical constraints based on the initial posture data and the initial motion observation data; A model solving unit, used for solving the generalized least squares model by using the Riemann alternating direction multiplier method to obtain the target posture data of the robot; A robot control unit is used to perform posture control on the robot based on the target posture data.

9. An electronic device, comprising: A memory and a processor, wherein the processor is used to implement the steps of the robot control method according to any one of claims 1 to 7 when executing the computer program stored in the memory.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the robot control method according to any one of claims 1 to 7 are implemented.

Images