A robot control method, device and robot

Abstract

The application provides a robot control method, device and robot. The robot control method provided by the application comprises the following steps: performing global path planning on a mobile chassis in a low-dimensional sampling space according to a starting pose of the mobile chassis at a starting point and a target pose of the mobile chassis at a target point, to obtain a collision-free path of the mobile chassis from the starting point to the target point; adding an initial pose of a mechanical arm to each path point on the collision-free path, to obtain a global full-body path of the robot; determining a target path segment to be re-planned; performing global path planning on the robot in a high-dimensional sampling space according to a starting pose of the robot at a starting point of the target path segment and an ending pose of the robot at an ending point of the target path segment, to obtain an optimal path of the robot between the starting point and the ending point; replacing the target path segment with the optimal path, to obtain an optimized global full-body collision-free path, and controlling the robot by using the global full-body collision-free path.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a robot control method, device and robot. Background Art

[0002] With the continuous expansion of intelligent robotics technology and applications, robot configurations are gradually shifting towards composite robots (comprising a mobile chassis and a robotic arm). Composite robots combine mobility and maneuverability, possessing a large and flexible workspace, bringing new possibilities to scenarios such as intelligent manufacturing and smart logistics. However, due to the introduction of redundant degrees of freedom, the planning and control of composite robots are becoming increasingly difficult. Developing safe and efficient motion planning methods for composite robots to fully utilize their performance has become a hot topic of research. Summary of the Invention

[0003] In view of this, the present application provides a robot control method, device and robot to quickly plan the path of the robot and achieve better coordination and optimization.

[0004] Specifically, this application is implemented through the following technical solutions:

[0005] A first aspect of the present application provides a robot control method, which is applied to a robot and includes:

[0006] performing global path planning for the mobile chassis in a low-dimensional sampling space based on a starting position of the mobile chassis at a starting point and a target position of a target point, thereby obtaining a collision-free path for the mobile chassis from the starting point to the target point; wherein the low-dimensional sampling space refers to a three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes a position of the mobile chassis in the three-dimensional space;

[0007] Adding the initial pose of the robot's manipulator to each path point on the collision-free path to obtain a global full-body path of the robot;

[0008] Determining a target path segment to be replanned in the global whole-body path; wherein the target path segment is a path segment that collides with a static obstacle in the environment;

[0009] For the target path segment, performing global path planning on the robot in a high-dimensional sampling space based on the robot's starting pose at the starting point of the target path segment and the robot's ending pose at the ending point of the target path segment, thereby obtaining an optimal path for the robot from the starting point to the ending point; wherein the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robotic arm;

[0010] The target path segment in the global whole-body path is replaced by the preferred path to obtain an optimized global whole-body collision-free path, and the robot is controlled by using the optimized global whole-body collision-free path.

[0011] A second aspect of the present application provides a robot control device, the device comprising an acquisition module, a determination module and a control module; wherein,

[0012] The acquisition module is configured to perform global path planning for the mobile chassis in a low-dimensional sampling space based on the starting posture of the mobile chassis at the starting point and the target posture of the target point, thereby obtaining a collision-free path for the mobile chassis from the starting point to the target point; the low-dimensional sampling space refers to a three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes the posture of the mobile chassis in the three-dimensional space;

[0013] The acquisition module is further configured to add the initial posture of the robot's manipulator to each path point on the collision-free path to obtain a global whole-body path of the robot;

[0014] The determination module is configured to determine a target path segment to be replanned in the global whole-body path; wherein the target path segment is a path segment that collides with a static obstacle in the environment;

[0015] The acquisition module is further configured to perform global path planning for the robot in a high-dimensional sampling space based on a starting position of the robot at a starting point of the target path segment and an ending position of the robot at an ending point of the target path segment, thereby obtaining an optimal path for the robot from the starting point to the ending point; wherein the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robotic arm;

[0016] The control module is used to replace the target path segment in the global whole-body path with the preferred path to obtain an optimized global whole-body collision-free path, and control the robot with the optimized global whole-body collision-free path.

[0017] The third aspect of the present application provides a robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of any one of the methods provided in the first aspect of the present application are implemented.

[0018] The fourth aspect of the present application provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any one of the methods provided in the first aspect of the present application.

[0019] The robot control method, device and robot provided in the present application, on the one hand, adopt a tightly coupled approach to perform path planning, which can improve the motion performance of the redundant system and give full play to the flexible advantages of the composite robot; on the other hand, only the mobile chassis is planned in sections where there is no collision (i.e., idle areas), and the whole body (the whole robot) is planned in sections where there is a collision (narrow areas), which can compress the sampling dimension and avoid the problem of long planning time and low efficiency due to excessive dimensions when the whole body is planned on all paths. It can effectively speed up the path planning process, shorten the path planning time, and improve the efficiency of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 This is a flowchart of Example 1 of the robot control method provided by this application;

