Robot stable gait planning method based on arc trajectory

Abstract

The invention relates to the technical field of robot control, in particular to a robot stable gait planning method based on an arc trajectory, and the method comprises the following steps: selecting trajectory points which can form a triangle in a robot motion space; calculating a corresponding circular arc track center and a circular arc track radius according to the track points; constructing an arc track plane equation according to the track point, the arc track center and the arc track radius; dividing the arc track passing through the track points into a plurality of segments to obtain a plurality of sampling points; calculating coordinates of each sampling point in the motion space of the robot, and substituting the coordinates into the arc track plane equation for inverse motion solution to obtain motion angles of the robot at the sampling points; and obtaining a gait planning result of the robot based on the motion angles corresponding to all the sampling points. According to the invention, the motion track of the robot is predictable, so that the control stability of the robot is ensured.

Description

Technical Field

[0001] The invention relates to the technical field of robot control, and in particular to a robot stable gait planning method based on circular arc trajectory. Background Art

[0002] There are many types of existing drones, among which motion robots with multiple joints are widely used in ground exploration and processing work. The environment of these works often has a more complex spatial layout, and there will inevitably be obstacles on the motion path. Therefore, for motion robots, the function of motion planning is crucial.

[0003] There are many forms of robot motion trajectory planning, the most common of which are trajectory planning based on joint space and trajectory planning based on Cartesian coordinate system. However, for a motion robot with multiple joints, the movement of each joint (which can be called the robot's foot) is different, and the landing point of different feet determines the robot's moving posture. However, although the above trajectory planning methods based on joint space and Cartesian coordinate system can control the speed of joint angle transformation, they cannot predict the position of the robot's foot landing point, resulting in certain limitations in the motion planning of multi-legged robots.

[0004] Therefore, it is necessary to propose a new gait planning method to solve the above problems. Summary of the invention

[0005] The present invention aims to solve the technical problem that the existing multi-legged robot gait planning cannot predict the foot end landing point position.

[0006] In order to solve the above technical problems, the present invention provides a robot stable gait planning method based on arc trajectory, comprising the following steps:

[0007] S1. Selecting trajectory points that can form a triangle in the robot motion space, wherein the trajectory points include a first trajectory point A, a second trajectory point B, and a third trajectory point C, wherein the gait planning of the robot is to move its foothold from the first trajectory point A through the second trajectory point B to the third trajectory point C;

[0008] S2, calculating the corresponding arc trajectory center and arc trajectory radius according to the trajectory point;

[0009] S3, constructing a circular arc trajectory plane equation according to the trajectory point, the circular arc trajectory center and the circular arc trajectory radius;

[0010] S4, dividing the arc trajectory passing through the trajectory point into multiple segments to obtain multiple sampling points;

[0011] S5, respectively calculating the coordinates of each sampling point in the robot motion space, and substituting the coordinates into the arc trajectory plane equation to perform inverse motion solution to obtain the motion angle of the robot at the sampling point;

[0012] S6. Obtaining a gait planning result of the robot based on the movement angles corresponding to all the sampling points.

[0013] Furthermore, the coordinates of the first trajectory point A, the second trajectory point B, and the third trajectory point C are defined as A(x1, y1, z1), B(x2, y2, z2), and C(x3, y3, z3), respectively, and the center of the arc trajectory is O(x, y, z), which satisfies the following relationship:

[0014] (x-x1) 2 +(y-y1) 2 +(z-z1) 2 =(x-x2) 2 +(y-y2) 2 +(z-z2) 2 ;

[0015] (x-x2) 2 +(y-y2) 2 +(z-z2) 2 =(x-x3) 2 +(y-y3) 2 +(z-z3) 2 .

[0016] Furthermore, the arc trajectory plane equation satisfies the following relationship:

[0017] Ax+B+Cz=1;

[0018] in:

[0019]

[0020] Furthermore, step S5 is specifically as follows:

[0021] The total number of sampling points is defined as n, the initial iteration value i=1, and the angle θ between the sampling point i and the first trajectory point A satisfies the following relationship:

[0022]

[0023] The sampling point i and the rotation axis ω of the first trajectory point A in the circular arc trajectory satisfy the following relationship:

[0024]

[0025] Normalize the rotation axis ω and construct the quaternion q:

[0026]

[0027] Wherein, x, y, z are unit vectors of the rotation axis; wherein the real part a and the imaginary parts b, c, d of the quaternion satisfy the following relationship:

[0028]

[0029] Construct the rotation matrix R based on the rotation axis and the quaternion:

[0030]

[0031] Multiply the rotation matrix R by the vector corresponding to the sampling point i on the left to obtain the coordinates of the sampling point i, substitute the coordinates of the sampling point i into the arc trajectory plane equation to perform an inverse motion solution, and obtain the motion angle of the robot at the sampling point i;

[0032] Let i=i+1, repeat the above steps until i>n, and obtain the movement angles of all sampling points.

[0033] Furthermore, when constructing the rotation matrix R, the signs of the imaginary parts b, c, and d of the quaternion are all reversed.

[0034] Furthermore, the vector angle θ between the first trajectory point A and the center O of the arc trajectory, and the vector angle θ between the third trajectory point and the center O of the arc trajectory is AC The following relationship is satisfied:

[0035]

[0036] The beneficial effect achieved by the present invention is that a robot stable gait planning method based on an arc trajectory is proposed, which can predict the robot's landing point. In the gait planning process, this method only needs to select three trajectory points, and then solve the center and radius of the robot's arc trajectory through the trajectory points, and solve the equation of the plane where the arc is located through the center and radius, and then intercept appropriate points on the arc through the interpolation method, substitute them into the robot's inverse kinematic equation, and then solve the angle of movement of the robot's joints on the arc path. This method makes the robot's motion trajectory predictable, thereby ensuring the stability of the robot control. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a schematic diagram of the steps of a method for planning a stable gait of a robot based on an arc trajectory provided by an embodiment of the present invention;

[0038] Figure 2is a schematic diagram of the movement of the robot foot provided by an embodiment of the present invention;

[0039] Figure 3 Schematic diagram of sampling points provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0040] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0041] Please refer to Figure 1 , Figure 1 1 is a schematic flow chart of the steps of a robot stable gait planning method based on an arc trajectory provided by an embodiment of the present invention, wherein the robot stable gait planning method based on an arc trajectory comprises the following steps:

