A trajectory planning system based on the kinematic model of a quadruped robot

Abstract

The present invention discloses a trajectory planning system based on a kinematic model of a quadruped robot. Based on the movement characteristics of the quadruped robot, the lateral movement ability of the quadruped robot is taken into account, and the influence of the angle between the robot speed and the robot orientation on its movement rate is also taken into account; the moving task of the quadruped robot is simplified to point-to-point movement, and it is only necessary to complete the state increment required by the quadruped robot within a specified time; using the kinematic model of the quadruped robot and the moving task, a passable trajectory is automatically generated, and the shortest time to complete the moving task is found at the same time. The present invention can realize the unstructured trajectory planning of the quadruped robot, has the advantages of high efficiency and speed, and solves the disadvantage that the current quadruped robot can only use inefficient and low-adaptability trajectory planning methods.

Description

Technical Field

[0001] The invention belongs to the field of robot motion trajectory planning, and in particular relates to a trajectory planning system based on a kinematic model of a quadruped robot. Background Art

[0002] For a mobile robot, its task is generally to move from its current position to a specified position, and to avoid obstacles or complete specified actions during the movement. To complete this task, the robot needs to find a path that leads to the target point. This process is called path planning. Path planning generally does not consider the actual execution capability of the robot, nor does it consider the influence of uncertain factors such as local environmental factors. If the robot moves completely along this path, unpredictable situations will occur. Therefore, during the robot's movement, it is necessary to combine environmental factors and its own kinematic or dynamic characteristics to find a local path that allows the robot to move safely. This process is trajectory planning.

[0003] The current trajectory planning methods for quadruped robots are directly copied from the trajectory planning methods for wheeled robots. They do not take into account the omnidirectional movement ability of quadruped robots and cannot give full play to the advantages of quadruped robots in lateral and diagonal movement. Moreover, these planning methods will also encounter various problems during operation due to the mismatch between the kinematic model of the wheeled robot and the motion model of the quadruped robot. For example, when using the dynamic window method (DWA) algorithm, if the initial planned forward direction is opposite to the current direction of the robot, the quadruped robot may spin in place and be unable to move. Summary of the invention

[0004] The purpose of the present invention is to provide a trajectory planning system based on the kinematic model of a quadruped robot in view of the fact that the existing method for planning the motion trajectory of a quadruped robot cannot well match the motion characteristics and motion ability of the quadruped robot. The present invention can automatically find a high-quality trajectory suitable for the movement of the quadruped robot, and then control the movement of the quadruped robot through the control input generated by the quadruped robot trajectory planner, so that the quadruped robot moves according to the trajectory generated by the quadruped robot trajectory planner, reaches the target point in the shortest time, and gives full play to the omnidirectional motion advantage of the quadruped robot.

[0005] The objective of the present invention is achieved through the following technical solutions: a trajectory planning system based on the kinematic model of a quadruped robot, comprising a quadruped robot kinematic model, a quadruped robot moving task and a quadruped robot trajectory planner, each part being organically combined to form a complete system, which, based on the kinematic model of the quadruped robot, can plan and generate a trajectory for the quadruped robot to quickly reach the target for a given moving task.

[0006] The basic process of the kinematic model of the quadruped robot is as follows:

[0007] A quadruped robot has the ability to move in all directions, that is, the speed direction of the robot may be inconsistent with the orientation of the robot, there may be an angle between the two, and this angle constrains the movement rate of the quadruped robot in this direction.

[0008] Furthermore, let x and y be the horizontal and vertical coordinates of the quadruped robot, v be the speed of the quadruped robot, β be the angle between the robot speed and the positive direction of the world coordinate system X axis, and γ be the angle between the robot speed and the robot orientation. The angle γ between the robot speed and the robot orientation will determine the maximum linear velocity of the quadruped robot in this velocity direction. During the actual movement of the quadruped robot, the vector sum of its maximum velocity in all directions forms an ellipse centered on the robot, and the actual movement speed of the robot is constrained within this ellipse.

[0009] Furthermore, using the ellipse property, the relationship between the robot speed v and γ can be obtained:

[0010]

[0011] Wherein, a represents the maximum linear velocity of the quadruped robot in the forward direction, and b represents the maximum linear velocity of the quadruped robot in the lateral direction.

[0012] Furthermore, by simplifying the above formula, we can obtain the constraints of the robot's linear velocity in all directions:

[0013]

[0014] Furthermore, let the maximum speed of the quadruped robot be:

[0015]

[0016] Then the robot linear velocity satisfies:

[0017]

[0018] Among them, k represents the ratio of the robot speed to the maximum speed in the speed direction, which is also the equivalent speed of the quadruped robot in this direction.