[0021] Figure 2 Schematic diagram of the implementation principle of the robot control method provided in this application;

[0022] Figure 3 This is a flow chart of a second embodiment of a robot control method according to an exemplary embodiment of the present application;

[0023] Figure 4 This is a flowchart of Example 3 of the robot control method provided in this application;

[0024] Figure 5 This is a diagram showing the effect of adding a lateral disturbance term according to an exemplary embodiment of the present application;

[0025] Figure 6 A schematic diagram of the regional composition of a velocity damper according to an exemplary embodiment of the present application;

[0026] Figure 7 This is a structural diagram of Example 1 of the robot control device provided in this application. DETAILED DESCRIPTION

[0027] Exemplary embodiments are described in detail herein, with examples illustrated in the accompanying drawings. When the following description refers to the drawings, identical numerals in different drawings represent identical or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments are not intended to represent all embodiments consistent with this application.

[0028] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms "a," "the," and "the" used in this application are also intended to include the plural forms unless the context clearly indicates otherwise. It should also be understood that the term "and / or" as used herein refers to and includes any or all possible combinations of one or more of the associated listed items.

[0029] It should be understood that although the terms first, second, third, etc. may be used in this application to describe various information, such information should not be limited to these terms. These terms are only used to distinguish information of the same type from each other. For example, without departing from the scope of this application, first information may also be referred to as second information, and similarly, second information may also be referred to as first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0030] Specific embodiments are given below to introduce the technical solutions of the present application in detail.

[0031] Figure 1 This is a flowchart of Example 1 of the robot control method provided in this application. Figure 2 This is the implementation principle diagram of the robot control method provided in this application. Please also refer to Figure 1 and Figure 2 The robot control method provided in this embodiment is applied to a robot, and the method includes:

[0032] S101. Based on the starting posture of the mobile chassis of the robot at the starting point and the target posture of the target point, global path planning is performed on the mobile chassis in a low-dimensional sampling space to obtain a collision-free path of the mobile chassis from the starting point to the target point; the low-dimensional sampling space refers to the three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes the posture of the mobile chassis in the three-dimensional space.

[0033] It should be noted that the robot control method and device provided in this embodiment are applied to a robot, which is a composite robot including a mobile chassis and a robotic arm composed of multiple joints.

[0034] Specifically, the starting point and target point of the robot, as well as the starting position of the robot's mobile chassis at the starting point and the target position of the robot's mobile chassis at the target point are set according to actual needs and are not limited in this embodiment.

[0035] It should be noted that the pose of the mobile chassis refers to the pose of the mobile chassis in the three-dimensional space corresponding to the mobile chassis, which is used to describe the accurate position and orientation of the mobile chassis in space, which is usually composed of position and direction. The position is usually represented by x and y coordinates on the plane, and the direction is represented by an angle. That is, the pose of the mobile chassis includes its partial speed on the x-axis, partial speed on the y-axis, and rotation angle, which can be represented as (x, y, θ).

[0036] Further, the low-dimensional sampling space refers to the three-dimensional space in which the mobile chassis is located. In specific implementation, a path planning algorithm (such as A*, Dijkstra, RRT, etc.) can be used to search for an optimal path from the starting point to the target point in the low-dimensional sampling space, so as to find a path that can connect the starting point and the target point without violating the collision avoidance and other constraint conditions. The mobile chassis of the robot on the path will not collide with the static obstacle, and the robot can move from the starting point to the target point without collision.

[0037] It should be noted that the collision-free path of the mobile chassis from the starting point to the target point includes a plurality of path points, and each path point represents the pose of the mobile chassis. As known from the foregoing, each path point on the collision-free path can be represented by a 1*3-dimensional vector.

[0038] S102, add the initial pose of the robot's mechanical arm to each path point on the collision-free path to obtain the global full-body path of the robot.

[0039] Specifically, the initial pose of the mechanical arm is set according to actual needs, which is not limited in the embodiment. It should be noted that, as described above, the mechanical arm is composed of a plurality of joints, which is a multi-joint mechanical arm, and the initial pose of the mechanical arm includes the angles of the joints included in the mechanical arm.

[0040] In specific implementation, the angles of the joints of the mechanical arm can be set according to the number of degrees of freedom of the mechanical arm to obtain the initial pose of the mechanical arm. For example, in an embodiment, the mechanical arm includes 7 joints, and the initial pose is a 1*7-dimensional vector including the angles of the joints.

[0041] In specific implementation, the initial pose is added to each path point on the collision-free path to obtain the global full-body path of the robot. In other words, the global full-body path of the robot can be obtained according to the following formula:

[0042] D = conc(B, ArmPose)

[0043] Wherein, the D is the global full-body path of the robot; the B is the collision-free path; and the ArmPose is the initial pose of the mechanical arm.