[0042] S1. Selecting trajectory points that can form a triangle in the robot motion space, wherein the trajectory points include a first trajectory point A, a second trajectory point B, and a third trajectory point C, wherein the gait planning of the robot is to move its foothold from the first trajectory point A through the second trajectory point B to the third trajectory point C;

[0043] S2, calculating the corresponding arc trajectory center and arc trajectory radius according to the trajectory point;

[0044] S3, constructing a circular arc trajectory plane equation according to the trajectory point, the circular arc trajectory center and the circular arc trajectory radius;

[0045] S4, dividing the arc trajectory passing through the trajectory point into multiple segments to obtain multiple sampling points;

[0046] S5, respectively calculating the coordinates of each sampling point in the robot motion space, and substituting the coordinates into the arc trajectory plane equation to perform inverse motion solution to obtain the motion angle of the robot at the sampling point;

[0047] S6. Obtaining a gait planning result of the robot based on the movement angles corresponding to all the sampling points.

[0048] For details, please refer to Figure 2 , Figure 2 A schematic diagram showing that the movement point of the robot in an embodiment of the present invention moves from the first trajectory point A through the second trajectory point B to the third trajectory point C. The coordinates of the first trajectory point A, the second trajectory point B, and the third trajectory point C are defined as A(x1, y1, z1), B(x2, y2, z2), and C(x3, y3, z3). It can be seen that the first trajectory point A, the second trajectory point B, and the third trajectory point C are respectively

[0049]

[0050] The center of the arc trajectory is defined as O(x, y, z), which satisfies the following relationship:

[0051] (x-x1) 2 +(y-y1) 2 +(z-z1) 2 =(x-x2) 2 +(y-y2) 2 +(z-z2) 2 ;

[0052] (x-x2) 2 +(y-y2) 2 +(z-z2) 2 =(x-x3) 2 +(y-y3) 2 +(z-z3) 2 ;

[0053] The above relationship can be decomposed and sorted out to obtain:

[0054]

[0055]

[0056] Furthermore, the arc trajectory plane equation satisfies the following relationship:

[0057] Ax+B+Cz=1;

[0058] Based on the plane equation, substituting the three trajectory points into the linear equation system:

[0059]

[0060] in:

[0061]

[0062] Furthermore, by It can be seen that the vector angle θ between the first trajectory point A and the center O of the arc trajectory, and the vector angle θ between the third trajectory point and the center O of the arc trajectory is AC The following relationship is satisfied:

[0063]

[0064] Step S5 is specifically as follows:

[0065] Define the total number of sampling points as n, and the initial iteration value i=1, such as Figure 3As shown, the angle θ between the sampling point i and the first trajectory point A satisfies the following relationship:

[0066]

[0067] It can be seen that the arc trajectory can be regarded as the robot foot end Start, go through the point O and connect with Orthogonal unit vector rotation angle θ i The obtained path.

[0068] The sampling point i and the rotation axis ω of the first trajectory point A in the circular arc trajectory satisfy the following relationship:

[0069]

[0070] Normalize the rotation axis ω and construct the quaternion q:

[0071]

[0072] Wherein, x, y, z are unit vectors of the rotation axis; wherein the real part a and the imaginary parts b, c, d of the quaternion satisfy the following relationship:

[0073]

[0074] Construct the rotation matrix R based on the rotation axis and the quaternion:

[0075]

[0076] Multiply the rotation matrix R by the vector corresponding to the sampling point i on the left to obtain the coordinates of the sampling point i, substitute the coordinates of the sampling point i into the arc trajectory plane equation to perform an inverse motion solution, and obtain the motion angle of the robot at the sampling point i;

[0077] Let i=i+1, repeat the above steps until i>n, and obtain the movement angles of all sampling points.

