Robot, motion control method and device thereof and robot cluster motion system

Abstract

The invention relates to the field of robot cluster control, and discloses a robot, a motion control method and device of the robot and a robot cluster motion system.The control method comprises the steps that a task instruction is responded to obtain the current state of the robot and external obstacle information; based on the obstacle information, planning a safe path of the robot from the current state to the target state indicated in the task instruction; vectorizing the obstacle information to construct a static safety space along the safety path; optimizing based on a target function to obtain a motion track of the robot, wherein the target function comprises constraint conditions constructed based on a safe path, a static safe space and received track information of other robots in the cluster network; and controlling the robot to execute target motion according to the motion trail, and broadcasting the motion trail to a cluster network. The method can effectively solve the congestion and collision problems of multi-robot movement.

Description

Technical Field

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

[0002] At present, wheeled robots are increasingly used in restaurants, shopping malls, and office buildings. These robots cover many service functions such as cleaning, delivery, and welcoming guests. For example, an office building usually deploys multiple robots to perform multiple different types of tasks at the same time. As the number of robots deployed in the same application scenario increases, congestion often occurs between robots. This is because the goal of each robot is to complete its own task first. When multiple robots meet, they will regard other robots as obstacles and look for space to bypass other robots. At the same time, other robots are also doing the same action, and congestion will occur. As the congestion lasts longer, more and more robots will be congested here. Summary of the invention

[0003] In view of this, the present application provides a robot and a motion control method, device and robot cluster motion system thereof, which can solve the congestion problem of multi-robot motion.

[0004] In a first aspect, an embodiment of the present application provides a robot motion control method, comprising:

[0005] Respond to task instructions to obtain the robot's current state and external obstacle information;

[0006] Based on the obstacle information, planning a safe path for the robot to reach the target state indicated in the task instruction from the current state;

[0007] vectorizing the obstacle information to construct a static safety space along the safety path;

[0008] Obtaining a motion trajectory of the robot based on an objective function optimization, wherein the objective function includes constraint conditions constructed based on the safety path, the static safety space, and trajectory information of other robots in the received cluster network;

[0009] The robot is controlled to execute target motion according to the motion trajectory, and the motion trajectory is broadcast to the cluster network.

[0010] In a second aspect, an embodiment of the present application further provides a robot motion control device, comprising:

[0011] The data acquisition module is used to respond to task instructions to obtain the current state of the robot itself and external obstacle information;

[0012] A path planning module, used to plan a safe path for the robot to reach the target state indicated in the task instruction from the current state based on the obstacle information;

[0013] A vector processing module, used for vectorizing the obstacle information to construct a static safety space along the safety path;

[0014] A path optimization module, used for optimizing the motion trajectory of the robot based on an objective function, wherein the objective function includes constraints constructed based on the safe path, the static safety space, and trajectory information of other robots in the received cluster network;

[0015] The motion control module is used to control the robot to perform target motion according to the motion trajectory and broadcast the motion trajectory to the cluster network.

[0016] In a third aspect, an embodiment of the present application further provides a robot cluster motion system, comprising: a plurality of robots, wherein each of the robots forms a cluster network through a broadcast protocol;

[0017] Each of the robots is used to receive real-time trajectory information of other robots in the cluster network, and execute the robot motion control method to generate its own motion trajectory to control the robot to execute its own target motion, and broadcast the own motion trajectory to the cluster network.

[0018] In a fourth aspect, an embodiment of the present application further provides a robot, comprising a processor and a memory, wherein the memory stores a computer program, and the processor is used to execute the computer program to implement the robot motion control method.

[0019] In a fifth aspect, an embodiment of the present application further provides a computer-readable storage medium storing a computer program, which, when executed, implements the robot motion control method.

[0020] The embodiments of the present application have the following advantages:

[0021] The robot motion control method proposed in the present application adopts a decentralized communication framework, by allowing each robot to respond to task instructions to obtain the robot's own current state and external obstacle information; based on the obstacle information, a safe path is planned from the current state to the target state; the obstacle information is vectorized to construct a static safety space along the safe path; the motion trajectory of the robot is then optimized based on the objective function, and the objective function includes constraints constructed based on the safe path, the static safety space and the trajectory information of other robots in the received cluster network; the robot is controlled to perform the target motion according to the motion trajectory, and the motion trajectory is broadcast to the cluster network for other robots to use in the generation of motion trajectories. The method of the present application can realize the efficient movement of multiple robots in an area without collision or blockage with each other. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0023] Figure 1 An application schematic diagram of the robot cluster motion system according to an embodiment of the present application is shown;

[0024] Figure 2 (a) and (b) show schematic diagrams of using a central node and a central node exception, respectively;

[0025] Figure 3 A schematic diagram showing a robot in an embodiment of the present application sharing trajectory information through broadcasting;

[0026] Figure 4 A first flow chart of the robot motion control method according to an embodiment of the present application is shown;

[0027] Figure 5 A flow chart of planning a safe path according to an embodiment of the present application is shown;

[0028] Figure 6 A schematic diagram showing the robot searching for all possible states for the next step from the current state;

[0029] Figure 7 A schematic diagram showing a planned safe path from the current state to the target state;

[0030] Figure 8 A flow chart showing the construction of a static security space in an embodiment of the present application is shown;

[0031] Fig. 9 A schematic diagram showing that the convex polygon constituting the static safety space is defined as a rectangle;

[0032] Fig.10 A schematic diagram of determining and expanding a convex polygon from the current state of the robot is shown;

[0033] Fig.11 A schematic diagram showing the search for all maximum convex polygons along a safe path;