[0044] It should be noted that, as described above, each path point on the global full-body path of the robot includes the pose of the mobile chassis and the pose of the robot arm. For example, in combination with the above example, it can be seen that for each path point on the global path, it is a 1*10-dimensional vector, the first 3 dimensions represent the pose of the mobile chassis, and the last 7 dimensions represent the pose of the robot arm.

[0045] S103, determining a target path segment to be re-planned in the global full-body path; wherein the target path segment is a path segment that collides with a static obstacle in the environment.

[0046] Specifically, the target path segment refers to a path segment that collides with a static obstacle in the environment, and there is a path point in the target path segment that collides with the static obstacle. In this step, the target path segment needs to be determined, and then the target path segment is replaced by a safe path segment (a path segment that does not collide with a static obstacle in the environment) in the subsequent step.

[0047] For example, in one possible implementation, collision detection can be used to determine which path segments intersect or collide with static obstacles in the environment to find the target path segment. For another example, in another possible implementation, simulation experiments can be used to determine which path segments intersect or collide with static obstacles in the environment to find the target path segment.

[0048] Optionally, Figure 3 A flowchart of a second embodiment of the robot control method according to an example embodiment of the present application is shown. Please refer to Figure 3 The method provided in this embodiment is based on the above-mentioned embodiments, and the determination of the target path segment to be re-planned in the global full-body path includes:

[0049] S301, determining the validity of each path point in the global full-body path by simulation experiment; wherein the validity of each path point in the global full-body path represents whether the path point collides with a static obstacle in the environment.

[0050] Specifically, the validity of each path point represents whether the path point collides with a static obstacle in the environment, and its value includes 0 or 1. A value of 0 represents that the path point does not collide with a static obstacle in the environment and is valid. A value of 1 represents that the path point collides with a static obstacle in the environment and is invalid.

[0051] In practice, the environment between the robot's starting point and the target point can be modeled based on the environmental information between the starting point and the target point (including the location, shape, and size of static obstacles), generating an environmental map. Furthermore, based on the environmental map, a collision detection algorithm is used to check the validity of each path point in the global path. If a path point collides with a static obstacle, the path point is marked as invalid.

[0052] S302. Determine a valid path segment based on the validity of each path point in the global whole-body path; wherein each path point included in the valid path segment is a valid path point.

[0053] Specifically, all path points in a valid path segment are valid path points, that is, path points with a value of 0.

[0054] In specific implementation, the first valid path point can be found from the global path, and the traversal can be continued until the first invalid path point is found. The previous valid path point of the invalid path point can be determined, and the path segment formed by these two valid path points can be determined as a valid path segment.

[0055] For example, referring to the example above, the first valid pathpoint found in the global path is pathpoint 1, which is then the starting pathpoint. Furthermore, the first invalid pathpoint is found to be pathpoint 4, and the previous valid pathpoint before pathpoint 4 is determined to be pathpoint 3, which is then the ending pathpoint. In this step, the path segment consisting of pathpoints 1 to 3 is determined to be a valid path segment, where pathpoints 1, 2, and 3 within the valid path segment are all valid pathpoints.

[0056] S303: Determine a path segment between the end path point of the preceding valid path segment and the start path point of the following valid path segment in two adjacent valid path segments as a target path segment.

[0057] Specifically, the end path point refers to the last path point in a valid path segment, and the start path point refers to the first path point in a valid path segment.

[0058] In a specific implementation, the path segment between the last path point of the preceding valid path segment and the first path point of the following valid path segment of two adjacent valid path segments is determined as the target path segment.

[0059] For example, in combination with the above example, if the two adjacent effective path segments are effective path segment 1 and effective path segment 2, wherein effective path segment 1 is from path point 1 to path point 3 and effective path segment 2 is from path point 6 to path point 9, in this step, the path segment between the end path point 3 of effective path segment 1 and the start path point 6 of effective path segment 2 is determined as the target path segment, i.e., the path segment between path point 3 and path point 6 is determined as the target path segment.

[0060] S104, for the target path segment, according to the start pose of the robot at the start point of the target path segment and the end pose of the robot at the end point of the target path segment, globally planning a path for the robot in a high-dimensional sampling space to obtain a preferred path of the robot from the start point to the end point; wherein the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robot arm.

[0061] Referring to the foregoing description, in combination with Figure 2 It can be understood that the poses of the start point and the end point both include the pose of the mobile chassis and the pose of the robot arm, which is a 1*10-dimensional vector.

[0062] Specifically, the high-dimensional sampling space refers to a multi-dimensional space with multiple dimensions, which includes a three-dimensional space where the mobile chassis is located and a multi-dimensional space where the robot arm is located. For example, in combination with the above example, the high-dimensional sampling space includes the sub-speed of the mobile chassis on the x-axis, the sub-speed on the y-axis, the turning angle, and the initial pose of the robot arm. The high-dimensional sampling space is a 10-dimensional space.