[0019] Furthermore, the five state quantities x, y, k, γ and β of the quadruped robot are selected to establish the state space of the quadruped robot, where: and The relationship with speed v is shown in the formula, which can be converted into the relationship between x, y and k:

[0020]

[0021] Furthermore, the kinematic model of the quadruped robot can be obtained:

[0022]

[0023] Among them, z is the state variable of the quadruped robot, the model has five state variables x, y, k, γ and β, and three input variables and f1(k,γ,β) and f2(k,γ,β) are as follows:

[0024]

[0025] There are many constraints on the kinematic model of a quadruped robot. The general constraints are as follows:

[0026]

[0027] The basic process of the quadruped robot's moving task is as follows:

[0028] On a two-dimensional plane, the moving task of the quadruped robot is to move from the current point to a certain point in the two-dimensional plane. Let the coordinates of the current point be (x0, y0), the current equivalent rate be k0, the angle between the current robot speed and the positive direction of the world coordinate system X-axis be β0, and the angle between the current robot speed and the robot orientation be γ0. Then its current state is z0:

[0029]

[0030] Furthermore, let the coordinates of the target point be (x f ,y f ), the target equivalent rate is k f , the angle between the target robot speed and the world coordinate system is β f , the angle between the target robot speed and the robot orientation is γ f , then the state quantity of its target is z f :

[0031]

[0032] Furthermore, the moving task of the quadruped robot is to change the state z0 to z in a finite time. f .

[0033] Furthermore, let the state increment Δz = z f -z0, then

[0034]

[0035] Furthermore, the moving task of the quadruped robot is transformed into the state of the quadruped robot starting from the initial z0, and after a change of Δz in a finite time, it finally reaches z f .

[0036] The basic process of the quadruped robot trajectory planner is as follows:

[0037] The goal of trajectory planning for a quadruped robot is to find a feasible solution with the shortest possible motion time. To achieve this goal, the quadruped robot needs to accelerate to the highest equivalent velocity in the shortest time, and then decelerate when it is close to the target point to achieve the target equivalent velocity requirement. This ensures that the robot reaches the target point in the shortest time.

[0038] Based on this, first give a total time T and set the equivalent rate k as a linear function or a constant function of the time variable t:

[0039] or The former is the change of k when time is sufficient, and the latter is the change of k when time is insufficient. In the formula, c1, c2, m1, m2, c1′, c2′, m1′, and m′2 are the parameters that determine the change of k, which can be obtained from the time T and the actual motion ability of the quadruped robot and can be regarded as known constants. Then the state input of the quadruped robot is

[0040] c is 0 or a constant

[0041] Furthermore, two more input quantities are set and is a function of t, and let its primitive functions γ and β be quadratic functions of time t, that is

[0042] γ(t)=a0+a1t+a2t 2

[0043] β(t)=b0+b1t+b2t 2

[0044] Then, the problem of trajectory planning of the quadruped robot is transformed into finding suitable a0, a1, a2, b0, b1, b2 so that after time T, the robot's state changes from z0 to z after Δz. f .

[0045] Furthermore, the constraint equations of the quadruped robot can be established to solve a0, a1, a2, b0, b1, b2:

[0046]

[0047] Furthermore, if the above equations have no solution, it means that there is no trajectory that can reach the target point at a given time T, and the time needs to be given again; if the above equations have a solution, it means that there is a trajectory that can reach the target point at a given time T. At this time, by substituting the solved a0, a1, a2, b0, b1, b2, we can get γ(t) and β(t). Since k(t) is known, f1(k,γ,β) and f2(k,γ,β) are actually functions of time t, that is, f1(t) and f2(t). Since f1(t) and f2(t) are the derivatives of the quadruped robot's horizontal coordinate x(t) and vertical coordinate y(t) with time, the planned quadruped robot motion trajectory is:

[0048]

[0049] Furthermore, the expression of the quadruped robot state z(t) changing with time is:

[0050]

[0051] Furthermore, in order to make the quadruped robot move along this trajectory, its control input is:

[0052]

[0053] Furthermore, the expression of velocity v(t) changing with time can also be obtained:

[0054]

[0055] Furthermore, binary search is used to find the minimum time T: first evaluate the states of the starting point and the target point, and given an initialization time T0, if a0, a1, a2, b0, b1, and b2 still have solutions under the constraints of the starting point and the target point, it means that the quadruped robot can reach the target point within T0. At this time, the time is set to T0 / 2, and a0, a1, a2, b0, b1, and b2 are continued to be solved; if there is no solution, the time is set to 3T0 / 4 and the solution is continued. This cycle is repeated until the time reaches a certain accuracy or the number of calculations reaches a certain value. Finally, the shortest time T for a0, a1, a2, b0, b1, and b2 to have a solution is taken to generate the motion trajectory of the quadruped robot.