[0034] Fig.12 A schematic diagram showing a robot defined as a rectangular model is shown;

[0035] Fig.13 A schematic diagram showing a robot not colliding with other robots at different times;

[0036] Fig.14 (a) and (b) are schematic diagrams showing the robot and other robots without collision and collision respectively;

[0037] Fig.15 (a) and (b) are schematic diagrams showing the distance from a point to a line of a first polygon A and a second polygon B, respectively;

[0038] Fig.16 A structural schematic diagram of a robot motion control device according to an embodiment of the present application is shown. DETAILED DESCRIPTION

[0039] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0040] The components of the embodiments of the present application generally described and shown in the drawings herein may be arranged and designed in various configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0041] Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meanings as those generally understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meanings as the contextual meanings in the relevant technical field and will not be interpreted as having idealized meanings or overly formal meanings unless clearly defined in the various embodiments of the present application.

[0042] In scenarios where multiple robots are moving, congestion is likely to occur between the robots. However, the common cluster control algorithms are currently mainly used in drone scenarios. This type of algorithm mainly dispatches each drone by solving the motion trajectory of each drone in three-dimensional space to avoid collisions between drones. However, for wheeled robots running on the ground, they essentially run in two-dimensional space, and two-dimensional space does not have enough space like three-dimensional space for all robots to make avoidance like drones. Therefore, the existing drone cluster control algorithm cannot be transplanted to multi-robot application scenarios. To this end, the present application discloses a robot motion control method, which realizes the control of multiple robot cluster motions by proposing a robot trajectory generation algorithm based on two-dimensional space to solve the problem of insufficient avoidance space.

[0043] The technical solution of the present application is described below in conjunction with some specific embodiments.

[0044] An embodiment of the present application proposes a robot. Demonstratively, the robot includes a processor and a memory, wherein the memory stores a computer program. The processor runs the computer program to enable the robot to execute the robot motion control method of the present application. Not only for the control of a single robot, the operating efficiency of the robot's own motion trajectory can be improved, but also for the entire cluster system, on the one hand, mutual avoidance between multiple robots can be effectively achieved. On the other hand, since each robot deploys the same control strategy, that is, a decentralized distributed framework design is adopted, task allocation and resource optimization can be achieved, and the adaptability and scalability of the entire cluster system can be improved.

[0045] Among them, the processor can be an integrated circuit chip with signal processing capabilities. The processor can be a general-purpose processor, including a central processing unit (CPU), a graphics processing unit (GPU) and a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA) or at least one of other programmable logic devices, discrete gates or transistor logic devices, and discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc., which can implement or execute the disclosed methods, steps and logic block diagrams in the embodiments of the present application.

[0046] The memory may be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), etc. The memory is used to store a computer program, and the processor may execute the computer program accordingly after receiving an execution instruction.

[0047] It is understood that the above-mentioned robot can be a wheeled robot that moves on the ground through wheels, for example, it can be a delivery robot (including robots that deliver meals and letters), a cleaning robot, a guide robot, etc.; or it can also be a robot that moves through structures such as tracks, etc. Further, taking a wheeled robot as an example, it can be a single-wheeled robot, or a humanoid robot in which the robot's two feet and rolling wheels are spliced ​​into one body, or it can also be a two-wheeled, three-wheeled or four-wheeled robot, and its existence form is not specifically limited.

[0048] Based on the robot with the above structure, the present application proposes a robot cluster motion system. Exemplarily, the cluster motion system includes: several robots as mentioned above, each robot forms a cluster network through a broadcast protocol, wherein each robot can send information (including but not limited to its own trajectory information, etc.) to the network through broadcasting, and can also receive all information in the network (including but not limited to the trajectory information of other robots, etc.).

[0049] For example, Figure 1 As shown in FIG. 1 , it is assumed that there are three robots (respectively denoted as robots A, B and C) forming a cluster network. These three robots can send information to any other robots, that is, broadcast to the network. It can be seen that the robot cluster motion system of the present application adopts a decentralized framework, that is, there is no central control system such as Figure 2 The central node shown in (a) is used to calculate and forward information uniformly for each robot. It can be understood that if a central node fails, such as network communication abnormality, Figure 2As shown in (b), all robots will not be able to receive the trajectory information of the central node, and then all robots will become abnormal, which may cause collisions between robots. Therefore, this application adopts the design of a decentralized framework, that is, each robot can communicate through broadcast messages. Even if a single robot has an abnormality, it will not affect the normal operation of other robots. Compared with the existing solutions based on the centralized framework, it has a stronger fault tolerance and higher risk resistance. Not only that, the decentralized system can flexibly adapt to changes in the scale of tasks. For example, new robots only need to deploy the same control strategy to seamlessly join or exit the task without redesigning the control strategy of the overall system.

[0050] Based on the decentralized framework, in order to solve the problems of multiple robots blocking or colliding, in the robot cluster motion system, each robot is used to receive the real-time trajectory information of other robots in the cluster network, such as Figure 3 As shown, the robot motion control method of the present application is executed to generate its own motion trajectory, so as to control the robot itself to perform the target motion. At the same time, each robot also broadcasts its own motion trajectory to the cluster network for other robots to receive and use, such as as an avoidance condition to generate their own motion trajectory, etc. It can be understood that each robot makes the optimal motion decision based on local information, and then cooperates with each other through local optimization to achieve the effect of global approximate optimization, so as to cooperate to form efficient actions of the entire system.