[0063] Further, the start point of the target path segment refers to the start path point, and the end point of the target path segment refers to the end path point. In specific implementation, according to the start pose of the robot at the start point and the end pose of the robot at the end point, a global path for the robot is planned in the high-dimensional sampling space based on a path search algorithm to obtain a preferred path of the robot from the start point to the end point.

[0064] For example, in combination with the above example, the target path segment is the path segment between path point 3 and path point 6, and in this step, according to the start pose of path point 3 and the end pose of path point 6, a global path for the robot is planned in the high-dimensional sampling space to obtain a preferred path 1 of the robot from path point 3 to path point 6.

[0065] S105, replacing the target path segment in the global whole-body path with the preferred path to obtain an optimized global whole-body collision-free path, and using the optimized global whole-body collision-free path to control the robot.

[0066] Specifically, the target path segment in the global path is replaced with the preferred path to obtain an optimized global full-body collision-free path. When the robot is controlled to move along the optimized global full-body collision-free path, it will not collide with static obstacles in the environment.

[0067] For example, based on the above example, the target path segment is the path segment between path points 3 and 6, and the determined preferred path is preferred path 1. In this step, the path segment between path points 3 and 6 in the global path is replaced with preferred path 1 to obtain the optimized global full-body collision-free path, and this optimized global full-body collision-free path is used to control the robot.

[0068] It is understandable that when planning a path for a robot with both a mobile chassis and a robotic arm, if the mobile chassis and the robotic arm are separated into independent components and the path planning is performed according to the idea that the mobile chassis moves into position and the robotic arm moves subsequently, then the divergence in the sampling spaces implemented by the mobile chassis and the robotic arm will inevitably become one of the obstacles to the planning solution. The method provided in this application adopts a tight coupling approach to path planning, which can improve the motion performance of the redundant system and give full play to the flexibility of the composite robot. In addition, only the mobile chassis is planned in sections without collisions (i.e., idle areas), and the whole body (the entire robot) is planned in sections with collisions (narrow areas). This also avoids the problem of long planning time and low efficiency when planning the whole body on all paths due to excessive dimensions.

[0069] The robot control method provided in this embodiment, on the one hand, adopts a tightly coupled approach to perform path planning, which can improve the motion performance of the redundant system and give full play to the flexibility of the composite robot. On the other hand, only the mobile chassis is planned on sections where there is no collision (i.e., idle areas), and the whole body (the entire robot) is planned on sections where there is a collision (narrow areas). This can compress the sampling dimension and avoid the problem of long planning time and low efficiency due to excessive dimensions when the whole body is planned on all paths. This can effectively speed up the path planning process, shorten the path planning time, and improve the efficiency of path planning.

[0070] Figure 4 This is a flow chart of the third embodiment of the robot control method provided by this application. Figure 4 The method provided in this embodiment, based on the above embodiment, further comprises:

[0071] S401. In the process of controlling the robot to move from the first path point to the second path point in the optimized global full-body collision-free path, the objective function is optimized secondary with the self-constraints of each joint of the robot, the preset differential motion equation constraints, and the preset velocity damper inequality constraints as constraints to find the optimal solution of the objective function while satisfying the constraints.

[0072] Specifically, the first path point and the second path point are two adjacent path points in the optimized global full-body collision-free path.

[0073] It should be noted that, as previously described, by determining an optimized global, full-body collision-free path, the robot can avoid static obstacles and satisfy the static obstacle avoidance constraint. However, when controlling the robot to move from a first pathpoint to a second pathpoint, obstacles or other unforeseen circumstances may arise between the pathpoints, making it impossible to guarantee that the dynamic obstacle avoidance constraint will be satisfied. Therefore, optimization is required to achieve dynamic obstacle avoidance when controlling the robot from the first pathpoint to the second pathpoint.

[0074] In this application, the motion control problem is expressed as a quadratic programming problem under motion constraints, which ensures the flexible whole-body movement of the composite robot while having the ability to actively avoid dynamic obstacles in the environment.

[0075] Specifically, the objective function is the function to be minimized or maximized in the optimization problem, and is set according to actual needs. In this embodiment, the objective function is a function for solving the speed of each joint of the robot, and the optimal solution of the objective function is the speed of each joint point of the robot. For example, in one possible implementation, the objective function is:

[0076]

[0077] Wherein, x is the control variable, i.e., the velocity of each joint point; Q is the weight matrix; and C is the Jacobian matrix.

[0078] Furthermore, constraints can be set, and by performing a secondary optimization on the objective function, the optimal solution of the objective function can be found while satisfying all the constraints. It should be noted that constraints are used to limit the way the robot moves, ensuring that the robot does not violate its own constraints during movement.

[0079] In the present application, the self-constraints of each joint of the robot, the preset differential motion equation constraints, and the preset velocity damper inequality constraints are used as constraint conditions.

[0080] Among them, the self-constraints of each joint of the robot usually include self-constraints on the physical structure, such as the rotation range, maximum speed, maximum torque, etc. of the joints. These constraints limit the motion range and speed of each joint of the robot.