[0056] The beneficial effects of the present invention are as follows:

[0057] 1. Describe the motion capability of the quadruped robot, creatively take into account the omnidirectional motion capability and rate constraints of the quadruped robot, and construct a kinematic model of the quadruped robot in order to effectively control the motion of the quadruped robot;

[0058] 2. By simplifying the moving task of the quadruped robot into the change of state quantity, the efficiency of subsequent trajectory planning is greatly improved;

[0059] 3. Converting the quadruped robot speed into an equivalent rate and directly giving it in trajectory planning can effectively reduce the complexity of trajectory planning and improve the robustness of the trajectory planning system;

[0060] 4. The trajectory planning system can automatically find the shortest time to make the trajectory so that the quadruped robot can complete the moving task in the shortest time;

[0061] The present invention overcomes the problem that the current quadruped robot trajectory planning method does not match its motion characteristics, and efficiently and quickly plans a high-quality trajectory suitable for the movement of the quadruped robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 Schematic diagram of a trajectory planning system based on a kinematic model of a quadruped robot according to the present invention;

[0063] Figure 2 Schematic diagram of the kinematic model of the quadruped robot of the present invention;

[0064] Figure 3 Schematic diagram of omnidirectional motion speed constraint of the quadruped robot of the present invention;

[0065] Figure 4 Schematic diagram of the change of the equivalent rate k of the omnidirectional motion of the quadruped robot of the present invention over time. DETAILED DESCRIPTION

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

[0067] like Figure 1 As shown, the present invention provides a trajectory planning system based on the kinematic model of a quadruped robot, including a kinematic model of a quadruped robot, a moving task of a quadruped robot, and a quadruped robot trajectory planner; the three are organically combined to form a complete system. Based on the kinematic model of the quadruped robot, for a given moving task, a trajectory can be planned and generated to allow the quadruped robot to quickly reach the target, and then the movement of the quadruped robot is controlled by the control input generated by the quadruped robot trajectory planner, so that it moves according to the trajectory generated by the quadruped robot trajectory planner and reaches the target point in the shortest time.

[0068] The basic process of the kinematic model of the quadruped robot is as follows:

[0069] For a quadruped robot, it has the ability to move in all directions. The robot's speed direction and the robot's orientation may not be consistent, and there may be an angle between the two. This angle has a constraint relationship on the movement rate of the quadruped robot in this direction.

[0070] like Figure 2 As shown, x and y are the horizontal and vertical coordinates of the quadruped robot, v is the speed of the quadruped robot, β is the angle between the robot speed and the positive direction of the X-axis of the world coordinate system, and γ is the angle between the robot speed and the robot orientation.

[0071] like Figure 3 As shown in the figure, the angle γ between the robot speed and the robot orientation will determine the maximum linear velocity of the quadruped robot in the direction of the robot speed. During the actual movement of the quadruped robot, the vector sum of its maximum velocity in all directions forms an ellipse with the robot as the center. The actual movement speed of the robot is constrained within this ellipse.

[0072] Using the properties of the ellipse, the relationship between the robot speed v and γ can be obtained:

[0073]

[0074] Wherein, a represents the maximum linear velocity of the quadruped robot in the forward direction, and b represents the maximum linear velocity of the quadruped robot in the lateral direction.

[0075] Simplifying the above formula, we can get the constraints of the robot's linear velocity in all directions:

[0076]

[0077] Let the maximum speed of the quadruped robot be:

[0078]

[0079] Then the robot linear velocity satisfies:

[0080]

[0081] Among them, k represents the ratio of the robot speed to the maximum speed in the direction of the speed, which is also the equivalent rate of the quadruped robot in the direction of the robot speed.

[0082] Select the five state quantities x, y, k, γ and β of the quadruped robot and establish the state space of the quadruped robot, where: and The relationship with speed v is shown in the formula, which can be converted into the relationship between x, y and k:

[0083]

[0084] The kinematic model of the quadruped robot can be obtained:

[0085]

[0086] Among them, z is the state variable of the quadruped robot, the model has five state variables x, y, k, γ and β, and three input variables and f1(k,γ,β) and f2(k,γ,β) are as follows:

[0087]

[0088]

[0089] There are many constraints on the kinematic model of a quadruped robot. The general constraints are as follows:

[0090]

[0091] The basic process of the quadruped robot's moving task is as follows:

[0092] On a two-dimensional plane, the moving task of the quadruped robot is to move from the current point to a certain point in the two-dimensional plane; that is, the current state z0 changes by a value of Δz in a finite time and finally reaches the target state z f .

[0093] Let the coordinates of the current point be (x0, y0), the current equivalent rate be k0, the angle between the current robot speed and the positive direction of the world coordinate system X-axis be β0, and the angle between the current robot speed and the robot orientation be γ0, then the current state is z0:

[0094]

[0095] Let the coordinates of the target point be (x f ,y f ), the target equivalent rate is k f , the angle between the target robot speed and the world coordinate system is β f , the angle between the target robot speed and the robot orientation is γ f , then the state quantity of the target is z f :

[0096]

[0097] State increment Δz = z f -z0, then:

[0098]

[0099] The basic process of the quadruped robot trajectory planner is as follows:

[0100] The goal of trajectory planning for a quadruped robot is to find a feasible solution with the shortest possible motion time. To achieve this goal, the quadruped robot needs to accelerate to the highest equivalent velocity in the shortest time, and then decelerate when it is close to the target point to achieve the target equivalent velocity requirement, so that the robot can reach the target point in the shortest time.

[0101] like Figure 4 As shown, based on this, a total time T is given first, and the equivalent rate k is set as a linear function or a constant function of the time variable t.

[0102] The change of k with sufficient time:

[0103]

[0104] Changes in k when time is insufficient:

[0105]

[0106] Where c1, c2, m1, m2, c1′, c2′, m1′, and m′2 are the parameters that determine the change of k, which can be obtained from the time T and the actual motion ability of the quadruped robot and can be regarded as known constants; t1 is the time for acceleration to k=1, and t2 is the time for deceleration from k=1 to k f time; t1′ is the time to accelerate to the maximum equivalent rate.

[0107] Then the state input of the quadruped robot is

[0108] c is 0 or a constant

[0109] Set the other two inputs and is a function of t, and its primitive functions γ and β are quadratic functions of time t, that is

[0110] γ(t)=a0+a1t+a2t 2

[0111] β(t)=b0+b1t+b2t 2

[0112] Then, the problem of trajectory planning of the quadruped robot is transformed into finding suitable a0, a1, a2, b0, b1, b2 so that after time T, the robot's state changes from z0 to z after Δz. f .

[0113] Furthermore, the constraint equations of the quadruped robot can be established to solve a0, a1, a2, b0, b1, b2:

[0114]

[0115] If the above equations have no solution, it means that there is no trajectory that can reach the target point at the given time T, and the time needs to be given again; if the above equations have a solution, it means that there is a trajectory that can reach the target point at the given time T. At this time, by substituting the solved a0, a1, a2, b0, b1, b2, we can get γ(t) and β(t). Since k(t) is known, f1(k,γ,β) and f2(k,γ,β) are actually functions of time t, that is, f1(t) and f2(t). Since f1(t) and f2(t) are the derivatives of the quadruped robot's horizontal coordinate x(t) and vertical coordinate y(t) with time, the planned quadruped robot motion trajectory is:

[0116]

[0117] The expression of the quadruped robot state z(t) changing with time is:

[0118]

[0119] In order to make the quadruped robot move along this trajectory, its control input is:

[0120]

[0121] The expression of velocity v(t) changing with time can also be obtained as:

[0122]

[0123] Use binary search to find the minimum time T: first evaluate the state of the starting point and the target point, given an initialization time T0, if a0, a1, a2, b0, b1, b2 still have solutions under the constraints of the starting point and the target point, it means that the quadruped robot can reach the target point within T0. At this time, set the time to T0 / 2 and continue to solve a0, a1, a2, b0, b1, b2; if there is no solution, set the time to 3T0 / 4 and continue to solve. Repeat this cycle until the time reaches a certain accuracy or the number of calculations reaches a certain value. Finally, take the shortest time T for a0, a1, a2, b0, b1, b2 to generate the motion trajectory of the quadruped robot.

[0124] The present invention provides a kinematic model based on the movement characteristics of a quadruped robot, taking into account the lateral movement ability of the quadruped robot, and also taking into account the influence of the angle between the robot speed and the robot orientation on its movement rate; simplifying the movement task of the quadruped robot to point-to-point movement, and only needing to complete the state increment required by the quadruped robot within a specified time; using the kinematic model and movement task of the quadruped robot, automatically generating a passable trajectory, and at the same time finding the shortest time to complete the movement task. The present invention can realize the unstructured trajectory planning of the quadruped robot, and has the advantages of high efficiency and speed, etc., and solves the disadvantage that the current quadruped robot can only use inefficient and low-adaptability trajectory planning methods.