[0051] For example, in some embodiments, the broadcasted motion trajectory information includes the unique number of each robot (such as the ID number, etc.) and the trajectory point (such as the two-dimensional coordinate position, etc.) and posture (such as the heading angle, etc.) of the robot at the corresponding time. Since the number of each robot is unique, each robot removes the information with the same number as its own from all received trajectory information to obtain the trajectory information of all other robots. It can be understood that the message communication between robots mainly shares the necessary motion trajectory information, rather than global data, which can greatly reduce the network burden.

[0052] Optionally, when multiple robots are initially forming a cluster network, there is no information in the network. At this time, each robot does not need to consider the status of other robots. It only needs to use a trajectory generation method based on its own current status to generate an initial motion trajectory, and then broadcast it to the network.

[0053] For each robot in the above robot cluster motion system, by respectively adopting the robot motion control method of the embodiment of the present application, the robot can plan its own motion trajectory based on two-dimensional space and execute its own target task without affecting each other. At the same time, the motion trajectory generated by the method can realize mutual avoidance of each robot and collision avoidance with obstacles in the external environment. The robot motion control method is described in detail below.

[0054] Figure 4 A flow chart of a robot motion control method according to an embodiment of the present application is shown. Exemplarily, the robot motion control method comprises the following steps:

[0055] S110, responding to the task instruction to obtain the current state of the robot itself and external obstacle information.

[0056] Among them, the task instructions can be issued in various forms, for example, the user obtains the task instructions through voice interaction with the robot, or the robot receives the task instructions issued by the server background, etc. The type of task can also be set according to the type of service required by each robot in the application scenario, which is not specifically limited here. For example, in a certain area, robot A can be assigned to perform item delivery tasks, robot B to perform cleaning tasks, robot C to perform road guidance tasks, etc.