[0081] Furthermore, the preset differential motion equation constraints describe the motion relationship between the robot's joints, which are usually used to ensure that the robot can remain stable when performing tasks. These constraints can be preset motion relationships based on task requirements, control strategies, or dynamic equations.

[0082] The preset velocity damper inequality constraints are used to limit the movement speed of the robot joints, playing a role in smoothing and stabilizing the motion control. These constraints are usually used to control the acceleration and deceleration of the robot to prevent instability caused by sudden changes in joint velocity.

[0083] Optionally, in a possible implementation, the differential motion equation constraint is represented by the following formula:

[0084]

[0085] Among them, the is the velocity of the robot's joints; v1(t) is the update velocity of the robot's end effector; J(q) is the Jacobian matrix; the update velocity is the velocity obtained by adding the lateral disturbance term to the initial velocity;

[0086] The initial velocity is calculated according to the following formula:

[0087]

[0088] The v1(t) is calculated according to the following formula:

[0089] v1(t)=(x1(t), y1(t), θ1(t))

[0090] x1(t)=(sin(λ))·arg max(x0(t), y0(t)), y1(t)=y0(t), θ1(t)=θ0(t)

[0091] Wherein, v0(t) is the initial velocity; β is the weight matrix; Ψ is the function that converts the homogeneous error into the six-dimensional space error; is the posture error from the current posture to the target posture; x0(t) is the lateral component velocity in the initial velocity; y0(t) is the longitudinal component velocity in the initial velocity; θ0(t) is the rotation angle in the initial velocity; x1(t) is the lateral component velocity in the updated velocity; y1(t) is the longitudinal component velocity in the updated velocity; θ1(t) is the rotation angle in the updated velocity; sin(λ) is the lateral disturbance factor.

[0092] It should be noted that the longitudinal component velocity refers to the component velocity along the forward direction; the lateral component velocity refers to the component velocity along the lateral direction, and the lateral direction is a direction perpendicular to the forward direction.

[0093] Specifically, Figure 5 This is an effect diagram of adding a lateral disturbance term as shown in an exemplary embodiment of this application. Figure 5 , Figure 5 Figure (a) shows the effect of controlling the robot based on the initial velocity. Figure 5 Figure (b) shows the effect of controlling the robot based on the updated speed after adding the lateral disturbance term. Figure 5 If the robot, dynamic obstacle, and target point are aligned on the same straight line, the spatial error is only in the relative direction. The desired velocity output will "pull the robot straight," which can easily lead to dynamic obstacle avoidance constraint conflicts, unsolvable solvers, and motion deadlock. By adding a lateral perturbation term to the robot and setting the lateral perturbation factor, the range of options for the robot's terminal velocity is increased, enhancing flexibility. The max function is equivalent to the lateral velocity being primarily affected by the longitudinal velocity when the longitudinal error is large. When the lateral error is large, it reverts to being primarily affected by the lateral error, ensuring that the robot can accurately reach the target position.

[0094] Furthermore, in a possible implementation of the present application, the preset velocity damper inequality constraint may include velocity damper inequality constraints corresponding to all joints of the robot or velocity damper inequality constraints corresponding to some joints of the robot; the velocity damper inequality constraints corresponding to the joints of the robot are represented by the following formula:

[0095]

[0096] Among them, the is the speed of the robot's joints; is the Jacobian matrix; is the velocity of the robot's joint points on the line between the robot and the dynamic obstacle; is the unit vector of the dynamic obstacle pointing to the robot; is the speed of the dynamic obstacle; is the velocity of the dynamic obstacle on the line between the robot and the dynamic obstacle; i is the distance of the preset speed damper; ξ is the state adjustment factor; d is the distance between the robot and the dynamic obstacle; d s is the preset stopping distance.

[0097] Figure 6 This is a schematic diagram of the regional composition of a velocity damper according to an exemplary embodiment of the present application. Figure 6 It can be understood that when the robot gradually approaches the obstacle, it can be roughly divided into three stages: the first stage is when d i When d≤d, the range of the relative speed v between the robot and the obstacle is large and shrinks as the distance d decreases, and the robot movement is not greatly affected; in the second stage, when d s ≤d≤d i When d≤d s When , the relative speed v between the robot and the obstacle can be selected to be a negative value, and the robot may be stationary or retreat to avoid it.

[0098] In the first stage, ensure that the robot's movement is not affected as much as possible; in the second stage, ensure that the robot's movement is adjusted as quickly as possible as d continues to decrease; in the third stage, when d approaches or exceeds d s When the robot is in an awkward position, ensure that it can make obvious avoidance or retreat movements.

[0099] Based on the above considerations, the velocity damper inequality constraint is:

[0100]

[0101] Wherein, v is the relative speed between the robot and the obstacle; d i is the distance of the preset speed damper; ξ is the state adjustment factor; d is the distance between the robot and the obstacle; d s is the preset stopping distance.