[0125] The above embodiments are used to illustrate the present invention rather than to limit the present invention. Any modification and change made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A trajectory planning system based on a quadruped robot kinematic model, characterized in that: include: Quadruped robot kinematic model, quadruped robot movement task, quadruped robot trajectory planner; Among them: The kinematic model of the quadruped robot incorporates the omnidirectional motion capability of the quadruped robot and its velocity constraints, and uses an equivalent velocity instead of the actual velocity; The moving task of the quadruped robot is to move from the current point to the target point. The state space is used to describe the movement of the quadruped robot, and the specific moving task is simplified to the change of the state quantity of the quadruped robot. The quadruped robot trajectory planner is used to find the minimum movement time and generate a corresponding movement trajectory so that the quadruped robot can complete the moving task in the shortest time; In the kinematic model of the quadruped robot: The constraint relationship between the quadruped robot speed v, the robot speed direction and the angle γ between the robot orientation: Where a represents the maximum linear velocity of the quadruped robot in the forward direction, and b represents the maximum linear velocity of the quadruped robot in the lateral direction; The linear velocity |v| of the quadruped robot satisfies: Where k represents the ratio of the robot speed to the maximum speed in the speed direction, which is also the equivalent speed of the quadruped robot in this direction; v max is the maximum speed of the quadruped robot; Create the state space of the quadruped robot: Among them, x and y are the horizontal and vertical coordinates of the quadruped robot. is the robot speed in the corresponding directions of x and y; β is the angle between the robot speed and the positive direction of the X axis of the world coordinate system; The kinematic model of the quadruped robot is as follows: Among them, z is the state variable of the quadruped robot; is the robot equivalent acceleration, is the angular velocity of the robot, is the angular velocity of γ; The moving task of the quadruped robot is that the quadruped robot changes from the current state z0 to the target state z0 after a state increment Δz changes within a limited time. f ;in: Among them, (x0, y0) is the coordinate of the current point, k0 is the current equivalent velocity, β0 is the angle between the current robot speed and the positive direction of the world coordinate system X axis, and γ0 is the angle between the current robot speed and the robot orientation; (x f ,y f ) is the coordinate of the target point, k f is the target equivalent rate, β f is the angle between the target robot speed and the positive direction of the X-axis of the world coordinate system, γ f is the angle between the target robot speed and the robot orientation; The quadruped robot trajectory planner uses binary search to find the minimum motion time T and generate the quadruped robot motion trajectory: Assume the motion time is T, let the equivalent rate k be a function of the time variable t; including: When there is sufficient time, the equivalent rate can reach a maximum value of 1: Where c1, c2, m1, m2, c1′, c2′, m1′, and m′2 are the parameters that determine the change of k, which are obtained from time T and the actual motion ability of the quadruped robot; t1 is the time for acceleration to k=1, and t2 is the time for deceleration from k=1 to k f time; When time is short: Where t1′ is the time to accelerate to the highest equivalent rate; set up and is a linear function of t, then γ and β are quadratic functions of time t, that is, Establish the constraints of the quadruped robot to solve a0, a1, a2, b0, b1, b2: Among them, Δx and Δy are the changes in the horizontal and vertical coordinates; Solve a0, a1, a2, b0, b1, b2, and then get γ(t) and β(t). The trajectory of the quadruped robot is planned as: Among them, f1(t) and f2(t) are functions of f1(k,γ,β) and f2(k,γ,β) with respect to time t, and are also the derivatives of the horizontal and vertical coordinates x(t) and y(t) of the quadruped robot with time; The expression of the quadruped robot state z(t) changing with time is: but: The expression of velocity v(t) changing with time is: Use binary search to find the minimum time T, including: (1) Set the initial movement time T = T0; (2) Determine whether a0, a1, a2, b0, b1, b2 still have a solution while satisfying all the constraints of the quadruped robot: (2.1) If there is a solution, set the new movement time to half of the current one; (2.2) If there is no solution, set the new movement time to 3 / 4 of the current one; (3) According to the new motion time in step (2), step (2) is executed again to solve a0, a1, a2, b0, b1, and b2; until the motion time reaches the expected value or the number of cycles reaches the set value; (4) Finally, the minimum motion time is obtained so that a0, a1, a2, b0, b1, and b2 have a solution; The constraints of the quadruped robot include: Starting point and target point constraints: z(0)=z0,z(T)=z f Acceleration constraints: in, is the equivalent acceleration The lower and upper limits of Angular acceleration constraint: in, is the angular acceleration of the robot in the direction of velocity The lower and upper limits of for The lower and upper limits of .

Images