A robot control method and device, electronic equipment and storage medium
By using the path point information and current angular velocity at the next moment after path point planning, the robot control method independently generates control commands, solving the problem of unsmooth and unstable robot motion caused by the DWA algorithm and achieving better motion control effect.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-03-05
- Publication Date
- 2026-03-17
AI Technical Summary
Existing DWA algorithms combine path points and control commands in their calculations, which makes it impossible to guarantee the smoothness and stability of robot motion.
After planning the path points, the second pose is determined using the path point information at the next moment. Combined with the current angular velocity and the first pose, the initial state variables are calculated, and then the target angular velocity and control commands are generated. The control commands are calculated independently to ensure the smoothness and stability of the robot's motion.
By independently calculating control commands, the smoothness and stability of robot motion are ensured, avoiding the influence of path point planning on control command calculation.
Smart Images
Figure CN115008450B_ABST
Abstract
Description
Technical Field
[0001] This application relates to the field of robotics, and in particular to a robot control method, device, electronic device, and storage medium. Background Technology
[0002] The purpose of trajectory tracking is to enable mobile robots to autonomously generate a series of continuous control commands (control commands include: velocity v, angular velocity ω), so that the robot can move smoothly along a pre-given path within the allowable tracking error range and safely reach the target point.
[0003] The commonly used tracking algorithm is the DWA (dynamic window approach) algorithm. Its calculation process involves sampling multiple sets of velocities in the velocity space (v, ω) and simulating their trajectories over a certain time period. These trajectories include corresponding path points and control commands. By evaluating the cost, the trajectory with the lowest cost is selected as the target trajectory. In other words, the path points and control commands are combined for calculation, and the trajectory with the lowest cost is chosen as the target trajectory.
[0004] However, since the DWA algorithm combines path points and control commands in its calculations, the planning of path points affects the calculation of control commands, thus failing to guarantee the smoothness and stability of the robot's motion. Summary of the Invention
[0005] The purpose of this application is to provide a robot control method, device, electronic device, and storage medium to solve the problem that the DWA algorithm combines path points and control commands for calculation, and that path point planning affects the calculation of control commands, thus failing to guarantee the smoothness and stability of robot motion. The specific technical solution is as follows:
[0006] Firstly, a robot control method is provided, the method comprising:
[0007] Obtain the velocity corresponding to each path point in the multiple path points of the movement trajectory, where each path point corresponds to a time point;
[0008] Obtain the robot's current angular velocity, first pose information at the current moment, and second pose information at the next moment;
[0009] Based on the current angular velocity, the first pose, and the second pose, determine the initial state quantity corresponding to the initial moment of the preset time period;
[0010] For each moment within a preset time period, a target angular velocity is calculated based on the initial state quantity. A control command is generated based on the target angular velocity and the velocity corresponding to that moment. The control command is used to control the robot's movement.
[0011] Optionally, the first pose includes: a first horizontal and vertical coordinate and a first heading angle; the second pose includes: a second horizontal and vertical coordinate and a second heading angle; the step of determining the initial state quantity corresponding to the initial moment of a preset time period based on the current angular velocity, the first pose, and the second pose includes:
[0012] The lateral displacement error is calculated based on the first heading angle, the second heading angle, the first horizontal and vertical coordinates, and the second horizontal and vertical coordinates; the heading angle error is calculated based on the first heading angle and the second heading angle.
[0013] The initial state quantities are determined based on the current angular velocity, the lateral displacement error, and the heading angle error.
[0014] Optionally, calculating the target angular velocity based on the initial state quantity includes:
[0015] Substituting the initial state quantities and multiple preset output angular velocities into a preset kinematic model, the rate of change of multiple initial state quantities corresponding to the initial moment is obtained;
[0016] Multiple candidate state quantities are calculated using the initial state quantities and the rate of change of the multiple initial state quantities;
[0017] Substitute the multiple output angular velocities and the multiple candidate state variables into the quadratic optimization formula to determine the target angular velocity from the multiple output angular velocities.
[0018] Optionally, the kinematic model is as follows:
[0019]
[0020] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; ω is the angular velocity; v is the velocity; t is the control period; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0021] Optionally, the kinematic model is as follows:
[0022]
[0023] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; v is the velocity; ω d To output angular velocity, ω r Let be the feedforward angular velocity. Optionally, the quadratic optimization formula is as follows:
[0024] J = X T QX+(UU ref ) T R(UU ref )
[0025] Where X is the state variable; U is the output angular velocity; Uref is the expected value; J is the optimization index; Q is the state variable weighting coefficient; and R is the output angular velocity weighting coefficient.
[0026] Optionally, the step of calculating multiple candidate state quantities using the initial state quantity and the rate of change of the multiple initial state quantities includes:
[0027] For each initial state variable change rate, the initial state variable change rate is multiplied by the control period to obtain the change amount of the initial state variable;
[0028] For each change in an initial state quantity, the initial state quantity is added to the change in the initial state quantity to obtain the candidate state quantity.
[0029] Secondly, a robot control device is provided, the device comprising:
[0030] The first acquisition module is used to acquire the speed corresponding to each path point among multiple path points of the movement trajectory, wherein each path point corresponds to a time point;
[0031] The second acquisition module is used to acquire the robot's current angular velocity at the current moment, the first pose information at the current moment, and the second pose information at the next moment.
[0032] The determination module is used to determine the initial state quantity corresponding to the initial moment of a preset time period based on the current angular velocity, the first pose, and the second pose.
[0033] The calculation module is used to calculate the target angular velocity based on the initial state quantity for each moment within a preset time period, and to generate control commands based on the target angular velocity and the velocity corresponding to that moment. The control commands are used to control the movement of the robot.
[0034] Optionally, the first pose includes: a first horizontal and vertical coordinate and a first heading angle; the second pose includes: a second horizontal and vertical coordinate and a second heading angle; the determining module includes:
[0035] The calculation unit is used to calculate the lateral displacement error based on the first heading angle, the second heading angle, the first lateral coordinate, and the second lateral coordinate, and to calculate the heading angle error based on the first heading angle and the second heading angle;
[0036] The first determining unit is used to determine the initial state quantity based on the current angular velocity, the lateral displacement error, and the heading angle error.
[0037] Optionally, the computing module includes:
[0038] The first obtaining unit is used to substitute the initial state quantity and a preset plurality of output angular velocities into a preset kinematic model to obtain the rate of change of the plurality of initial state quantities corresponding to the initial moment.
[0039] The second obtaining unit is used to calculate multiple candidate state quantities using the initial state quantities and the rate of change of the multiple initial state quantities;
[0040] The second determining unit is used to substitute the plurality of output angular velocities and the plurality of candidate state variables into the quadratic optimization formula to determine the target angular velocity among the plurality of output angular velocities.
[0041] Optionally, the kinematic model is as follows:
[0042]
[0043] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; ω is the angular velocity; v is the velocity; t is the control period; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0044] Optionally, the kinematic model is as follows:
[0045]
[0046] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; v is the velocity; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0047] Optionally, the quadratic optimization formula is as follows:
[0048] J = X T QX+(UU ref ) T R(UU ref )
[0049] Where X is the state variable; U is the output angular velocity; Uref is the expected value; J is the optimization index; Q is the state variable weighting coefficient; and R is the output angular velocity weighting coefficient.
[0050] Optionally, the second obtaining unit includes:
[0051] The first subunit is used to multiply the rate of change of the initial state quantity with the control period for each rate of change of the initial state quantity to obtain the change of the initial state quantity.
[0052] The second sub-unit is used to add the initial state quantity to the change of the initial state quantity for each initial state quantity to obtain the candidate state quantity.
[0053] Thirdly, an electronic device is provided, including a processor, a communication interface, a memory, and a communication bus, wherein the processor, the communication interface, and the memory communicate with each other through the communication bus;
[0054] Memory, used to store computer programs;
[0055] When a processor executes a program stored in memory, it implements any of the steps described in the first aspect.
[0056] Fourthly, a computer-readable storage medium is provided, characterized in that the computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of any of the methods described in the first aspect.
[0057] Fifthly, a computer program product containing instructions is provided, which, when run on a computer, causes the computer to execute any of the robot control methods described above.
[0058] Beneficial effects of the embodiments in this application:
[0059] This application provides a robot control method, device, electronic device, and storage medium. Through this application, after planning path points, the second pose corresponding to the next moment is determined using the information of the path points at the next moment. Then, by combining the current angular velocity, the first pose corresponding to the previous moment, and the second pose corresponding to the next moment, the initial state quantity corresponding to the initial moment of a preset time period can be determined. Finally, for each moment within the preset time period, a target angular velocity is calculated based on the initial state quantity. Furthermore, a control command is generated based on the target angular velocity and the velocity corresponding to that moment to control the robot's movement. That is, in this application, the control command is calculated separately after the path points are planned. Therefore, the planning of the path points does not affect the calculation of the control command, and the calculated control command can better ensure the smoothness and stability of the robot's movement.
[0060] Of course, implementing any product or method of this application does not necessarily require achieving all of the advantages described above at the same time. Attached Figure Description
[0061] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.
[0062] Figure 1 A flowchart illustrating a robot control method provided in this application embodiment;
[0063] Figure 2 A flowchart of a robot control method provided in another embodiment of this application;
[0064] Figure 3 This is a schematic diagram of the structure of a robot control device provided in an embodiment of this application;
[0065] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation
[0066] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0067] Because the DWA algorithm combines path points and control commands in its calculations, the planning of path points affects the calculation of control commands, thus compromising the smoothness and stability of robot motion. Therefore, this application provides a robot control method applicable to robots.
[0068] The robot control method provided in this application will be described in detail below with reference to specific implementation methods, such as... Figure 1 As shown, the specific steps are as follows:
[0069] S101, obtain the speed corresponding to each path point among multiple path points of the movement trajectory, where each path point corresponds to a time point.
[0070] In this embodiment of the application, multiple path points can be planned in advance using a path planning algorithm, and the speed corresponding to each path point can be calculated in advance, wherein each path point corresponds to a time.
[0071] In one embodiment of this application, the speed of each path point can be calculated using a PID algorithm. The specific calculation process is as follows: determine the position corresponding to the next path point based on the information of the next path point, calculate the longitudinal position error between the current position and the position corresponding to the next path point, substitute the longitudinal position error into the PID formula to obtain the speed corresponding to the first path point, and so on, to calculate the speed corresponding to each path point.
[0072] S102, obtain the robot's current angular velocity, first pose information, and second pose information at the next moment.
[0073] In this embodiment, the pose information includes position coordinates and heading angle. The current angular velocity and the first pose information corresponding to the current moment of the robot can be directly obtained, and the second pose information corresponding to the next moment can be determined based on the information of the next path point.
[0074] S103, based on the current angular velocity, the first pose and the second pose, determine the initial state quantity corresponding to the initial moment of the preset time period.
[0075] In this embodiment of the application, the initial time of the preset time period is the next time after the current time. The initial state quantity corresponding to the initial time of the preset time period can be determined based on the current angular velocity, the first pose, and the second pose.
[0076] S104, for each moment within a preset time period, calculate the target angular velocity based on the initial state quantity, and generate a control command based on the target angular velocity and the velocity corresponding to that moment. The control command is used to control the movement of the robot.
[0077] In this embodiment, each moment corresponds to a control command, which includes the velocity and target angular velocity at that moment, used to control the robot to move at the velocity and target angular velocity included in the control command. After obtaining the initial state variables, for each moment within a preset time period, the target angular velocity can be calculated based on the initial state variables. Then, the control command corresponding to that moment is generated based on the target angular velocity and the velocity at that moment.
[0078] In this embodiment, after the path points are planned, the second pose corresponding to the next moment is determined using the information of the path points at the next moment. Then, the initial state quantity corresponding to the initial moment of the preset time period can be determined by combining the current angular velocity, the first pose corresponding to the previous moment, and the second pose corresponding to the next moment. Finally, for each moment within the preset time period, the target angular velocity is calculated based on the initial state quantity. Then, control commands are generated based on the target angular velocity and the velocity corresponding to that moment to control the robot's movement. That is, in this application, the control commands are calculated separately after the path points are planned. Therefore, the planning of the path points does not affect the calculation of the control commands, so the calculated control commands can better ensure the smoothness and stability of the robot's movement.
[0079] In another embodiment of this application, the first pose includes: a first horizontal and vertical coordinate and a first heading angle; the second pose includes: a second horizontal and vertical coordinate and a second heading angle; step S103 may include the following steps:
[0080] Step 1: Calculate the lateral displacement error based on the first heading angle, the second heading angle, the first lateral coordinate, and the second lateral coordinate; calculate the heading angle error based on the first heading angle and the second heading angle.
[0081] In this embodiment, the first pose includes a first lateral and longitudinal coordinate and a first heading angle; the second pose includes a second lateral and longitudinal coordinate and a second heading angle. Lateral displacement error can be calculated based on the first heading angle, the second heading angle, the first lateral and longitudinal coordinates, and the second lateral and longitudinal coordinates; and heading angle error can be calculated based on the first heading angle and the second heading angle.
[0082] For example, if the first horizontal and vertical coordinates (x1, y1) are (0, 0), the first heading angle th is 0, the second horizontal and vertical coordinates (x2, y2) are (1, 1), and the second heading angle th is 0.3, then the lateral displacement error is 1 and the heading angle error is 0.3.
[0083] Step 2: Determine the initial state quantities based on the current angular velocity, the lateral displacement error, and the heading angle error.
[0084] In this embodiment of the application, the state variables include angular velocity, lateral displacement error, and heading angle error. The initial state variables corresponding to the initial moment of a preset time period can be determined based on the current angular velocity, lateral displacement error, and heading angle error. For example, if the lateral displacement error is 1, the heading angle error is 0.3, and the current angular velocity is 0, then the initial state variables corresponding to the initial moment are [e,th, ω] = [1,0.3,0].
[0085] In this embodiment, firstly, the lateral displacement error is calculated based on the first heading angle, the second heading angle, the first lateral coordinate, and the second lateral coordinate; then, the heading angle error is calculated based on the first heading angle and the second heading angle; and finally, the initial state quantity is determined based on the current angular velocity, the lateral displacement error, and the heading angle error. The calculation process is simple and efficient.
[0086] In another embodiment of this application, step S104 may include the following steps:
[0087] S201, Substitute the initial state quantities and multiple preset output angular velocities into a preset kinematic model to obtain the rate of change of multiple initial state quantities corresponding to the initial moment.
[0088] In this embodiment of the application, the output angular velocity ω d With constraints, any value can be taken within the constraints. For each value taken, it and the corresponding initial state quantity are substituted into the preset kinematic model to obtain a corresponding rate of change of the state quantity. Therefore, multiple rates of change of the initial state quantity corresponding to the initial time can be obtained.
[0089] In one embodiment of this application, the kinematic model is as shown in formula (1):
[0090]
[0091] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; ω is the angular velocity; v is the velocity; t is the control period; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0092] In another embodiment of this application, the kinematic model is as shown in formula (2):
[0093]
[0094] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; v is the velocity; ω dTo output angular velocity, ω r This is the feedforward angular velocity.
[0095] S202, using the initial state quantity and the rate of change of the multiple initial state quantities, multiple candidate state quantities are calculated.
[0096] In this embodiment of the application, the initial state quantity and the rate of change of multiple initial state quantities can be used to calculate the multiple candidate state quantities corresponding to the next time of the initial time. Then, the multiple candidate state quantities corresponding to the next time are substituted into formula (2) to calculate the rate of change of multiple state quantities corresponding to the next time. Then, the candidate state quantity corresponding to the next time of the next time is obtained by using the multiple candidate state quantities corresponding to the next time and the rate of change of multiple state quantities corresponding to the next time. By analogy, the multiple candidate state quantities corresponding to each time of the preset time period are calculated.
[0097] S203, Substitute the plurality of output angular velocities and the plurality of candidate state variables into the quadratic optimization formula to determine the target angular velocity from the plurality of output angular velocities.
[0098] In this application embodiment, the definition of the optimal solution index varies depending on the product: if accuracy is the priority, the lateral displacement error e of the state variables at each moment needs to be summed to ensure that the sum of e is minimized; if yaw prevention is required, the heading angle error th of the state variables at each moment needs to be summed to ensure that the sum is minimized; if output smoothness is required, the output angular velocity ω at each moment needs to be ensured to be equal to the previous moment. d The difference is minimized, and so on.
[0099] For each output angular velocity, the output angular velocity and its corresponding multiple candidate state variables are successively substituted into the quadratic optimization formula to obtain multiple optimization indices. The value of the optimization index that is the smallest is the optimal solution index, and the output angular velocity corresponding to the optimal solution index is determined as the target angular velocity.
[0100] The secondary optimization formula is shown in formula (3):
[0101] J = X T QX+(UU ref ) T R(UU ref (3)
[0102] Where X is the state variable; U is the output angular velocity; Uref is the expected value; J is the optimization index; Q is the state variable weight coefficient; and R is the output angular velocity weight coefficient. Q is a three-dimensional diagonal matrix, with the three values on the diagonal corresponding to the weights of each value in the state variable. R is a one-dimensional matrix representing the weights of the difference between the output and the feedforward (expectation).
[0103] In this embodiment, the initial state variables and multiple preset output angular velocities can be substituted into a preset kinematic model to obtain multiple initial state variable change rates corresponding to the initial moment; multiple candidate state variables are calculated using the initial state variables and the multiple initial state variable change rates; the multiple output angular velocities and the multiple candidate state variables are substituted into a quadratic optimization formula to determine the target angular velocity among the multiple output angular velocities. In this scheme, the angular velocity is calculated separately, and the calculation process does not involve path point planning and velocity. That is, the calculation process of the angular velocity can be optimized separately through this scheme, so the obtained target angular velocity can make the robot run more smoothly and stably.
[0104] In another embodiment of this application, step S202 may include the following steps:
[0105] Step 1: For each initial state variable change rate, multiply the initial state variable change rate by the control period to obtain the change amount of the initial state variable.
[0106] In this embodiment of the application, the control period is the period for sending control commands. For each initial state variable change rate, the change rate of the initial state variable is multiplied by the control period to obtain the change amount of the initial state variable.
[0107] Step 2: For each change in the initial state quantity, add the initial state quantity to the change in the initial state quantity to obtain the candidate state quantity.
[0108] In this embodiment, for each change in an initial state variable, the initial state variable is added to the change in that initial state variable to obtain a candidate state variable. This scheme provides a simple and efficient method for calculating candidate state variables.
[0109] In this embodiment, after the path points are planned, the second pose corresponding to the next moment is determined using the information of the path points at the next moment. Then, the initial state quantity corresponding to the initial moment of the preset time period can be determined by combining the current angular velocity, the first pose corresponding to the previous moment, and the second pose corresponding to the next moment. Finally, for each moment within the preset time period, the target angular velocity is calculated based on the initial state quantity. Then, control commands are generated based on the target angular velocity and the velocity corresponding to that moment to control the robot's movement. That is, in this application, the control commands are calculated separately after the path points are planned. Therefore, the planning of the path points does not affect the calculation of the control commands, so the calculated control commands can better ensure the smoothness and stability of the robot's movement.
[0110] Based on the same technical concept, embodiments of this application also provide a robot control device, such as... Figure 3 As shown, the device includes:
[0111] The first acquisition module 301 is used to acquire the speed corresponding to each path point among multiple path points of the movement trajectory, wherein each path point corresponds to a time moment;
[0112] The second acquisition module 302 is used to acquire the robot's current angular velocity at the current moment, the first pose information at the current moment, and the second pose information at the next moment.
[0113] The determining module 303 is used to determine the initial state quantity corresponding to the initial moment of a preset time period based on the current angular velocity, the first pose, and the second pose.
[0114] The calculation module 304 is used to calculate the target angular velocity based on the initial state quantity for each moment within a preset time period, and to generate a control command based on the target angular velocity and the velocity corresponding to that moment. The control command is used to control the movement of the robot.
[0115] The calculation module is used to calculate the target angular velocity based on the initial state quantity for each moment within a preset time period, and to generate control commands based on the target angular velocity and the velocity corresponding to that moment. The control commands are used to control the movement of the robot.
[0116] Optionally, the first pose includes: a first horizontal and vertical coordinate and a first heading angle; the second pose includes: a second horizontal and vertical coordinate and a second heading angle; the determining module includes:
[0117] The calculation unit is used to calculate the lateral displacement error based on the first heading angle, the second heading angle, the first lateral coordinate, and the second lateral coordinate, and to calculate the heading angle error based on the first heading angle and the second heading angle;
[0118] The first determining unit is used to determine the initial state quantity based on the current angular velocity, the lateral displacement error, and the heading angle error.
[0119] Optionally, the computing module includes:
[0120] The first obtaining unit is used to substitute the initial state quantity and a preset plurality of output angular velocities into a preset kinematic model to obtain the rate of change of the plurality of initial state quantities corresponding to the initial moment.
[0121] The second obtaining unit is used to calculate multiple candidate state quantities using the initial state quantities and the rate of change of the multiple initial state quantities;
[0122] The second determining unit is used to substitute the plurality of output angular velocities and the plurality of candidate state variables into the quadratic optimization formula to determine the target angular velocity among the plurality of output angular velocities.
[0123] Optionally, the kinematic model is as follows:
[0124]
[0125] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; ω is the angular velocity; v is the velocity; t is the control period; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0126] Optionally, the kinematic model is as follows:
[0127]
[0128] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; v is the velocity; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0129] Optionally, the quadratic optimization formula is as follows:
[0130] J = X T QX+(UU ref ) T R(UU ref )
[0131] Where X is the state variable; U is the output angular velocity; Uref is the expected value; J is the optimization index; Q is the state variable weighting coefficient; and R is the output angular velocity weighting coefficient.
[0132] Optionally, the second obtaining unit includes:
[0133] The first subunit is used to multiply the rate of change of the initial state quantity with the control period for each rate of change of the initial state quantity to obtain the change of the initial state quantity.
[0134] The second sub-unit is used to add the initial state quantity to the change of the initial state quantity for each initial state quantity to obtain the candidate state quantity.
[0135] In this embodiment, after the path points are planned, the second pose corresponding to the next moment is determined using the information of the path points at the next moment. Then, the initial state quantity corresponding to the initial moment of the preset time period can be determined by combining the current angular velocity, the first pose corresponding to the previous moment, and the second pose corresponding to the next moment. Finally, for each moment within the preset time period, the target angular velocity is calculated based on the initial state quantity. Then, control commands are generated based on the target angular velocity and the velocity corresponding to that moment to control the robot's movement. That is, in this application, the control commands are calculated separately after the path points are planned. Therefore, the planning of the path points does not affect the calculation of the control commands, so the calculated control commands can better ensure the smoothness and stability of the robot's movement.
[0136] Based on the same technical concept, embodiments of the present invention also provide an electronic device, such as... Figure 4 As shown, it includes a processor 401, a communication interface 402, a memory 403, and a communication bus 404, wherein the processor 401, the communication interface 402, and the memory 403 communicate with each other through the communication bus 404.
[0137] Memory 403 is used to store computer programs;
[0138] When processor 401 executes the program stored in memory 403, it performs the following steps:
[0139] Obtain the velocity corresponding to each path point in the multiple path points of the movement trajectory, where each path point corresponds to a time point;
[0140] Obtain the robot's current angular velocity, first pose information at the current moment, and second pose information at the next moment;
[0141] Based on the current angular velocity, the first pose, and the second pose, determine the initial state quantity corresponding to the initial moment of the preset time period;
[0142] For each moment within a preset time period, a target angular velocity is calculated based on the initial state quantity. A control command is generated based on the target angular velocity and the velocity corresponding to that moment. The control command is used to control the robot's movement.
[0143] Optionally, the first pose includes: a first horizontal and vertical coordinate and a first heading angle; the second pose includes: a second horizontal and vertical coordinate and a second heading angle; the step of determining the initial state quantity corresponding to the initial moment of a preset time period based on the current angular velocity, the first pose, and the second pose includes:
[0144] The lateral displacement error is calculated based on the first heading angle, the second heading angle, the first horizontal and vertical coordinates, and the second horizontal and vertical coordinates; the heading angle error is calculated based on the first heading angle and the second heading angle.
[0145] The initial state quantities are determined based on the current angular velocity, the lateral displacement error, and the heading angle error.
[0146] Optionally, calculating the target angular velocity based on the initial state quantity includes:
[0147] Substituting the initial state quantities and multiple preset output angular velocities into a preset kinematic model, the rate of change of multiple initial state quantities corresponding to the initial moment is obtained;
[0148] Multiple candidate state quantities are calculated using the initial state quantities and the rate of change of the multiple initial state quantities;
[0149] Substitute the multiple output angular velocities and the multiple candidate state variables into the quadratic optimization formula to determine the target angular velocity from the multiple output angular velocities.
[0150] Optionally, the kinematic model is as follows:
[0151]
[0152] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; ω is the angular velocity; v is the velocity; t is the control period; ω d To output angular velocity, ω r This is the feedforward angular velocity.
[0153] Optionally, the kinematic model is as follows:
[0154]
[0155] in, For state variables; ω is the rate of change of state variables; e is the lateral displacement error; th is the heading angle error; v is the velocity; ω d To output angular velocity, ω r Let be the feedforward angular velocity. Optionally, the quadratic optimization formula is as follows:
[0156] J = X T QX+(UU ref ) T R(UU ref )
[0157] Where X is the state variable; U is the output angular velocity; Uref is the expected value; J is the optimization index; Q is the state variable weighting coefficient; and R is the output angular velocity weighting coefficient.
[0158] Optionally, the step of calculating multiple candidate state quantities using the initial state quantity and the rate of change of the multiple initial state quantities includes:
[0159] For each initial state variable change rate, the initial state variable change rate is multiplied by the control period to obtain the change amount of the initial state variable;
[0160] For each change in an initial state quantity, the initial state quantity is added to the change in the initial state quantity to obtain the candidate state quantity.
[0161] The communication bus mentioned in the above electronic devices can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into address bus, data bus, control bus, etc. For ease of illustration, only one thick line is used to represent it in the diagram, but this does not mean that there is only one bus or one type of bus.
[0162] The communication interface is used for communication between the aforementioned electronic devices and other devices.
[0163] The memory may include random access memory (RAM) or non-volatile memory (NVM), such as at least one disk storage device. Optionally, the memory may also be at least one storage device located remotely from the aforementioned processor.
[0164] The processors mentioned above can be general-purpose processors, including central processing units (CPUs), network processors (NPs), etc.; they can also be digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.
[0165] In another embodiment of the present invention, a computer-readable storage medium is also provided, which stores a computer program that, when executed by a processor, implements the steps of any of the robot control methods described above.
[0166] In another embodiment of the present invention, a computer program product containing instructions is also provided, which, when run on a computer, causes the computer to execute any of the robot control methods described above.
[0167] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of the present invention are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid state disk (SSD)).
[0168] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.
[0169] The above description is merely a specific embodiment of this application, enabling those skilled in the art to understand or implement this application. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of this application. Therefore, this application is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features claimed herein.
Claims
1. A robot control method characterized by, The method comprises: obtaining a speed corresponding to each path point in a plurality of path points of a moving track, wherein each path point corresponds to a time point; obtaining a current angular velocity corresponding to a current time point, first pose information corresponding to the current time point, and second pose information corresponding to a next time point; determining an initial state quantity corresponding to an initial time point in a preset time period based on the current angular velocity, the first pose, and the second pose; for each time point in the preset time period, calculating a target angular velocity based on the initial state quantity, and generating a control instruction based on the target angular velocity and a speed corresponding to the time point, the control instruction being used to control movement of the robot.
2. The method of claim 1, wherein, The first pose comprises a first horizontal and vertical coordinate and a first heading angle; the second pose comprises a second horizontal and vertical coordinate and a second heading angle; the determining of the initial state quantity corresponding to the initial time point in the preset time period based on the current angular velocity, the first pose, and the second pose comprises: calculating a horizontal displacement error based on the first heading angle, the second heading angle, the first horizontal and vertical coordinate, and the second horizontal and vertical coordinate, and calculating a heading angle error based on the first heading angle and the second heading angle; determining the initial state quantity based on the current angular velocity, the horizontal displacement error, and the heading angle error.
3. The method of claim 1, wherein, The calculation of the target angular velocity based on the initial state quantity comprises: substituting the initial state quantity and a preset plurality of output angular velocities into a preset kinematic model to obtain a plurality of initial state quantity change rates corresponding to the initial time point; calculating a plurality of candidate state quantities by using the initial state quantity and the plurality of initial state quantity change rates; substituting the plurality of output angular velocities and the plurality of candidate state quantities into a quadratic optimization formula to determine a target angular velocity from the plurality of output angular velocities.
4. The method of claim 3, wherein, The kinematic model is as follows: wherein, is a state quantity; is a state quantity change rate; e is a lateral displacement error; th is a heading angle error; ω is an angular velocity; v is a speed; t is a control period; ω d is an output angular velocity; ω r is a feedforward angular velocity.
5. The method of claim 3, wherein, The kinematic model is as follows: wherein, is a state quantity; is a state quantity change rate; e is a lateral displacement error; th is a heading angle error; v is a speed; ω d is an output angular velocity; ω r is a feedforward angular velocity.
6. The method of claim 3, wherein, The quadratic optimization formula is as follows: J = X T QX + (U - U ref ) T R(U - U ref ) wherein X is a state quantity; U is an output angular velocity; Uref is an expected value; J is an optimization index; Q is a state quantity weight coefficient; and R is an output angular velocity weight coefficient.
7. The method of claim 3, wherein, The calculation of the plurality of candidate state quantities by using the initial state quantity and the plurality of initial state quantity change rates comprises: for each initial state quantity change rate, multiplying the initial state quantity change rate by a control period to obtain a change quantity of the initial state quantity; for each change quantity of the initial state quantity, adding the initial state quantity and the change quantity of the initial state quantity to obtain the candidate state quantity.
8. A robot control device characterized by comprising: The device comprises: a first obtaining module configured to obtain a speed corresponding to each path point in a plurality of path points of a moving track, wherein each path point corresponds to a time point; a second obtaining module configured to obtain a current angular velocity corresponding to a current time point, first pose information corresponding to the current time point, and second pose information corresponding to a next time point; a determining module configured to determine an initial state quantity corresponding to an initial time point in a preset time period based on the current angular velocity, the first pose, and the second pose; The computing module is configured to calculate a target angular velocity based on the initial state quantity for each time point in a preset time period, and generate a control instruction based on the target angular velocity and a speed corresponding to the time point, the control instruction being used to control the robot motion.
9. An electronic device, comprising: The device comprises a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other through the communication bus. The memory is configured to store a computer program. The processor is configured to execute the program stored in the memory to implement the method steps in any one of claims 1-7.
10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the method steps in any one of claims 1-7.
Citation Information
Patent Citations
Welding robot control method and device, welding robot and storage medium
CN111376271A