[0102] It can be understood that the relative speed between the robot and the dynamic obstacle can be expressed as:

[0103]

[0104] Among them, the is the speed of the robot's joints; is the Jacobian matrix; is the velocity of the robot's joint points on the line between the robot and the dynamic obstacle; is the unit vector of the dynamic obstacle pointing to the robot; is the speed of the dynamic obstacle; is the component of the velocity of the dynamic obstacle on the line between the robot and the dynamic obstacle; and v is the relative velocity between the robot and the obstacle.

[0105] Integrating the formula we get:

[0106]

[0107] Further sorting out:

[0108]

[0109] Specifically, the existence of the state adjustment factor can adjust the state of the speed damper. A larger value corresponds to a strict speed limit in the second and third stages, and vice versa.

[0110] It should be noted that the above equation represents the correspondence between a pair of joint points of the robot. It is understandable that the movement of the robot joints affects the speed of each joint point of the robot. In practice, it is necessary for each joint point of the robot to satisfy the velocity damper inequality constraint, that is, the velocity damper inequality constraint corresponding to all joint points of the robot or the velocity damper inequality constraint corresponding to some joint points of the robot. That is, for multiple joint points, it should satisfy: A2x≤B2

[0111] in:

[0112]

[0113] Furthermore, in a possible implementation, the objective function is subjected to secondary optimization using the self-constraints of each joint of the robot, the preset differential motion equation constraints, and the preset velocity damper inequality constraints as constraints to find the optimal solution of the objective function while satisfying the constraints, including:

[0114] The objective function is optimized secondaryally with any of the following constraints as constraints; any of the constraints include: a first constraint consisting of the self-constraints of each joint of the robot, velocity boundary constraints, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to all joints of the robot; a second constraint consisting of the self-constraints of each joint of the robot, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to all joints of the robot; and a third constraint consisting of the self-constraints of each joint of the robot, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to some joints of the robot.

[0115] Specifically, a velocity bound constraint is a constraint used to limit the range of a robot's motion speed. Velocity bounds are often used to ensure that a robot adheres to system design or safety specifications while performing a task, while also avoiding exceeding system capabilities or causing unstable motion.

[0116] It should be noted that the preset velocity damper inequality constraint is for a specific joint of the robot. When the constraint is applied to some joints of the robot, the velocity damper inequality constraint corresponding to those joints needs to be used for the constraint. Similarly, when the constraint is applied to all joints of the robot, the velocity damper inequality constraint corresponding to all joints needs to be used for the constraint.

[0117] In a specific implementation, any one of the first constraint, the second constraint, and the third constraint can be selected as a constraint to perform secondary optimization on the objective function. For example, in one possible implementation, the first constraint is selected as the constraint. In another possible implementation, the second constraint is selected as the constraint. In yet another possible implementation, the third constraint is selected as the constraint.

[0118] Furthermore, optionally, in one possible implementation, the levels of the first, second, and third constraints are sequentially lower, i.e., the first constraint has the highest level and the third constraint has the lowest level. If the optimal solution to the objective function cannot be found using the current constraints, the optimal solution to the objective function is sought using constraints at a level lower than the current constraints. If the optimal solution to the objective function still cannot be found using the constraints at the last level, then the objective function has no optimal solution, and an error is reported.

[0119] For example, in combination with the above example, in a possible implementation, when the optimal solution of the target function cannot be found by using the first constraint condition, the optimal solution of the target function is found by using the second constraint condition. If the optimal solution of the target function cannot be found by using the second constraint condition, the optimal solution of the target function is found by using the third constraint condition. If the optimal solution of the target function cannot be found by using the third constraint condition, the target function has no optimal solution, and an error is reported. For another example, in another possible implementation, when the optimal solution of the target function cannot be found by directly using the third constraint condition, the target function has no optimal solution, and an error is reported.

[0120] The method provided in the application guarantees the maximum constraint on the target function, improves the generalization, and makes the calculated optimal solution more accurate, by designing the first constraint condition, the second constraint condition, and the third constraint condition in a descending order of levels and performing secondary optimization on the target function in the order of levels. The secondary optimization on the target function is performed by using the constraint condition composed of multiple constraints, the robot is controlled based on the obtained optimal solution of the target function, the problems of opposite motion deadlock and over-constraint are solved, the implementation of the dynamic obstacle avoidance function of the robot is ensured, and an efficient and easy-to-use path planning and dynamic obstacle avoidance scheme is provided.

[0121] S402, controlling the robot by using the optimal solution, so that the robot moves from the first path point to the second path point.

[0122] Specifically, in combination with the above example, the optimal solution refers to the speed of each joint of the robot. In this step, the robot is controlled by using the solved speed of each joint of the robot, so that the robot moves from the first path point to the second path point.