[0057] Exemplarily, after receiving the task instruction, each robot will parse the task instruction to obtain specific task information, for example, including but not limited to the target state to be reached (such as the position and posture of the task target point to be reached), etc. In addition, the robot will also obtain the obstacle information of its current environment and its current state (including the robot's position and motion state, etc.).

[0058] Among them, the external obstacle information mainly includes the position and shape of static obstacles in the robot's environment, for example, the occupied position information in the environment map. Usually, in order to facilitate the processing of position information, the environment map will be rasterized to obtain the corresponding grid map. Then, accordingly, based on the position and shape information of each obstacle, the grid area occupied by each obstacle in the grid map of the environment can be obtained.

[0059] In some cases, the above obstacle information can be identified by the external perception module deployed in the environment to identify these static obstacles (such as walls, tables and chairs or other static objects that may block the robot's movement). It is understandable that these robots can also pre-store maps of the environment and the above obstacle information. Of course, if the obstacles are updated, the latest obstacle information will be obtained in the robot accordingly. Among them, the obstacle information includes the position information of each obstacle in the map of the environment where the robot is located.

[0060] S120, based on the obstacle information, planning a safe path for the robot to reach the target state indicated in the task instruction from the current state.

[0061] The safe path refers to a continuous path that can safely reach the task target point from the current state of the robot. The safety here is relative to static obstacles, that is, it can avoid the above static obstacles.

[0062] In this application, after the obstacle information of the external environment is known, a continuous safe path will be planned first, and then a motion trajectory based on the two-dimensional space will be generated based on the safe path.

[0063] In some embodiments, Figure 5 As shown, step S120 includes the following sub-steps:

[0064] S210, determining the kinematic state parameters and control variables of the robot based on the robot configuration to construct a state transfer equation of the robot.

[0065] Among them, different robot configurations will affect the robot's kinematic state parameters and control quantities. It can be understood that by inputting corresponding control quantities to the robot, the robot's state parameters can be changed as expected on time, thereby making the robot move and reach the task target point.

[0066] For example, in one embodiment, the robot is configured as the wheeled robot mentioned above, and its kinematic state parameters may include the position and posture of the wheeled robot in the environment map. For example, the state parameters are defined as ξ = [x, y, φ] T , where x and y represent two-dimensional coordinates for indicating position, and φ is the heading angle for indicating attitude; accordingly, its kinematic control quantity may include the linear velocity and angular velocity of the robot. For example, the control quantity is defined as u = [v, ω] T , where v and w represent linear velocity and angular velocity respectively. Therefore, the state transition equation of the wheeled robot is constructed as follows: Expand to get:

[0067]

[0068] In the formula, are the differentials of x, y, φ with respect to time t;

[0069] Based on this, in the unit time interval, the k+1th period state parameter ξ k+1 and the kth period state parameter ξ k The change Δξ can be described as: Δξ=ξ k+1 -ξ k =f′(ξ k ,u k ), expand it to get:

[0070]

[0071] Where Δx, Δy, Δφ are x, y, and φ is the change that occurs in the unit time interval Δt.

[0072] It can be understood that if the control quantity is sampled periodically, when the current state and the change in the state parameter per unit time interval are known, the state of the robot in each subsequent sampling period can be gradually searched, thereby obtaining all possible states of the robot sampled in each period. Among them, regarding the selection of the sampling period, for example, the safe path can be sampled at equal distances (the size can be determined according to the actual situation) to determine the position information of each sampling point.

[0073] In addition, before searching for candidate states in the robot's environment map, the environment map can be rasterized to obtain a grid map, so that all possible states (including positions and postures) that can be reached at each step in the search process can be determined in the grid map.

[0074] S220, starting from the current state, all candidate states of each subsequent sampling period are searched step by step according to the state transition equation according to the preset step length, and invalid candidate states that collide with any obstacle are removed from all candidate states until the target state indicated in the task instruction is searched to obtain a safe path. The preset step length can be set according to actual needs and is not limited here.

[0075] Assume that from the current state of the robot (the starting state) ξ0 = [x0, y0, φ0] T And the initial control amount u0 = [u0, 0] T Start searching according to the set step size, such as Figure 6 As shown, all possible states (i.e. candidate states) of the second cycle obtained by sampling in the first cycle are:

[0076] ξ1={ξ1|ξ1=ξ0+f′(ξ0,u0+KΔu)};

[0077] Where Ξ1 is the set of all possible states ξ1 in the second cycle, and the step length Δu = [Δv, Δω] T , K is a coefficient matrix, where for one period sampling, different states are obtained with different K. Taking the second period sampling as an example, multiple different candidate states can be obtained by adjusting the element values ​​of K.

[0078] Considering that there are still obstacles in the environment, it is necessary to further use the obstacle information to remove the candidate states that collide with any obstacle in the above set (recorded as invalid candidate states). Figure 6 As shown, among the six candidate states, one of the candidate states represented by the dotted line will collide and therefore needs to be removed, and the remaining five are all candidate states of the second cycle.

[0079] By analogy, the same method as above can be used to search for all possible states {Ξ2, Ξ3, ...} in each subsequent cycle until the target state ξ is found. f =[x f ,y f ,φ f ] T ∈Ξ n , where Ξ n It represents all possible states including the target state. It can be understood that the number of sampling cycles can be determined according to requirements.

[0080] Finally, by moving from the target state ξ f By backtracking to the current state ξ0, we can find a continuous safe path (denoted as P) that can avoid obstacles, such as Figure 7 shown.

[0081] S130, vectorizing obstacle information to construct a static safety space along the safety path.

[0082] Among them, vectorization refers to the process of converting raster data describing obstacle information into vector data. Considering that each robot itself has a certain volume, since it moves in a two-dimensional space such as the ground, it is impossible to simply regard each robot as a particle to generate its motion trajectory. In addition, there are static obstacles with different shapes, and the avoidance space on the ground itself is limited. Therefore, in order to avoid collisions with these obstacles, this application uses geometric graphics (i.e. vectorization processing) to describe the safe space (i.e. static safe space) in which the robot can avoid various static obstacles when traveling along a safe path. It can be understood that as long as the robot is always in the safe space during the movement, collisions with external static obstacles can be avoided.

[0083] For example, in some embodiments, Figure 8As shown, step S130 includes the following sub-steps:

[0084] S310, based on the occupied position information of each obstacle, starting from the current state, preliminarily determine a convex polygonal area that does not contain any obstacle in the map, expand the convex polygonal area to the maximum area without collision, and obtain the maximum convex polygonal area.

[0085] The above-mentioned occupied position information mainly refers to the grid area occupied by the obstacle in the grid map. For the safe space in the two-dimensional space to be constructed, it can be defined as a set of convex polygons (denoted as q), which should satisfy that any convex polygon should not include any obstacle or intersect with it, and its area should be as large as possible, so as to ensure that the robot will not collide with obstacles in any convex polygon and has enough space for movement.

[0086] In some embodiments, each convex polygon q is surrounded by a plurality of straight lines, defining a straight line l in two-dimensional space. m The expression is: Where [x, y] is the coordinate of a point on a line in two-dimensional space. For the convenience of calculation, we take the simplified convex polygon q as a rectangle surrounded by four straight lines as an example. Then, the vector data of a rectangle can be expressed as a set like Fig. 9 As shown. Therefore, the set ∈ composed of multiple rectangles can be expressed as:

[0087]

[0088] in,

[0089] For step S310, for example, taking the current state of the robot as an example, first determine an initial convex polygon (such as Fig.10 The area of ​​the convex polygon is then increased until it cannot be increased any further (without colliding with obstacles). It can be understood that any grid area cannot be included when initially determining or expanding the convex polygon area. In this way, a convex polygon with the largest area corresponding to the current state (i.e., the largest convex polygon area) can be found.

[0090] S320, continue searching for the maximum convex polygon area of ​​the next state around the safe path until the target state is reached, and describe all the obtained maximum convex polygon areas in vector form to obtain a static safe space.

[0091] Similarly, the above method is used to search for the next largest convex polygon around the safe path P on the grid map until the largest convex polygon containing the target state is found. These largest convex polygons form a convex polygon set ∈, such as Fig.11 The dashed rectangles shown are the vector data of the safe space. So far, the vectorization process of the static obstacle is completed.

[0092] S140, obtaining the motion trajectory of the robot based on the objective function optimization, where the objective function includes constraint conditions constructed based on the safe path, the static safe space, and the received trajectory information of other robots in the cluster network.

[0093] Among them, the objective function is used to solve the optimal motion trajectory of the robot when performing the current target task. It can be understood that in this application, the objective function and constraint range are constructed using the above-solved safe path, safe space, and trajectory information of other robots, and then the extreme value of the objective function within the constraint range is solved, and finally a motion trajectory that is safe relative to static obstacles and other robots is obtained.

[0094] The construction of the objective function and constraint conditions in step S140 is described below.

[0095] First, before constructing the objective function, the motion trajectory is first defined. In one embodiment, the motion trajectory is defined as a continuous curve. t represents time, T is the maximum running time of the entire trajectory, Among them, the points on the motion trajectory are defined as (Note that T in the matrix or vector represents a permutation operation.) Usually, a motion trajectory can be divided into multiple sub-trajectories, namely: In the formula, the j-th sub-trajectory can be expressed as M is the total number of sub-trajectories, which can be divided according to actual conditions and is not limited here. Optionally, the running time t′ required for each sub-trajectory is equal, and its value range is

[0096] Based on this, a sub-trajectory can be expressed by a parameter vector α(t) and a basis matrix β:

[0097]

[0098] In the formula, α j (t) = [1, t, t 2 , ..., t N ] T , N is the order. For the convenience of calculation, the empirical value N=5 can be selected as an example. It can also be other values, which are set according to actual needs. Generally, the larger N is, the smoother the trajectory is, but the amount of calculation will also increase.

[0099] Therefore, a motion trajectory can be determined by a set of basis matrices, where the set of basis matrices is expressed as:

[0100]

[0101] Furthermore, according to the expression of a sub-trajectory, the entire motion trajectory can be expressed as:

[0102]

[0103] Thus, a motion trajectory represented by a basis matrix β and a maximum running time T can be obtained. It can be understood that the basis matrix is ​​a linearly independent vector, and any vector in the vector space can be obtained by a linear combination of the vectors in the basis matrix. Since there are multiple sub-trajectories in the motion trajectory, this application uses a set of basis matrices to describe these sub-trajectories and the entire motion trajectory, which is convenient for calculation and analysis. It should be understood that by performing differential operations on the basis matrix and the parameter vector, the heading angle (i.e., attitude information) contained in the trajectory can be obtained. Next, after defining the expression of the motion trajectory, the objective function is constructed below.

[0104] Among them, the objective function consists of two parts, one is the optimization target, and the other is the optimized variable. The process of numerical optimization is to continuously iterate the optimized variable so that the objective function reaches the minimum value. For example, as an optional solution, the base matrix β and the maximum running time T used to represent the motion trajectory can be used as optimization variables, and the minimum efficiency of the robot running along the motion trajectory can be selected as the optimization target.

[0105] For the quantification of the operating efficiency, for example, two aspects can be considered, including: the maximum operating time of the entire motion trajectory and the energy required to control the robot to run along the motion trajectory. If described by an expression, then: J(β, T) = G + q T T s , where J is the operating efficiency, G is the energy, T s is the maximum running time, q T is the weight factor used to adjust the maximum running time.

[0106] For example, by integrating the entire motion trajectory represented by the basis matrix and the maximum running time over the maximum running time, an expression for the energy can be constructed. If described by an expression, then: Q is the weight coefficient used to adjust energy; where T s =T,

[0107] Among them, the superscript letter s and the motion trajectory The highest differential order of differs by one, and s is usually greater than 1. For example, if s = 2, then If s = 3, then It can be understood that when any time t is given, by calculating The position of the robot can be obtained by calculating The direction and speed of the robot can be obtained by calculating The acceleration can be obtained. Since the acceleration information of the robot is needed here, s can be set to 3. It can be abbreviated as a function η(t) related to time t, and the final form of the objective function can be:

[0108]

[0109] in,

[0110] It should be noted that the above objective function J(β, T) is also provided with some constraints, for example, including but not limited to a first constraint item related to trajectory smoothness and a second constraint item related to motion safety.

[0111] Among them, the first constraint item includes but is not limited to the following: the states of the starting point and the end point of the motion trajectory to be generated respectively conform to the current state and the target state of the robot, that is, the state of the starting point of the motion trajectory is the current state, and the state of the end point is the target state; and the state continuity is satisfied between each two adjacent sub-trajectories in the motion trajectory to be generated, that is, the state of the previous sub-trajectory at the last moment is the same as the state of the next sub-trajectory at the starting moment, including position, heading angle, velocity and acceleration, etc.

[0112] For example, in order to ensure that the state of the starting point of the motion trajectory is the current state ξ0, and the state of the end point is the target state ξ f , then the corresponding design constraints are:

[0113]

[0114] In the formula,

[0115]

[0116] In the formula, and are the current state ξ0 and the target state ξ f The first-order differential of , v0 and v fare the velocities at the starting point and end point of the motion trajectory, φ0 and φ f The starting and ending points.

[0117] Due to the trajectory Multiple sub-trajectories In order to ensure the smoothness and continuity of the entire trajectory, an equality constraint is designed here to ensure that the jth sub-trajectory and the j+1th sub-trajectory can be smoothly connected, that is: In the formula, the superscript [d] represents the differential order of the sub-trajectory. For example, when d = 1, it means that the velocity and heading angle at the last moment of the j-th sub-trajectory (t = T') and the initial moment of the j+1-th sub-trajectory (t = 0) are equal; similarly, when d = 2, it means that the accelerations at the corresponding moments of the two are equal.

[0118] Among them, the second constraint item, for example, includes but is not limited to, the kinematic constraint of the robot itself, the robot is located in the static safety space at any time, and the minimum safety distance between robots is met at any time according to the trajectory information of other robots.

[0119] Taking a wheeled robot as an example, the kinematic constraints of the robot itself include, but are not limited to, that the speed, acceleration and / or curvature at any time do not exceed the upper limit of the corresponding parameters of the robot. The construction of the above kinematic constraints is described below.

[0120] Taking speed as an example, we can design inequality constraints: In the formula, v max It is the maximum speed that the robot can reach. According to the calculation formula of state parameters and control quantity speed in kinematics So we have:

[0121]

[0122] Taking acceleration as an example, the acceleration a of a wheeled robot includes the lateral acceleration a n and longitudinal acceleration a t , it can be understood that according to the relationship between acceleration and velocity, the lateral acceleration a n and longitudinal acceleration a t are the projections in the direction of velocity normal and velocity direction respectively.

[0123] In some embodiments, the lateral acceleration a n and longitudinal acceleration a t The corresponding constraints are:

[0124]

[0125] In the formula

[0126] Taking curvature as an example, according to robot kinematics, we know the conversion relationship between curvature and motion state parameters, so we can design inequality constraints:

[0127]

[0128] Among them, for the constraint item that the robot is in the static safety space at any time, it is mainly necessary to satisfy the robot's safety relative to the static obstacles. This can be calculated from two factors, one is the size and position of the robot E itself, and the other is the convex polygon set ∈ corresponding to the static safety space. For example, in one embodiment, the robot can be defined as the first polygon that matches the size of the robot. Exemplarily, the polygon of the robot can be represented by n e 2D vertices For the convenience of calculation, we can let n e =4, such as Fig.12 As shown, at this time, the robot is defined as a rectangle. It can be understood that the polygonal definition of the robot, such as including but not limited to a rectangle, is only an example and is not the only limitation.

[0129] It can be understood that as long as all vertices v of the first polygon of robot E e A maximum convex polygon q that is in the set of convex polygons ∈ z Internally, it can ensure that robot E is always in q z Internally, this ensures that the robot is safe relative to static obstacles Θ.

[0130] For example, if the position of the robot is defined by the coordinates of the points on the motion trajectory and the rotation matrix R of the point, the corresponding constraint expression can be constructed as follows:

[0131]

[0132] In the formula

[0133] In addition, for the constraint item of the minimum safe distance between robots, it is mainly necessary to satisfy that the robot is safe relative to other moving robots, that is, the robot does not collide with other robots at any time, such as Fig.13 There is no collision at t = 0 and t = t'. This can be calculated from two factors: the size of robot E itself and its position at time t, and the trajectory information S of other robots. S iRefers to other robots numbered i. Then, for time t, the corresponding constraint expression can be constructed as:

[0134]

[0135] C Ψ，i (t) = d min -U i (t)≤0;

[0136] Where, d min represents the minimum distance between robots to ensure safety, U i (t) represents the robot E and the i-th robot S at time t i The distance between s is the total number of other robots.

[0137] To calculate U i (t), as an optional solution, considering that each robot has a certain size, the robot and other robots can be defined as a first polygon (denoted as A) and a second polygon (denoted as B) that match their respective sizes; then, a distance function is designed to calculate the minimum distance between the two polygons A and B.

[0138] In some embodiments, regarding robot E and the i-th other robot S i The distance between them can be further divided into two cases for design. The first is that there is no collision, such as Fig.14 As shown in (a), at this time U i (t)>0; where |U i (t)| is larger, C Ψ，i (t) is smaller; the second is a collision, such as Fig.14 As shown in (b), at this time U i (t)<0; where |U i The smaller (t)| is, the Ψ，i The smaller (t) is, the better. The above design can make C Ψ，i (t) is as small as possible, so that the optimized motion trajectory is as far away from other robots as possible.

[0139] For the convenience of calculation, an approximate method can be used here, that is, the distance between the edges and points of the two polygons is used to approximate the minimum distance between the two polygons. Demonstration, by calculating the distance set between the edges and vertices of the first polygon and the second polygon, and selecting the maximum value from the distance set, the minimum distance Δ(A, B) between the two robots can be approximately obtained. If described by an expression, then:

[0140]

[0141] In the formula, refers to the pth side of the first polygon A, refers to the qth side of the second polygon B. For example, Fig.15 As shown in (a), B is in below; Fig.15 As shown in (b), A is in Below.

[0142] Then, by selecting the maximum value from the minimum distances from the two sets of points to the line, the distance between A and B can be approximated. It can be understood that there is a corresponding relationship between the vertices of the polygons of each robot and the occupied position of the robot. Therefore, the vertex and edge information of the first polygon A used for calculation at any time t can be expressed by the trajectory information; similarly, the vertex and edge information of the second polygon B at any time t can also be directly expressed according to the corresponding trajectory information of other robots, which will not be described here.

[0143] Thus, the objective function and related constraints are constructed. Then, an open source numerical optimization solver can be used to iteratively solve the objective function, and finally a set of optimization variables (β, T) that minimize the objective function within the constraints can be obtained. These two known variables can determine a complete motion trajectory. As the output result of trajectory optimization.

[0144] It can be understood that by polygonal modeling of the trajectories and movements of other robots, the safety relative to other robots in time and space is taken into account (as a constraint condition) when generating its own motion trajectory. When the robot moves along the trajectory, this safety ensures that no robot will collide with each other at any time. Moreover, since the functional module used for trajectory generation runs in real time, it ensures that each robot can adjust its own motion trajectory in a scene with multiple robots in a timely manner, so that all robots can avoid each other smoothly and efficiently complete their business goals.

[0145] S150, controlling the robot to execute target motion according to the motion trajectory, and broadcasting the motion trajectory to the cluster network.

[0146] Exemplarily, by optimizing the output motion trajectory By sampling at equal intervals, we can obtain a trajectory S containing n discrete points, that is, S = {s0, ..., s w , ..., s n}, where the wth discrete point s w =[t w , x w ,y w ,φ w ] T , tw represents the w-th moment, x w ,y w It means that the robot is at time t w The two-dimensional coordinates (corresponding to the X direction and Y direction respectively), φ w It means that the robot is at time t w The heading angle (i.e. attitude) of .

[0147] Therefore, the linear velocity and angular velocity corresponding to the trajectory of these discrete points are calculated and then sent to the robot's drive module to control the robot's movement. At the same time, the trajectory S of these discrete points and the robot's own number are generated into a message and broadcast to the cluster network for use by other robots, so that each robot in the cluster network can move efficiently and without collision.

[0148] The robot motion control method of the present application is deployed on each robot, and each robot communicates through an agreed broadcast protocol to form a decentralized cluster network. It can be seen that for each robot in the network, the trajectory information of other robots can be received in real time, and a continuous safe path for the robot to avoid obstacles in the external environment can be planned according to the task instructions, and then a static safe space along the safe path is constructed through vector processing to ensure that the robot will not collide at any time; then, a trajectory optimization problem for motion efficiency is constructed, and a series of safety constraints are designed using safe paths, safe spaces, and trajectory information of other robots. Finally, the motion trajectory of the robot is generated by solving the optimization problem, which can not only ensure the smoothness of the robot's motion, but also improve work efficiency.

[0149] Fig.16 A schematic structural diagram of a robot motion control device 100 according to an embodiment of the present application is shown. Exemplarily, the robot motion control device 100 includes:

[0150] The data acquisition module 110 is used to respond to the task instruction to obtain the current state of the robot itself and external obstacle information;

[0151] A path planning module 120, for planning a safe path for the robot to reach a target state indicated in the task instruction from the current state based on the obstacle information;

[0152] A vector processing module 130, configured to vectorize the obstacle information to construct a static safety space along the safety path;

[0153] A path optimization module 140, configured to optimize the motion trajectory of the robot based on an objective function, wherein the objective function includes constraints constructed based on the safe path, the static safety space, and trajectory information of other robots in the received cluster network;

[0154] A motion control module 150, used to control the robot to perform a target motion according to the motion trajectory;

[0155] The trajectory broadcasting module 160 is used to broadcast the movement trajectory to the cluster network.

[0156] In some embodiments, the path planning module 120 includes a parameter definition unit and a state search unit, wherein the parameter definition unit is used to determine the kinematic state parameters and control quantities of the robot based on the robot configuration to construct the state transfer equation of the robot; the state search unit is used to search all candidate states of each subsequent sampling cycle step by step according to the state transfer equation, starting from the current state, according to a preset step size, and remove invalid candidate states that collide with any obstacle among all candidate states, until the target state indicated in the task instruction is searched to obtain a safe path.

[0157] In an optional solution, the obstacle information includes the position information of each obstacle in the environment map where the robot is located; the vector processing module 130 includes a convex polygon determination unit and a safe space generation unit, wherein the convex polygon determination unit is used to preliminarily determine a convex polygon area that does not contain any obstacle in the map based on the position information of each obstacle, starting from the current state, and perform collision-free expansion of the convex polygon area to the maximum area to obtain the maximum convex polygon area. The safe space generation unit is used to continue searching for the maximum convex polygon area of ​​the next state around the safe path until the target state is reached, and all the obtained maximum convex polygon areas are described in vector form to obtain a static safe space.

[0158] In some optional solutions, the path optimization module 140 obtains the motion trajectory of the robot based on the objective function optimization, which also includes:

[0159] The basis matrix used to represent the motion trajectory and the maximum running time are used as optimization variables, and minimizing the running efficiency of the motion trajectory is used as the optimization goal to construct the objective function.

[0160] In some optional schemes, the operating efficiency of the motion trajectory includes the maximum running time of the entire motion trajectory and the energy required to control the robot to run along the motion trajectory; the expression of the energy is constructed by integrating the trajectory represented by the basis matrix and the maximum running time within the maximum running time.

[0161] In some optional schemes, the constraint conditions of the objective function include a first constraint item related to trajectory smoothness and a second constraint item related to motion safety. Among them, the first constraint item includes that the states of the starting point and the end point of the motion trajectory to be generated are respectively consistent with the current state and the target state of the robot, and that the state continuity is satisfied between each two adjacent sub-trajectories in the motion trajectory to be generated. The second constraint item includes the kinematic parameter constraints of the robot itself, that the robot is located in the static safety space at any time, and that the minimum safety distance between robots is satisfied at any time according to the trajectory information of other robots.

[0162] In some optional schemes, the constraint item for the robot to be located in the static safety space at any time includes: defining the robot as a first polygon matching the size of the robot; and constructing the corresponding constraint item based on making all vertices of the first polygon located inside any maximum convex polygon area in the static safety space.

[0163] In some optional schemes, a constraint item that satisfies the minimum safety distance between robots at any time is constructed based on the trajectory information of other robots, including: defining the robot and the other robots as a first polygon and a second polygon that match their respective sizes; and constructing a corresponding constraint item based on ensuring that the minimum distance between the first polygon and the second polygon at any time is not greater than a preset minimum safety distance.

[0164] In some optional solutions, a distance set between the edges and vertices of the first polygon and the second polygon is calculated, and a maximum value is selected from the distance set to obtain the minimum distance between the two robots.

[0165] In some optional schemes, the kinematic parameter constraints of the robot itself include: the speed, acceleration and / or curvature at any time do not exceed the upper limit values ​​of the corresponding parameters of the robot.

[0166] It can be understood that the device of this embodiment corresponds to the robot motion control method of the above embodiment, and the options in the above embodiment are also applicable to this embodiment, so they will not be described repeatedly here.

[0167] The present application also provides a computer-readable storage medium for storing the computer program used in the robot. For example, the computer-readable storage medium may include, but is not limited to, various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0168] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and structure diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in an alternative implementation, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the structure diagram and / or the flow diagram, and the combination of boxes in the structure diagram and / or the flow diagram, can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0169] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A robot motion control method, characterized in that: include: Respond to task instructions to obtain the robot's current state and external obstacle information; Based on the obstacle information, planning a safe path for the robot to reach the target state indicated in the task instruction from the current state; vectorizing the obstacle information to construct a static safety space along the safety path; Obtaining a motion trajectory of the robot based on an objective function optimization, wherein the objective function includes constraint conditions constructed based on the safety path, the static safety space, and trajectory information of other robots in the received cluster network; The robot is controlled to execute target motion according to the motion trajectory, and the motion trajectory is broadcast to the cluster network.

2. The robot motion control method according to claim 1, characterized in that: The step of planning a safe path for the robot to reach a target state indicated in the task instruction from the current state based on the obstacle information includes: Determine the kinematic state parameters and control quantities of the robot based on the robot configuration to construct a state transfer equation of the robot; According to the state transfer equation, starting from the current state, all candidate states of each subsequent sampling cycle are searched step by step according to a preset step size, and invalid candidate states that collide with any of the obstacles are removed from all the candidate states until the target state indicated in the task instruction is searched to obtain a safe path.

3. The robot motion control method according to claim 2, characterized in that: The robot is configured as a wheeled robot, and the state transfer equation is constructed based on the kinematic state parameter and the control quantity of the wheeled robot; The state parameters include the position and posture of the wheeled robot in the environment map, and the control quantity includes the linear velocity and angular velocity of the wheeled robot.

4. The robot motion control method according to claim 1, characterized in that: The obstacle information includes the position information of each obstacle in the environment map where the robot is located; vectorizing the obstacle information to construct a static safety space along the safety path includes: According to the occupied position information of each obstacle, starting from the current state, preliminarily determining a convex polygonal area in the map that does not contain any of the obstacles, and performing collision-free expansion on the convex polygonal area until it reaches a maximum area, thereby obtaining a maximum convex polygonal area; Continue searching for the maximum convex polygon area of ​​the next state around the safety path until the target state is reached, and describe all the obtained maximum convex polygon areas in vector form to obtain a static safety space.

5. The robot motion control method according to claim 4, characterized in that: The vectorizing the obstacle information further includes: Performing rasterization processing on the environment map where the robot is located to obtain a raster map; According to the obstacle information, the grid area occupied by each obstacle in the grid map is determined, wherein the convex polygon area does not include any of the grid areas during the preliminary determination or the expansion.

6. The robot motion control method according to claim 1, characterized in that: The motion trajectory of the robot is obtained based on the objective function optimization, and the method further includes: The objective function is constructed by taking a basis matrix for representing the motion trajectory and a maximum running time as optimization variables and minimizing the running efficiency of the motion trajectory as an optimization goal.

7. The robot motion control method according to claim 6, characterized in that: The operation efficiency of the motion trajectory includes the maximum operation time of the entire motion trajectory and the energy required to control the robot to run along the motion trajectory; The expression for the energy is constructed by integrating the trajectory represented based on the basis matrix and the maximum runtime over the maximum runtime.

8. The robot motion control method according to claim 1, characterized in that: The constraint condition of the objective function includes a first constraint term related to trajectory smoothness and a second constraint term related to motion safety; The first constraint item includes that the states of the starting point and the end point of the motion trajectory to be generated respectively conform to the current state and the target state of the robot, and that the state continuity is satisfied between each two adjacent sub-trajectories in the motion trajectory to be generated; The second constraint item includes the kinematic parameter constraint of the robot itself, the robot being located in the static safety space at any time, and the minimum safety distance between robots being satisfied at any time according to the trajectory information of other robots.

9. The robot motion control method according to claim 8, characterized in that: The constraint item construction of the robot being in the static safety space at any time includes: The robot is defined as a first polygon matching the size of the robot; Corresponding constraint items are constructed based on making all vertices of the first polygon located inside any maximum convex polygon area in the static safety space.

10. The robot motion control method according to claim 8, characterized in that: The construction of the constraint item satisfying the minimum safety distance between robots at any time according to the trajectory information of other robots includes: The robot and the other robots are defined as a first polygon and a second polygon that match their respective sizes; The corresponding constraint item is constructed based on ensuring that the minimum distance between the first polygon and the second polygon at any time is not greater than a preset minimum safety distance.

11. The robot motion control method according to claim 10, characterized in that: The minimum distance between the two robots is obtained by calculating the distance set between the edges and vertices of the first polygon and the second polygon, and selecting the maximum value from the distance set.

12. The robot motion control method according to claim 8, characterized in that: The kinematic parameter constraints of the robot itself include: the speed, acceleration and / or curvature at any time do not exceed the upper limit value of the corresponding parameter of the robot.

13. A robot motion control device, characterized in that: include: The data acquisition module is used to respond to task instructions to obtain the current state of the robot itself and external obstacle information; A path planning module, used to plan a safe path for the robot to reach the target state indicated in the task instruction from the current state based on the obstacle information; A vector processing module, used for vectorizing the obstacle information to construct a static safety space along the safety path; A path optimization module, used for optimizing the motion trajectory of the robot based on an objective function, wherein the objective function includes constraints constructed based on the safe path, the static safety space, and trajectory information of other robots in the received cluster network; The motion control module is used to control the robot to perform target motion according to the motion trajectory and broadcast the motion trajectory to the cluster network.

14. A robot cluster motion system, characterized in that: include: A plurality of robots, each of which forms a cluster network through a broadcast protocol; Each of the robots is used to receive real-time trajectory information of other robots in the cluster network, and execute the robot motion control method described in any one of claims 1-12 to generate its own motion trajectory to control the robot to perform its own target motion, and broadcast the self-motion trajectory to the cluster network.

15. A robot, characterized in that: The robot comprises a processor and a memory, wherein the memory stores a computer program, and the processor is configured to execute the computer program to implement the robot motion control method according to any one of claims 1 to 12.

16. A computer-readable storage medium, characterized in that: The computer program is stored therein, and when the computer program is executed, the robot motion control method according to any one of claims 1 to 12 is implemented.

Images