[0078] In the motion inverse solution described in the embodiment of the present invention, a robot motion model is used to realize the motion control of the robot. The robot motion model can be an existing multi-group robot control model, which represents the relationship between various control parameters and joint motion when the robot moves. The present invention does not limit the specific control model.

[0079] Furthermore, in practical applications, especially when rotating quaternions, you need to ensure that the quaternion multiplication is consistent with the rotation matrix definition. Specifically, when using a quaternion to rotate a vector , use the following formula:

[0080] V′=qVq-1 ;

[0081] where q -1 is the conjugate of the quaternion q. In order to ensure that the rotation direction is consistent with the expected direction, when constructing the rotation matrix R, the signs of the imaginary parts b, c, and d of the quaternion are all reversed, that is:

[0082]

[0083] The beneficial effect achieved by the present invention is that a robot stable gait planning method based on an arc trajectory is proposed, which can predict the robot's landing point. In the gait planning process, this method only needs to select three trajectory points, and then solve the center and radius of the robot's arc trajectory through the trajectory points, and solve the equation of the plane where the arc is located through the center and radius, and then intercept appropriate points on the arc through the interpolation method, substitute them into the robot's inverse kinematic equation, and then solve the angle of movement of the robot's joints on the arc path. This method makes the robot's motion trajectory predictable, thereby ensuring the stability of the robot control.

[0084] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be implemented by instructing the relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium, and when the program is executed, it can include the processes of the embodiments of the above-mentioned methods. The storage medium can be a disk, an optical disk, a read-only memory (ROM) or a random access memory (RAM).

[0085] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0086] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, a magnetic disk, or an optical disk), and includes a number of instructions for enabling a terminal (which can be a mobile phone, a computer, a server, an air conditioner, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0087] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only the preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation manner. The above-mentioned specific implementation manner is only illustrative rather than restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A robot stable gait planning method based on arc trajectory, characterized in that: The following steps are involved: S1. Selecting trajectory points that can form a triangle in the robot motion space, wherein the trajectory points include a first trajectory point A, a second trajectory point B, and a third trajectory point C, wherein the gait planning of the robot is to move its foothold from the first trajectory point A through the second trajectory point B to the third trajectory point C; S2, calculating the corresponding arc trajectory center and arc trajectory radius according to the trajectory point; S3, constructing a circular arc trajectory plane equation according to the trajectory point, the circular arc trajectory center and the circular arc trajectory radius; S4, dividing the arc trajectory passing through the trajectory point into multiple segments to obtain multiple sampling points; S5, respectively calculating the coordinates of each sampling point in the robot motion space, and substituting the coordinates into the arc trajectory plane equation to perform inverse motion solution to obtain the motion angle of the robot at the sampling point; S6. Obtaining a gait planning result of the robot based on the movement angles corresponding to all the sampling points.

2. The method for robot stable gait planning based on arc trajectory according to claim 1, characterized in that: The coordinates of the first trajectory point A, the second trajectory point B, and the third trajectory point C are A(x1, y1, z1), B(x2, y2, z2), and C(x3, y3, z3), respectively. The center of the arc trajectory is O(x, y, z), which satisfies the following relationship: (x-x1) 2 +(y-y1) 2 +(z-z1) 2 =(x-x2) 2 +(y-y2) 2 +(z-z2) 2 4 (x-x2) 2 +(y-y2) 2 +(z-z2) 2 (x-x3) 2 +(y-y3) 2 +(z-z3) 2 。 3. The method for robot stable gait planning based on arc trajectory according to claim 2, characterized in that: The arc trajectory plane equation satisfies the following relationship: Ax+By+Cz=1; in:

4. The method for robot stable gait planning based on arc trajectory according to claim 3, characterized in that: Step S5 is specifically as follows: The total number of sampling points is defined as n, the initial iteration value i=1, and the angle θ between the sampling point i and the first trajectory point A satisfies the following relationship: The sampling point i and the rotation axis ω of the first trajectory point A in the circular arc trajectory satisfy the following relationship: Normalize the rotation axis ω and construct the quaternion q: Wherein, x, y, z are unit vectors of the rotation axis; wherein the real part a and the imaginary parts b, c, d of the quaternion satisfy the following relationship: Construct the rotation matrix R based on the rotation axis and the quaternion: Multiply the rotation matrix R by the vector corresponding to the sampling point i on the left to obtain the coordinates of the sampling point i, substitute the coordinates of the sampling point i into the arc trajectory plane equation to perform an inverse motion solution, and obtain the motion angle of the robot at the sampling point i; Let i=i+1, repeat the above steps until i>n, and obtain the movement angles of all sampling points.

5. The method for robot stable gait planning based on arc trajectory according to claim 4, characterized in that: When constructing the rotation matrix R, the signs of the imaginary parts b, c, and d of the quaternion are all reversed.

6. The method for robot stable gait planning based on arc trajectory according to claim 4, characterized in that: The vector angle θ between the first trajectory point A and the center O of the arc trajectory, and between the third trajectory point and the center O of the arc trajectory AC The following relationship is satisfied:

Images