[0123] The robot control method provided in the embodiment solves the problems of opposite motion deadlock and over-constraint, ensures the implementation of the dynamic obstacle avoidance function of the robot, and provides an efficient and easy-to-use path planning and dynamic obstacle avoidance scheme, by performing secondary optimization on the target function by using the constraint condition composed of multiple constraints and controlling the robot based on the obtained optimal solution of the target function. In addition, in the first aspect, the optional range of the end speed of the robot is increased and the flexibility is enhanced by adding a lateral disturbance term to the robot and setting a lateral disturbance factor. The max function is equivalent to the fact that when the longitudinal error is large, the lateral speed is mainly affected by the longitudinal speed, and when the lateral error is large, the lateral speed is mainly affected by the lateral error, so that the robot can accurately reach the target pose. In the second aspect, the speed damper inequality constraint can ensure that the robot adjusts its motion according to the relative speed between the robot and the obstacle, so as to achieve the effect of dynamic obstacle avoidance.

[0124] Corresponding to the foregoing embodiment of the robot control method, the application further provides an embodiment of a robot control device.

[0125] Figure 7 The structural schematic diagram of the embodiment one of the robot control device provided by the application is shown in the following figure. Figure 7 The device provided by the embodiment includes an acquisition module 1010, a determination module 1020 and a control module 1030; wherein,

[0126] The acquisition module 1010 is configured to perform global path planning on the mobile chassis in a low-dimensional sampling space according to a starting pose of the mobile chassis at a starting point and a target pose of the mobile chassis at a target point, to obtain a collision-free path of the mobile chassis from the starting point to the target point; the low-dimensional sampling space refers to a three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes a pose of the mobile chassis in the three-dimensional space;

[0127] The acquisition module 1010 is further configured to add an initial pose of the robot arm of the robot to each path point on the collision-free path, to obtain a global whole-body path of the robot;

[0128] The determination module 1020 is configured to determine a target path segment to be re-planned in the global whole-body path; wherein, the target path segment is a path segment that collides with a static obstacle in the environment;

[0129] The acquisition module 1010 is further configured to, for the target path segment, perform global path planning on the robot in a high-dimensional sampling space according to a starting pose of the robot at a starting point of the target path segment and an ending pose of the robot at an ending point of the target path segment, to obtain a preferred path of the robot from the starting point to the ending point; wherein, the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robot arm;

[0130] The control module 1030 is configured to replace the target path segment in the global whole-body path with the preferred path, to obtain an optimized global whole-body collision-free path, and to control the robot by using the optimized global whole-body collision-free path.

[0131] The robot control device provided in this embodiment, when determining that there is a target path segment to be re-planned in the global path, performs global path planning on the robot in a high-dimensional sampling space, and replaces the target path segment in the global path with the obtained preferred path, thereby obtaining an optimized global full-body collision-free path. When the optimized global full-body collision-free path is used to control the movement of the robot, it will not collide with static obstacles in the environment. The method provided in this embodiment provides a robot-adaptive global path planning method, and only requires global path planning for the target path segment, which reduces the sampling dimension of the robot when performing path planning, effectively speeds up the path planning process, and improves the efficiency, ease of use and obstacle avoidance effect of the robot.

[0132] The device of this embodiment can be used to perform Figure 1 The steps, specific implementation principles and implementation processes of the method embodiment shown are similar and will not be repeated here.

[0133] The present application also provides a robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of any one of the methods provided in the first aspect of the present application are implemented.

[0134] The present application also provides a computer-readable storage medium having a computer program stored thereon, which implements the steps of any one of the methods provided in the present application when the program is executed by a processor.

[0135] The implementation process of the functions and effects of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and will not be repeated here.

[0136] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0137] The above description is only a preferred embodiment of the present application and is not intended to limit the present application. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present application shall be included in the scope of protection of the present application.

Claims

1. A robot control method, characterized in that: The robot control method is applied to a robot, and the method comprises: performing global path planning for the mobile chassis in a low-dimensional sampling space based on a starting position of the mobile chassis at a starting point and a target position of a target point, thereby obtaining a collision-free path for the mobile chassis from the starting point to the target point; wherein the low-dimensional sampling space refers to a three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes a position of the mobile chassis in the three-dimensional space; Adding the initial pose of the robot's manipulator to each path point on the collision-free path to obtain a global full-body path of the robot; Determining a target path segment to be replanned in the global whole-body path; wherein the target path segment is a path segment that collides with a static obstacle in the environment; For the target path segment, performing global path planning on the robot in a high-dimensional sampling space based on the robot's starting pose at the starting point of the target path segment and the robot's ending pose at the ending point of the target path segment, thereby obtaining an optimal path for the robot from the starting point to the ending point; wherein the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robotic arm; The target path segment in the global whole-body path is replaced by the preferred path to obtain an optimized global whole-body collision-free path, and the robot is controlled by using the optimized global whole-body collision-free path.

2. The method according to claim 1, characterized in that In the process of controlling the robot using the optimized global full-body collision-free path, the method further includes: In the process of controlling the robot to move from a first path point to a second path point in the optimized global full-body collision-free path, a secondary optimization is performed on an objective function using the inherent constraints of each joint of the robot, a preset differential motion equation constraint, and a preset velocity damper inequality constraint as constraint conditions to find an optimal solution to the objective function while satisfying the constraint conditions; The robot is controlled using the optimal solution to move from the first path point to the second path point.

3. The method according to claim 1, characterized in that The determining of a target path segment to be replanned in the global whole-body path includes: Determining the validity of each path point in the global full-body path using simulation experiments; wherein the validity of each path point in the global full-body path indicates whether the path point collides with a static obstacle in the environment; Determining a valid path segment based on the validity of each path point in the global whole-body path; wherein each path point included in the valid path segment is a valid path point; A path segment between an end path point of a preceding valid path segment and a start path point of a following valid path segment of two adjacent valid path segments is determined as the target path segment.

4. The method according to claim 2, characterized in that The differential motion equality constraint is represented by the following formula: ； Among them, the is the speed of the robot's joints; is the update speed of the end effector of the robot; is the Jacobian matrix; the update speed is the speed obtained by adding the lateral disturbance term to the initial speed; The initial velocity is calculated according to the following formula: ； described Calculated according to the following formula: ； ； Among them, the is the initial velocity; is the weight matrix; is a function that converts homogeneous error into six-dimensional space error; is the pose error from the current pose to the target pose; is the lateral component velocity in the initial velocity; is the longitudinal component velocity in the initial velocity; is the angle of rotation in the initial velocity; is the lateral component speed in the update speed; is the longitudinal component of the update speed; is the rotation angle in the update speed; is the lateral disturbance factor.

5. The method according to claim 2 or 4, characterized in that The preset velocity damper inequality constraint includes the velocity damper inequality constraints corresponding to all joints of the robot or the velocity damper inequality constraints corresponding to some joints of the robot; wherein the velocity damper inequality constraints corresponding to the joints of the robot are represented by the following formula: ； Among them, the is the speed of the robot's joints; is the Jacobian matrix; is the velocity of the robot's joint points on the line between the robot and the dynamic obstacle; is the unit vector of the dynamic obstacle pointing to the robot; is the speed of the dynamic obstacle; is the velocity of the dynamic obstacle on the line between the robot and the dynamic obstacle; is the working distance of the preset speed damper; is a state adjustment factor; is the distance between the robot and the dynamic obstacle; is the preset stopping distance.

6. The method according to claim 2, characterized in that The objective function is subjected to secondary optimization using the self-constraints of each joint of the robot, the preset differential motion equation constraints, and the preset velocity damper inequality constraints as constraint conditions to find the optimal solution of the objective function while satisfying the constraint conditions, including: The objective function is optimized secondaryally with any of the following constraints as constraints; any of the constraints include: a first constraint consisting of the self-constraints of each joint of the robot, velocity boundary constraints, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to all joints of the robot; a second constraint consisting of the self-constraints of each joint of the robot, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to all joints of the robot; and a third constraint consisting of the self-constraints of each joint of the robot, preset differential motion equation constraints, and velocity damper inequality constraints corresponding to some joints of the robot.

7. The method according to claim 6, characterized in that If the optimal solution of the objective function cannot be found using the current constraints, the method further includes: An optimal solution to the objective function is found using a constraint condition that is one level lower than the current constraint condition; wherein the levels of the first constraint condition, the second constraint condition, and the third constraint condition are sequentially reduced.

8. A robot control device, characterized in that: The device includes an acquisition module, a determination module and a control module; wherein, The acquisition module is configured to perform global path planning for the mobile chassis in a low-dimensional sampling space based on the starting posture of the mobile chassis at the starting point and the target posture of the target point, thereby obtaining a collision-free path for the mobile chassis from the starting point to the target point; the low-dimensional sampling space refers to a three-dimensional space corresponding to the mobile chassis, and each path point in the collision-free path includes the posture of the mobile chassis in the three-dimensional space; The acquisition module is further configured to add the initial posture of the robot's manipulator to each path point on the collision-free path to obtain a global whole-body path of the robot; The determination module is configured to determine a target path segment to be replanned in the global whole-body path; wherein the target path segment is a path segment that collides with a static obstacle in the environment; The acquisition module is further configured to perform global path planning for the robot in a high-dimensional sampling space based on a starting position of the robot at a starting point of the target path segment and an ending position of the robot at an ending point of the target path segment, thereby obtaining an optimal path for the robot from the starting point to the ending point; wherein the high-dimensional sampling space includes a three-dimensional space corresponding to the mobile chassis and a multi-dimensional space corresponding to the robotic arm; The control module is used to replace the target path segment in the global whole-body path with the preferred path to obtain an optimized global whole-body collision-free path, and control the robot using the optimized global whole-body collision-free path.

9. A robot, characterized in that: The method comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method according to any one of claims 1 to 7 when executing the program.

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

Images