CN116701821A - Robot speed calculation method and device, electronic equipment and storage medium - Google Patents

Robot speed calculation method and device, electronic equipment and storage medium Download PDF

Info

Publication number
CN116701821A
CN116701821A CN202310972254.9A CN202310972254A CN116701821A CN 116701821 A CN116701821 A CN 116701821A CN 202310972254 A CN202310972254 A CN 202310972254A CN 116701821 A CN116701821 A CN 116701821A
Authority
CN
China
Prior art keywords
robot
speed
measured
nodes
node
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202310972254.9A
Other languages
Chinese (zh)
Other versions
CN116701821B (en
Inventor
许金鹏
黃桢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ji Hua Laboratory
Original Assignee
Ji Hua Laboratory
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ji Hua Laboratory filed Critical Ji Hua Laboratory
Priority to CN202310972254.9A priority Critical patent/CN116701821B/en
Publication of CN116701821A publication Critical patent/CN116701821A/en
Application granted granted Critical
Publication of CN116701821B publication Critical patent/CN116701821B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q10/00—Administration; Management
    • G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
    • G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Business, Economics & Management (AREA)
  • General Physics & Mathematics (AREA)
  • Human Resources & Organizations (AREA)
  • Theoretical Computer Science (AREA)
  • Strategic Management (AREA)
  • Economics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Development Economics (AREA)
  • Computational Mathematics (AREA)
  • Game Theory and Decision Science (AREA)
  • Software Systems (AREA)
  • Entrepreneurship & Innovation (AREA)
  • Marketing (AREA)
  • Operations Research (AREA)
  • Quality & Reliability (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Numerical Control (AREA)

Abstract

The application belongs to the technical field of robot speed calculation, and discloses a robot speed calculation method, a device, electronic equipment and a storage medium, wherein the method comprises the following steps: the method comprises the steps of obtaining speed information of a robot to be detected and start and end information of a given path, dividing the given path into node paths of a plurality of nodes, obtaining Cartesian coordinates and orientation angles of the robot to be detected in the plurality of nodes, determining a relation among curvature, speed and angular speed of the robot to be detected, establishing speed constraint conditions based on the speed information, the relation, the Cartesian coordinates and the orientation angles, calculating to obtain optimal speeds of the plurality of nodes of the robot to be detected through a gradient-based optimization algorithm and combining the speed constraint conditions, and calculating the speed of the robot based on the curvature of the path and the speed constraint conditions of the robot, so that the calculation efficiency of the speed of the robot is improved.

Description

Robot speed calculation method and device, electronic equipment and storage medium
Technical Field
The present application relates to the technical field of calculating a robot speed, and in particular, to a method and apparatus for calculating a robot speed, an electronic device, and a storage medium.
Background
The mobile robot trajectory planning can be divided into two types, one is to describe the trajectory by using an equation, the trajectory is directly generated in the planning, and the other is to decouple the path and the speed, and the path planning is performed first and then the speed planning is performed. Common track generation algorithms mainly use piecewise polynomials, bazier, bspline curves and the like to represent tracks, then perform time allocation according to experience, and jointly optimize paths and speeds. The algorithm of path and speed decoupling is adopted to reduce the dimension of the planning problem, the solving difficulty of the optimization problem is reduced, and the method can better approach to the global optimal solution in an iterative solving mode, so that the method is the most widely applied mode in the robot. The common path planning algorithms comprise dijkstra, astar, jps, rrt, prm and the like, the paths output by the algorithms are a series of discrete path points, the path smoothness is not high, the condition that curvature mutation easily occurs due to unstable path quality is caused, if PID, purePsuit, MPC and the like are used for directly tracking, the angular velocity is out of limit and cannot be accurately tracked due to the fact that deceleration cannot be carried out in advance at the curvature mutation, and the path is deviated from the tracking path, so that it is necessary to carry out speed planning according to the related information of the path before tracking, and carry out speed tracking while tracking the path
Common speed planning algorithms utilize Double S, trapezoidal constant velocity curves to calculate speed, acceleration at discrete path points. Although such an algorithm can obtain a smoother speed, the problem of deviation from the tracking path when curvature is suddenly changed cannot be solved due to the lack of a global field of view.
Therefore, in order to solve the technical problem that the calculated speed deviates from the given path due to the abrupt change of curvature in the existing speed planning algorithm, a method, a device, an electronic device and a storage medium for calculating the speed of the robot are needed.
Disclosure of Invention
The application aims to provide a robot speed calculation method, a device, electronic equipment and a storage medium, which are used for calculating the speed of a robot based on the curvature of the robot to a path and the speed constraint condition of the robot, so that the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, the generated optimal speed of a node can meet the kinematic constraint requirement and the path curvature requirement of the robot, the requirement of sufficient smoothness can be met, and the calculation efficiency of the speed of the robot is improved.
In a first aspect, the present application provides a method for calculating a speed of a robot, for planning the speed of the robot, including the steps of:
Acquiring speed information of a robot to be tested and start and end information of a given path;
dividing the given path into node paths of a plurality of nodes according to the start-end information;
acquiring Cartesian coordinates and orientation angles of the robot to be tested in the plurality of nodes, and determining a relation among curvature, speed and angular speed of the robot to be tested;
establishing a speed constraint condition based on the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
and calculating the optimal speeds of a plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
The robot speed calculation method provided by the application can realize the planning of the speed of the robot, calculates the speed of the robot based on the curvature of the robot to the path and the speed constraint condition of the robot, solves the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm, ensures that the generated optimal speed of the node can not only meet the kinematic constraint requirement and the path curvature requirement of the robot, but also meet the requirement of sufficient smoothness, and improves the calculation efficiency of the speed of the robot.
Optionally, the speed information includes a start speed, a start acceleration, an end speed, and an end acceleration of the robot under test at the given path, and a maximum speed, a maximum angular speed, and a maximum acceleration of the robot under test;
the start-end information includes a start position and an end position of the given path.
Optionally, acquiring cartesian coordinates and orientation angles of the robot to be measured in the plurality of nodes, and determining a relational expression among curvature, speed and angular speed of the robot to be measured, including:
acquiring Cartesian coordinates and orientation angles of the robot to be tested in the plurality of nodes;
calculating to obtain the curvature of the robot to be measured at the plurality of nodes based on the Cartesian coordinates and the orientation angle;
and determining a relation among the curvature, the speed and the angular speed of the robot to be tested according to preset uniform acceleration linear motion conditions by combining the Cartesian coordinates, the orientation angle and the curvature.
Optionally, the speed constraint includes an actual movement speed constraint and an angular speed constraint;
the actual movement speed constraint condition is specifically as follows:
；
the angular velocity constraint condition is to convert the angular velocity constraint into a velocity constraint, and specifically comprises the following steps:
；
wherein ,speed of the robot under test for the ith node,/->For the maximum speed of the robot to be tested, +.>For the maximum angular velocity in the opposite direction of the robot to be measured, < > for>For the positive maximum angular velocity of the robot to be measured, < >>The angular velocity of the robot to be measured for the i-th node, < >>Acceleration of the robot under test for the ith node, +.>For the maximum acceleration of the robot to be measured, < >>The curvature of the robot under test for the i-th node,for the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>And the direction angle of the robot to be tested is the i-th node.
The robot speed calculation method provided by the application can realize the planning of the speed of the robot, and by setting the constraint condition of the actual movement speed and the constraint condition of the angular speed, the constraint condition which is more in line with the actual problem is set in consideration of the condition of curvature mutation, so that the calculation accuracy of the speed of the robot is improved.
Optionally, a gradient-based optimization algorithm, in combination with the speed constraint condition, the speed information, the relation, the cartesian coordinates and the orientation angle, calculates an optimal speed of a plurality of nodes of the robot to be measured, including:
Constructing a speed cost function of the robot to be tested based on the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
and calculating the optimal speeds of a plurality of nodes of the robot to be tested according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle by the gradient-based optimization algorithm and combining the speed cost function.
The method for calculating the speed of the robot can plan the speed of the robot, iterates the acceleration soft constraint item in the speed cost function through the optimization algorithm based on the gradient, so that the speed cost function is iterated, the optimal speed of the robot to be measured at each node is calculated, and the calculated speed of the robot is more in accordance with the actual requirement through the iteration of the speed cost function, so that the accuracy of the speed of the robot is improved.
Optionally, after calculating the optimal speeds of the plurality of nodes of the robot to be measured according to the speed constraint condition, the speed information, the relational expression, the cartesian coordinates and the orientation angle by the gradient-based optimization algorithm and in combination with the speed cost function, the method further includes:
And adjusting the weight coefficient of the speed cost function, and calculating the optimal speeds of a plurality of nodes of the robot to be tested under the weight coefficients of different proportions.
Optionally, the speed cost function is specifically:
；
wherein cost is a speed cost function, which represents the optimal speed of the robot to be tested at the ith node,speed of the robot under test for the ith node,/->For the maximum speed of the robot to be tested, +.>Acceleration of the robot under test for the ith node, +.>For the maximum acceleration of the robot to be measured, < >>For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>For speed smoothing item +.>Weight coefficient for velocity smoothing term, +.>For acceleration soft constraint term, ++>The weight coefficient of the acceleration soft constraint term can be calculated by any gradient-based optimization algorithm, wherein the weight coefficient is the weight coefficient of the acceleration soft constraint term>For acceleration smoothing term, ++>Weight coefficient for acceleration smoothing term, +.>For the shortest time item->For the weight coefficient of the shortest term, n is the total number of divided nodes-1, i.e. n+1 is the total number of divided nodes, and i and n are integers.
In a second aspect, the present application provides a robot speed calculation device for planning a speed of a robot, including:
The acquisition module is used for acquiring speed information of the robot to be detected and start and stop information of a given path;
the dividing module is used for dividing the given path into node paths of a plurality of nodes according to the start and end information;
the determining module is used for acquiring Cartesian coordinates and orientation angles of the robot to be detected in the plurality of nodes and determining a relational expression among the curvature, the speed and the angular speed of the robot to be detected;
the establishing module is used for establishing a speed constraint condition based on the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
and the calculation module is used for calculating the optimal speeds of a plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
According to the robot speed calculating device, the speed of the robot is calculated based on the curvature of the robot to the path and the speed constraint condition of the robot, the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, so that the generated optimal speed of the node can meet the kinematic constraint requirement and the path curvature requirement of the robot, the requirement of sufficient smoothness can be met, and the calculating efficiency of the speed of the robot is improved.
In a third aspect, the application provides an electronic device comprising a processor and a memory, the memory storing a computer program executable by the processor, when executing the computer program, running steps in a robot speed calculation method as described hereinbefore.
In a fourth aspect, the application provides a computer readable storage medium having stored thereon a computer program which when executed by a processor performs steps in a robot speed calculation method as described hereinbefore.
The beneficial effects are that:
according to the robot speed calculation method, the device, the electronic equipment and the storage medium, the speed of the robot is calculated based on the curvature of the robot on the path and the speed constraint condition of the robot, so that the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, the generated optimal speed of the node can meet the kinematic constraint requirement and the path curvature requirement of the robot, the requirement of sufficient smoothness can be met, and the calculation efficiency of the speed of the robot is improved.
Drawings
Fig. 1 is a flowchart of a robot speed calculation method according to an embodiment of the present application.
Fig. 2 is a schematic structural diagram of a robot speed calculating device according to an embodiment of the present application.
Fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application.
Fig. 4 is a schematic diagram of a partitioned node path.
Fig. 5 is a schematic diagram of a speed range of a robot under test at a plurality of nodes.
Fig. 6 is a schematic diagram of optimal speeds of a plurality of nodes of a robot under test.
Description of the reference numerals: 1. an acquisition module; 2. dividing the module; 3. a determining module; 4. establishing a module; 5. a computing module; 301. a processor; 302. a memory; 303. a communication bus.
Detailed Description
The following description of the embodiments of the present application will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present application, but not all embodiments. The components of the embodiments of the present application generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the application, as presented in the figures, is not intended to limit the scope of the application, as claimed, but is merely representative of selected embodiments of the application. All other embodiments, which can be made by a person skilled in the art without making any inventive effort, are intended to be within the scope of the present application.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only to distinguish the description, and are not to be construed as indicating or implying relative importance.
Referring to fig. 1, fig. 1 is a method for calculating a speed of a robot according to some embodiments of the present application, for planning the speed of the robot, including the steps of:
step S101, acquiring speed information of a robot to be detected and start and stop information of a given path;
step S102, dividing a given path into node paths of a plurality of nodes according to start and end information;
step S103, acquiring Cartesian coordinates and orientation angles of the robot to be tested in a plurality of nodes, and determining a relational expression among the curvature, the speed and the angular speed of the robot to be tested;
step S104, establishing a speed constraint condition based on the speed information, the relation, the Cartesian coordinates and the orientation angle;
step S105, calculating to obtain the optimal speeds of a plurality of nodes of the robot to be tested by combining the speed constraint condition, the speed information, the relation, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
According to the robot speed calculation method, the speed of the robot is calculated based on the curvature of the robot on the path and the speed constraint condition of the robot, the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, so that the generated optimal speed of the node can meet the kinematic constraint requirement and the path curvature requirement of the robot, the requirement of sufficient smoothness can be met, and the calculation efficiency of the speed of the robot is improved.
Specifically, in step S101, speed information of the robot to be measured and start and end information of the given path are obtained, where the speed information of the robot to be measured includes a start speed, a start acceleration, an end speed and an end acceleration of the robot to be measured in the given path, and a maximum speed, a maximum angular speed and a maximum acceleration of the robot to be measured; the start-end information of the given path includes a start position and an end position of the given path.
Specifically, in step S102, the given path is divided into node paths of a plurality of nodes according to the start-end information, i.e., the given path is divided into node paths of a plurality of nodes based on the plurality of nodes according to the start position and the end position of the given path, e.g., the given path The path is divided into node paths of 80 nodes, and then nodes exist, wherein ,for the position of the j+1th node, +.>Is the starting position->For the end position, there are 79 node paths and 80 nodes.
In the present application, the robot to be measured is a differential robot, and in order to increase the number of path speed plans (robot speeds) for the differential robot, the direction angle of the end point is not constrained, and only the position is constrained, so that a given path is divided into node paths of a plurality of nodes.
Specifically, in step S103, cartesian coordinates and orientation angles of the robot to be measured in the plurality of nodes are obtained, and a relational expression between the curvature, the speed, and the angular speed of the robot to be measured is determined, including:
acquiring Cartesian coordinates and orientation angles of a robot to be tested in a plurality of nodes;
calculating to obtain the curvature of the robot to be measured at a plurality of nodes based on the Cartesian coordinates and the orientation angles;
and determining a relation among the curvature, the speed and the angular speed of the robot to be tested according to preset uniform acceleration linear motion conditions and by combining Cartesian coordinates, an orientation angle and curvature.
In step S103, the cartesian coordinates and the orientation angle of the robot to be measured in the plurality of nodes are acquired, where the cartesian coordinates are (x i ，y i）, wherein ,xi For the i-th node (i.e. node number at path divisionA node of yes, i=j+1, and the node is not the last node) the robot to be testedIs y i For the ordinate, the orientation angle of the robot to be measured in the ith node +.>Indicating (I)>As shown in fig. 4, fig. 4 is a schematic diagram of a divided node path, where x is the direction angle of the robot to be measured in the i-th node i Is the abscissa of the robot to be measured in the ith node,/>Is the direction angle of the robot to be tested in the ith node,>the motion time of the robot to be measured from the ith node to the (i+1) th node is obtained.
Based on Cartesian coordinates and orientation angles, the curvature of the robot to be measured at a plurality of nodes is calculated, and a calculation formula of the curvature is specifically as follows:
；
wherein ,ki Is the curvature of the robot to be measured in the ith node, L i For the path distance from the ith node to the (i+1) th node of the robot to be tested,the direction angle of the robot to be measured in the ith node.
According to preset uniform acceleration rectilinear motion conditions and by combining Cartesian coordinates, orientation angles and curvatures, determining a relational expression among the curvatures, the speeds and the angular speeds of the robot to be tested, namely assuming that the robot to be tested performs uniform acceleration rectilinear motion among adjacent nodes, when the robot to be tested performs uniform acceleration rectilinear motion among the adjacent nodes, the following relational expression exists:
；
；
；
wherein ,ki Is the curvature of the robot to be measured in the ith node, L i For the path distance, x, from the ith node to the (i+1) th node of the robot to be tested i Is the abscissa, y, of the robot to be tested in the ith node i Is the ordinate of the robot to be measured in the i-th node,acceleration of the robot to be measured in the ith node,/->Is the linear velocity of the robot to be measured in the ith node (i.e. the velocity of the robot to be measured in the ith node), of>Is the direction angle of the robot to be tested in the ith node,>the angular velocity of the robot to be measured for the i-th node. I is more than or equal to 1 and less than or equal to n, wherein n is the total number of divided nodes-1. Since the robot to be measured needs to stop at the end position (last node), there is no need to calculate the curvature of the robot to be measured at the end position, and since the robot to be measured is in a stopped state at both the start position (first node) and the end position, the acceleration and the speed of the robot to be measured at both the start position and the end position are 0.
In the differential robot, when the euclidean distance between adjacent route points is small, there is the following relation:
；
；
；
wherein ,xi Is the abscissa, y, of the robot to be tested in the ith node i Is the ordinate of the robot to be measured in the i-th node, Acceleration of the robot to be measured in the ith node,/->Is the linear velocity of the robot to be measured in the ith node (i.e. the velocity of the robot to be measured in the ith node), of>Is the direction angle of the robot to be tested in the ith node,>for the movement time of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>The angular velocity of the robot to be measured for the i-th node.
Therefore, the speeds of the robot to be measured at the start position and the end position satisfy the following constraints:
；
；
；
；
wherein ,for the starting speed of the robot to be measured, +.>For the starting acceleration of the robot to be measured, +.>For the end speed of the robot to be measured, +.>The terminal acceleration of the robot to be measured.
And determining and obtaining a relational expression among the curvature, the speed and the angular speed of the robot to be measured according to the relational expression.
Specifically, in step S104, a speed constraint is established based on the speed information, the relational expression, the cartesian coordinates, and the orientation angle, the speed constraint including an actual movement speed constraint and an angular speed constraint.
The constraint condition of the actual movement speed of the robot to be tested is that the robot to be tested can only move within the range of maximum speed, maximum angular speed and maximum acceleration, namely the constraint condition of the actual movement speed of the robot to be tested is specifically as follows:
；
wherein ,speed of the robot to be measured for the i-th node,/->For the maximum speed of the robot to be tested,for the maximum angular velocity in the opposite direction of the robot to be measured,/-for>For the positive maximum angular velocity of the robot to be measured,/->Angular velocity of the robot to be measured for the ith node,/->Acceleration of the robot to be measured for the ith node,/->The maximum acceleration of the robot to be measured.
In order to meet the angular velocity constraint condition of the robot to be tested, according to the relation among the curvature, the velocity and the angular velocity of the robot to be tested, as the path distance among a plurality of nodes is known and the curvature of each node is also known, the angular velocity constraint is converted into the velocity constraint, and the angular velocity constraint condition is that the angular velocity constraint is converted into the velocity constraint, specifically:
；
wherein ,speed of the robot to be measured for the i-th node,/->For the positive maximum angular velocity of the robot to be measured,/->Curvature of robot to be measured for ith node, L i For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>The direction angle of the robot to be tested is the i-th node.
Specifically, in step S105, by combining a gradient-based optimization algorithm with a speed constraint condition, speed information, a relationship, a cartesian coordinate, and an orientation angle, an optimal speed of a plurality of nodes of the robot to be measured is calculated, including:
Constructing a speed cost function of the robot to be tested based on the speed constraint condition, the speed information, the relation, the Cartesian coordinates and the orientation angle;
and calculating the optimal speeds of a plurality of nodes of the robot to be measured according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle and by combining the speed cost function through a gradient-based optimization algorithm.
In step S105, a speed cost function of the robot to be measured is constructed based on the speed constraint condition, the speed information, the relational expression, the cartesian coordinates and the orientation angle, and the speed cost function is specifically:
；
wherein cost is a speed cost function, which represents the optimal speed of the robot to be tested at the ith node,speed of the robot to be measured for the i-th node,/->Maximum speed of robot to be tested, +.>Acceleration of the robot to be measured for the ith node,/->For maximum acceleration of the robot to be measured, +.>For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>For speed smoothing item +.>As the weight coefficient of the velocity smoothing term,for acceleration soft constraint term, ++>The weight coefficient of the acceleration soft constraint term can be calculated by any gradient-based optimization algorithm, wherein the weight coefficient is the weight coefficient of the acceleration soft constraint term >For acceleration smoothing term, ++>Weight coefficient for acceleration smoothing term, +.>For the shortest time item->For the weight coefficient of the shortest term, n is the total number of divided nodes-1, i.e. n+1 is the total number of divided nodes, and i and n are integers. The weight coefficient of the velocity smoothing term, the weight coefficient of the acceleration soft constraint term, the weight coefficient of the acceleration smoothing term and the weight coefficient of the shortest term are set to be 0.1, 1.2, 5.0 and 4.0 respectively, and can be adjusted according to actual needs, but the method is not limited to the above.
The aim of the application is to find a set of speed variables such that the linear speed (speed) and the angular speed of the robot to be measured at each node meet the practical constraints and reduce the navigation time as much as possible. Therefore, the velocity optimization of the known path solves for a significant non-convex problem, designing the acceleration constraint as a soft constraint.
It should be noted that the number of the substrates,for velocity smoothing terms, the velocity smoothing term represents the velocity for three pointsThe difference in velocity between the two points increases the square penalty and constrains the rate of change of the velocity of the front and rear points.
And the acceleration soft constraint term represents that the acceleration hard constraint is converted into punishment for the acceleration overrun by using a punishment function method, so that the overrun variable is constrained to be within a limit range as much as possible.
The acceleration smoothing term represents a penalty added to the acceleration difference between two points.
The shortest time term means that the acceleration at each point is converged to the maximum speed as much as possible, and the navigation time is reduced.
And calculating to obtain the optimal speeds of a plurality of nodes of the robot to be tested according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle and by combining the speed cost function through a gradient-based optimization algorithm, namely, carrying out iterative solution on the speed cost function through the gradient-based optimization algorithm, and calculating to obtain the optimal speeds of the robot to be tested at all the nodes.
For example, assume that the maximum linear velocity of the robot to be measured is 1.0m/s, the maximum angular velocity is 0.5m/s, and the maximum acceleration is 0.5m/s 2 Dividing a given path into node paths of 80 nodes, acquiring Cartesian coordinates and orientation angles of a robot to be tested in the nodes, knowing that the starting point speed, the starting point acceleration, the end point speed and the end point acceleration of the robot to be tested are all 0, and calculating to obtain the speed range of each node of the robot to be tested by combining the actual motion speed constraint condition and the angular speed constraint condition, wherein as shown in FIG. 5, FIG. 5 is a schematic diagram of the speed range of the robot to be tested in a plurality of nodes, wherein the abscissa is the node path, the ordinate is the speed of the robot, the unit is m/s, and the shaded part in the diagram is the speed range of the robot to be tested in a plurality of nodes The speed range of the points, namely the programmable movement speed of each node of the robot to be tested, is within the limit range of the actual movement speed constraint condition and the angular speed constraint condition.
Setting the weight coefficient of the speed smoothing term, the weight coefficient of the acceleration soft constraint term, the weight coefficient of the acceleration smoothing term and the weight coefficient of the shortest time term to be 0.1, 1.2, 5.0 and 4.0 respectively, and iterating the acceleration soft constraint term in the speed cost function by adopting a gradient-based optimization algorithm when calculating the optimal speed of each node, namelyThe iteration is carried out for 1000 times, the optimal acceleration soft constraint item of each node is determined, so that the optimal speed of the robot to be tested at each node is obtained through calculation, as shown in fig. 6, fig. 6 is a schematic diagram of the optimal speeds of the robot to be tested at a plurality of nodes, wherein the abscissa is a node path, the ordinate is the speed of the robot, the unit is m/s, black lines in a shadow part in the diagram are the optimal speeds of the robot to be tested at each node, a calculated speed planning result (the optimal speed of the robot to be tested at each node) can be seen from the diagram, meanwhile, the speed planning is smooth and short in time consumption, and the given path of the robot can be planned according to the calculated optimal speeds of the robot to be tested at a plurality of nodes. The weight coefficient and the iteration number can be adjusted according to actual needs, but are not limited thereto.
Specifically, in step S105, after calculating the optimal speeds of the plurality of nodes of the robot to be measured according to the speed constraint condition, the speed information, the relation, the cartesian coordinates and the orientation angle and in combination with the speed cost function by using the gradient-based optimization algorithm, the method further includes:
and adjusting the weight coefficient of the speed cost function, and calculating the optimal speeds of a plurality of nodes of the robot to be tested under the weight coefficients of different proportions.
In step S105, the weight coefficients of different terms in the speed cost function are adjusted, the speed cost function under different weight coefficients is iterated, the optimal speeds of a plurality of nodes of the robot to be tested under different weight coefficients can be obtained by calculation, the specific gravity of each weight coefficient can be adjusted according to actual needs, and a corresponding speed optimization result (the optimal speeds of a plurality of nodes of the robot to be tested) is obtained. Different weight coefficients can calculate different speed optimization results to obtain different path speed plans, if the path speed plan with optimal time is wanted, the corresponding weight coefficients are required to be adjusted, the corresponding speed optimization results are obtained by calculation, and the speed optimization result with the shortest time consumption (optimal time) is selected to be used as the target (optimal time) path speed plan. The specific gravity of each weight coefficient can be adjusted according to actual needs, different speed optimization results (the optimal speeds of a plurality of nodes of the robot to be tested) are obtained, and different path speed plans are obtained.
According to the robot speed calculation method, the speed information of the robot to be measured and the start and end information of a given path are obtained, the given path is divided into node paths of a plurality of nodes according to the start and end information, cartesian coordinates and orientation angles of the robot to be measured in the nodes are obtained, a relational expression among the curvature, the speed and the angular speed of the robot to be measured is determined, speed constraint conditions are established based on the speed information, the relational expression and the Cartesian coordinates and the orientation angles, and the optimal speeds of the nodes of the robot to be measured are calculated by combining the speed constraint conditions, the speed information, the relational expression and the Cartesian coordinates and the orientation angles through a gradient-based optimization algorithm; therefore, the speed of the robot is calculated based on the curvature of the robot to the path and the speed constraint condition of the robot, the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, so that the generated optimal speed of the node can not only meet the kinematic constraint requirement of the robot and the path curvature requirement, but also reach the requirement of sufficient smoothness, and the calculation efficiency of the speed of the robot is improved.
Referring to fig. 2, the present application provides a robot speed calculation apparatus for planning a speed of a robot, including:
the acquisition module 1 is used for acquiring speed information of the robot to be detected and start and end information of a given path;
the dividing module 2 is used for dividing the given path into node paths of a plurality of nodes according to the start and end information;
the determining module 3 is used for acquiring Cartesian coordinates and orientation angles of the robot to be detected in the plurality of nodes and determining a relational expression among the curvature, the speed and the angular speed of the robot to be detected;
a building module 4, configured to build a speed constraint condition based on the speed information, the relation, the cartesian coordinates and the orientation angle;
and the calculating module 5 is used for calculating the optimal speeds of a plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relation, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
According to the robot speed calculating device, the speed of the robot is calculated based on the curvature of the robot to the path and the speed constraint condition of the robot, the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, so that the generated optimal speed of the node can meet the kinematic constraint requirement and the path curvature requirement of the robot, the requirement of sufficient smoothness can be met, and the calculating efficiency of the speed of the robot is improved.
Specifically, when the acquisition module 1 executes, acquiring speed information of a robot to be detected and start and end information of a given path, wherein the speed information of the robot to be detected comprises a start speed, a start acceleration, an end speed and an end acceleration of the robot to be detected in the given path, and a maximum speed, a maximum angular speed and a maximum acceleration of the robot to be detected; the start-end information of the given path includes a start position and an end position of the given path.
Specifically, the dividing module 2 divides the given path into node paths of a plurality of nodes according to the start and end information, that is, divides the given path into node paths of a plurality of nodes based on the plurality of nodes according to the start position and the end position of the given path, and if the given path is divided into node paths of 80 nodes, there are nodes, wherein ,/>For the position of the j+1th node, +.>Is the starting position->For the end position, there are 79 node paths and 80 nodes.
In the present application, the robot to be measured is a differential robot, and in order to increase the number of path speed plans (robot speeds) for the differential robot, the direction angle of the end point is not constrained, and only the position is constrained, so that a given path is divided into node paths of a plurality of nodes.
Specifically, the determining module 3 obtains cartesian coordinates and orientation angles of the robot to be measured in the plurality of nodes, determines a relation among curvature, speed, and angular speed of the robot to be measured, and performs:
acquiring Cartesian coordinates and orientation angles of a robot to be tested in a plurality of nodes;
calculating to obtain the curvature of the robot to be measured at a plurality of nodes based on the Cartesian coordinates and the orientation angles;
and determining a relation among the curvature, the speed and the angular speed of the robot to be tested according to preset uniform acceleration linear motion conditions and by combining Cartesian coordinates, an orientation angle and curvature.
The determining module 3, when executing, obtains the cartesian coordinates and the orientation angle of the robot to be measured in the plurality of nodes, the cartesian coordinates being (x i ，y i）, wherein ,xi For the i-th node (i.e. node number at path divisionNode i=j+1, and this node is not the last node), y i For robots to be tested in the ith nodeOrdinate, for orientation angle->Indicating (I)>As shown in fig. 4, fig. 4 is a schematic diagram of a divided node path, where x is the direction angle of the robot to be measured in the i-th node i Is the abscissa of the robot to be measured in the ith node,/ >Is the direction angle of the robot to be tested in the ith node,>the motion time of the robot to be measured from the ith node to the (i+1) th node is obtained.
Based on Cartesian coordinates and orientation angles, the curvature of the robot to be measured at a plurality of nodes is calculated, and a calculation formula of the curvature is specifically as follows:
；
wherein ,ki Is the curvature of the robot to be measured in the ith node, L i For the path distance from the ith node to the (i+1) th node of the robot to be tested,the direction angle of the robot to be measured in the ith node.
According to preset uniform acceleration rectilinear motion conditions and by combining Cartesian coordinates, orientation angles and curvatures, determining a relational expression among the curvatures, the speeds and the angular speeds of the robot to be tested, namely assuming that the robot to be tested performs uniform acceleration rectilinear motion among adjacent nodes, when the robot to be tested performs uniform acceleration rectilinear motion among the adjacent nodes, the following relational expression exists:
；
；
；
wherein ,ki Is the curvature of the robot to be measured in the ith node, L i For the path distance, x, from the ith node to the (i+1) th node of the robot to be tested i Is the abscissa, y, of the robot to be tested in the ith node i Is the ordinate of the robot to be measured in the i-th node,acceleration of the robot to be measured in the ith node,/- >Is the linear velocity of the robot to be measured in the ith node (i.e. the velocity of the robot to be measured in the ith node), of>Is the direction angle of the robot to be tested in the ith node,>the angular velocity of the robot to be measured for the i-th node. I is more than or equal to 1 and less than or equal to n, wherein n is the total number of divided nodes-1. Since the robot to be measured needs to stop at the end position (last node), there is no need to calculate the curvature of the robot to be measured at the end position, and since the robot to be measured is in a stopped state at both the start position (first node) and the end position, the acceleration and the speed of the robot to be measured at both the start position and the end position are 0.
In the differential robot, when the euclidean distance between adjacent route points is small, there is the following relation:
；
；
；
wherein ,xi Is the abscissa, y, of the robot to be tested in the ith node i Is the ordinate of the robot to be measured in the i-th node,acceleration of the robot to be measured in the ith node,/->Is the linear velocity of the robot to be measured in the ith node (i.e. the velocity of the robot to be measured in the ith node), of>Is the direction angle of the robot to be tested in the ith node,>for the movement time of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured >The angular velocity of the robot to be measured for the i-th node.
Therefore, the speeds of the robot to be measured at the start position and the end position satisfy the following constraints:
；
；
；
；
wherein ,for the starting speed of the robot to be measured, +.>For the starting acceleration of the robot to be measured, +.>For the end speed of the robot to be measured, +.>The terminal acceleration of the robot to be measured.
And determining and obtaining a relational expression among the curvature, the speed and the angular speed of the robot to be measured according to the relational expression.
Specifically, the establishing module 4 establishes the speed constraint conditions including the actual movement speed constraint condition and the angular speed constraint condition based on the speed information, the relational expression, the cartesian coordinates, and the orientation angle at the time of execution.
The constraint condition of the actual motion speed of the robot to be tested is that the robot to be tested can only move within the range of maximum speed, maximum angular speed and maximum acceleration, namely the constraint of the actual motion speed of the robot to be tested is specifically as follows:
；
wherein ,speed of the robot to be measured for the i-th node,/->For the maximum speed of the robot to be tested,for the maximum angular velocity in the opposite direction of the robot to be measured,/-for>For the positive maximum angular velocity of the robot to be measured,/->Angular velocity of the robot to be measured for the ith node,/- >Acceleration of the robot to be measured for the ith node,/->The maximum acceleration of the robot to be measured. />
In order to meet the angular velocity constraint condition of the robot to be tested, according to the relation among the curvature, the velocity and the angular velocity of the robot to be tested, as the path distance among a plurality of nodes is known and the curvature of each node is also known, the angular velocity constraint is converted into the velocity constraint, and the angular velocity constraint condition is that the angular velocity constraint is converted into the velocity constraint, specifically:
；
wherein ,speed of the robot to be measured for the i-th node,/->For the positive maximum angular velocity of the robot to be measured,/->Curvature of robot to be measured for ith node, L i For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>The direction angle of the robot to be tested is the i-th node.
Specifically, the calculating module 5 calculates, through a gradient-based optimization algorithm, an optimal speed of a plurality of nodes of the robot to be measured by combining a speed constraint condition, speed information, a relation, cartesian coordinates and an orientation angle, and performs:
constructing a speed cost function of the robot to be tested based on the speed constraint condition, the speed information, the relation, the Cartesian coordinates and the orientation angle;
And calculating the optimal speeds of a plurality of nodes of the robot to be measured according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle and by combining the speed cost function through a gradient-based optimization algorithm.
When the calculation module 5 executes, a speed cost function of the robot to be measured is constructed based on the speed constraint condition, the speed information, the relation, the Cartesian coordinates and the orientation angle, and the speed cost function is specifically:
；
wherein cost is a speed cost function, which represents the optimal speed of the robot to be tested at the ith node,speed of the robot to be measured for the i-th node,/->Maximum speed of robot to be tested, +.>Acceleration of the robot to be measured for the ith node,/->For maximum acceleration of the robot to be measured, +.>For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>For velocity smoothing terms，/>As the weight coefficient of the velocity smoothing term,for acceleration soft constraint term, ++>The weight coefficient of the acceleration soft constraint term can be calculated by any gradient-based optimization algorithm, wherein the weight coefficient is the weight coefficient of the acceleration soft constraint term>For acceleration smoothing term, ++>Weight coefficient for acceleration smoothing term, +.>For the shortest time item- >For the weight coefficient of the shortest term, n is the total number of divided nodes-1, i.e. n+1 is the total number of divided nodes, and i and n are integers. The weight coefficient of the velocity smoothing term, the weight coefficient of the acceleration soft constraint term, the weight coefficient of the acceleration smoothing term and the weight coefficient of the shortest term are set to be 0.1, 1.2, 5.0 and 4.0 respectively, and can be adjusted according to actual needs, but the method is not limited to the above. The weight coefficient of the velocity smoothing term, the weight coefficient of the acceleration soft constraint term, the weight coefficient of the acceleration smoothing term and the weight coefficient of the shortest term are set to be 0.1, 1.2, 5.0 and 4.0 respectively, and can be adjusted according to actual needs, but the method is not limited to the above.
The aim of the application is to find a set of speed variables such that the linear speed (speed) and the angular speed of the robot to be measured at each node meet the practical constraints and reduce the navigation time as much as possible. Therefore, the velocity optimization of the known path solves for a significant non-convex problem, designing the acceleration constraint as a soft constraint.
It should be noted that the number of the substrates,the velocity smoothing term is used for increasing square penalty to the velocity difference between three points and restraining the change rate of the velocity of the front point and the rear point.
And the acceleration soft constraint term represents that the acceleration hard constraint is converted into punishment for the acceleration overrun by using a punishment function method, so that the overrun variable is constrained to be within a limit range as much as possible.
The acceleration smoothing term represents a penalty added to the acceleration difference between two points.
The shortest time term means that the acceleration at each point is converged to the maximum speed as much as possible, and the navigation time is reduced.
And calculating to obtain the optimal speeds of a plurality of nodes of the robot to be tested according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle and by combining the speed cost function through a gradient-based optimization algorithm, namely, carrying out iterative solution on the speed cost function through the gradient-based optimization algorithm, and calculating to obtain the optimal speeds of the robot to be tested at all the nodes.
For example, assume that the maximum linear velocity of the robot to be measured is 1.0m/s, the maximum angular velocity is 0.5m/s, and the maximum acceleration is 0.5m/s 2 Dividing a given path into node paths of 80 nodes, acquiring Cartesian coordinates and orientation angles of robots to be detected in the nodes, knowing that the starting point speed, the starting point acceleration, the end point speed and the end point acceleration of the robots to be detected are all 0, and calculating to obtain all nodes of the robots to be detected by combining the actual motion speed constraint condition and the angular speed constraint condition Fig. 5 is a schematic diagram of a speed range of a robot to be measured in a plurality of nodes, wherein an abscissa is a node path, an ordinate is a robot speed, a unit is m/s, and a shaded portion in the diagram is a programmable movement speed of each node of the robot to be measured in the speed range of the robot to be measured in the plurality of nodes, that is, in a limit range of an actual movement speed constraint condition and an angular speed constraint condition.
Setting the weight coefficient of the speed smoothing term, the weight coefficient of the acceleration soft constraint term, the weight coefficient of the acceleration smoothing term and the weight coefficient of the shortest time term to be 0.1, 1.2, 5.0 and 4.0 respectively, and iterating the acceleration soft constraint term in the speed cost function by adopting a gradient-based optimization algorithm when calculating the optimal speed of each node, namelyThe iteration is carried out for 1000 times, the optimal acceleration soft constraint item of each node is determined, so that the optimal speed of the robot to be tested at each node is obtained through calculation, as shown in fig. 6, fig. 6 is a schematic diagram of the optimal speeds of the robot to be tested at a plurality of nodes, wherein the abscissa is a node path, the ordinate is the speed of the robot, the unit is m/s, black lines in a shadow part in the diagram are the optimal speeds of the robot to be tested at each node, a calculated speed planning result (the optimal speed of the robot to be tested at each node) can be seen from the diagram, meanwhile, the speed planning is smooth and short in time consumption, and the given path of the robot can be planned according to the calculated optimal speeds of the robot to be tested at a plurality of nodes. The weight coefficient and the iteration number can be adjusted according to actual needs, but are not limited thereto.
Specifically, the calculation module 5 calculates, by using a gradient-based optimization algorithm, the optimal speeds of a plurality of nodes of the robot to be measured according to the speed constraint condition, the speed information, the relation, the cartesian coordinates and the orientation angle, in combination with the speed cost function, and then performs:
and adjusting the weight coefficient of the speed cost function, and calculating the optimal speeds of a plurality of nodes of the robot to be tested under the weight coefficients of different proportions.
When the calculation module 5 executes, the weight coefficients of different items in the speed cost function are adjusted, the speed cost function under different weight coefficients is iterated, the optimal speeds of a plurality of nodes of the robot to be measured under different weight coefficients can be calculated, the specific gravity of each weight coefficient can be adjusted according to actual needs, and a corresponding speed optimization result (the optimal speeds of a plurality of nodes of the robot to be measured) is obtained. Different weight coefficients can calculate different speed optimization results to obtain different path speed plans, if the path speed plan with optimal time is wanted, the corresponding weight coefficients are required to be adjusted, the corresponding speed optimization results are obtained by calculation, and the speed optimization result with the shortest time consumption (optimal time) is selected to be used as the target (optimal time) path speed plan. The specific gravity of each weight coefficient can be adjusted according to actual needs, different speed optimization results (the optimal speeds of a plurality of nodes of the robot to be tested) are obtained, and different path speed plans are obtained.
As can be seen from the above, the robot speed calculating device divides a given path into node paths of a plurality of nodes according to start and end information of the given path and speed information of the robot to be measured, acquires cartesian coordinates and orientation angles of the robot to be measured in the nodes, determines a relational expression between curvature, speed and angular speed of the robot to be measured, establishes a speed constraint condition based on the speed information, the relational expression and the cartesian coordinates and the orientation angles, and calculates optimal speeds of the nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression and the cartesian coordinates and the orientation angles through an optimization algorithm based on gradients; therefore, the speed of the robot is calculated based on the curvature of the robot to the path and the speed constraint condition of the robot, the problem that the calculated speed deviates from a given path due to curvature mutation in the existing speed planning algorithm is solved, so that the generated optimal speed of the node can not only meet the kinematic constraint requirement of the robot and the path curvature requirement, but also reach the requirement of sufficient smoothness, and the calculation efficiency of the speed of the robot is improved.
Referring to fig. 3, fig. 3 is a schematic structural diagram of an electronic device according to an embodiment of the present application, where the electronic device includes: processor 301 and memory 302, the processor 301 and memory 302 being interconnected and in communication with each other by a communication bus 303 and/or other form of connection mechanism (not shown), the memory 302 storing a computer program executable by the processor 301, which computer program, when the electronic device is running, is executed by the processor 301 to perform the robot speed calculation method in any of the alternative implementations of the above embodiments to carry out the following functions: acquiring speed information of a robot to be measured and start and end information of a given path, dividing the given path into node paths of a plurality of nodes according to the start and end information, acquiring Cartesian coordinates and orientation angles of the robot to be measured in the nodes, determining a relational expression among curvature, speed and angular speed of the robot to be measured, establishing a speed constraint condition based on the speed information, the relational expression and the Cartesian coordinates and the orientation angles, and calculating to obtain the optimal speed of the plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression and the Cartesian coordinates and the orientation angles through a gradient-based optimization algorithm.
An embodiment of the present application provides a computer-readable storage medium having stored thereon a computer program which, when executed by a processor, performs the robot speed calculation method in any of the alternative implementations of the above embodiments to implement the following functions: acquiring speed information of a robot to be measured and start and end information of a given path, dividing the given path into node paths of a plurality of nodes according to the start and end information, acquiring Cartesian coordinates and orientation angles of the robot to be measured in the nodes, determining a relational expression among curvature, speed and angular speed of the robot to be measured, establishing a speed constraint condition based on the speed information, the relational expression and the Cartesian coordinates and the orientation angles, and calculating to obtain the optimal speed of the plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression and the Cartesian coordinates and the orientation angles through a gradient-based optimization algorithm. The storage medium may be implemented by any type of volatile or nonvolatile Memory device or combination thereof, such as static random access Memory (Static Random Access Memory, SRAM), electrically erasable Programmable Read-Only Memory (Electrically Erasable Programmable Read-Only Memory, EEPROM), erasable Programmable Read-Only Memory (Erasable Programmable Read Only Memory, EPROM), programmable Read-Only Memory (PROM), read-Only Memory (ROM), magnetic Memory, flash Memory, magnetic disk, or optical disk.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other manners. The above-described apparatus embodiments are merely illustrative, for example, the division of the units is merely a logical function division, and there may be other manners of division in actual implementation, and for example, multiple units or components may be combined or integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be through some communication interface, device or unit indirect coupling or communication connection, which may be in electrical, mechanical or other form.
Further, the units described as separate components may or may not be physically separate, and components shown as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
Furthermore, functional modules in various embodiments of the present application may be integrated together to form a single portion, or each module may exist alone, or two or more modules may be integrated to form a single portion.
In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and variations will be apparent to those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.

Claims (10)

1. A robot speed calculation method for planning the speed of a robot, comprising the steps of:
acquiring speed information of a robot to be tested and start and end information of a given path;
dividing the given path into node paths of a plurality of nodes according to the start-end information;
acquiring Cartesian coordinates and orientation angles of the robot to be tested in the plurality of nodes, and determining a relation among curvature, speed and angular speed of the robot to be tested;
establishing a speed constraint condition based on the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
And calculating the optimal speeds of a plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
2. The robot speed calculation method according to claim 1, wherein the speed information includes a start speed, a start acceleration, an end speed, and an end acceleration of the robot to be measured at the given path, and a maximum speed, a maximum angular speed, and a maximum acceleration of the robot to be measured;
the start-end information includes a start position and an end position of the given path.
3. The robot speed calculation method according to claim 1, wherein acquiring cartesian coordinates and an orientation angle of the robot to be measured among the plurality of nodes and determining a relational expression between a curvature, a speed, and an angular speed of the robot to be measured, comprises:
acquiring Cartesian coordinates and orientation angles of the robot to be tested in the plurality of nodes;
calculating to obtain the curvature of the robot to be measured at the plurality of nodes based on the Cartesian coordinates and the orientation angle;
And determining a relation among the curvature, the speed and the angular speed of the robot to be tested according to preset uniform acceleration linear motion conditions by combining the Cartesian coordinates, the orientation angle and the curvature.
4. The robot speed calculation method according to claim 1, wherein the speed constraint condition includes an actual movement speed constraint condition and an angular speed constraint condition;
the actual movement speed constraint condition is specifically as follows:
；
the angular velocity constraint condition is to convert the angular velocity constraint into a velocity constraint, and specifically comprises the following steps:
；
wherein ,speed of the robot under test for the ith node,/->For the maximum speed of the robot to be tested,for the maximum angular velocity of the robot to be tested in the opposite direction，/>For the positive maximum angular velocity of the robot to be measured, < >>The angular velocity of the robot to be measured for the i-th node, < >>Acceleration of the robot under test for the ith node, +.>For the maximum acceleration of the robot to be measured, < >>Curvature of the robot under test for the ith node, +.>For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>And the direction angle of the robot to be tested is the i-th node.
5. The method according to claim 1, wherein calculating the optimal speeds of the plurality of nodes of the robot under test by a gradient-based optimization algorithm in combination with the speed constraint, the speed information, the relational expression, the cartesian coordinates, and the orientation angle, comprises:
constructing a speed cost function of the robot to be tested based on the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
and calculating the optimal speeds of a plurality of nodes of the robot to be tested according to the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle by the gradient-based optimization algorithm and combining the speed cost function.
6. The method according to claim 5, wherein calculating the optimal speeds of the plurality of nodes of the robot under test according to the speed constraint condition, the speed information, the relational expression, the cartesian coordinates, and the orientation angle by the gradient-based optimization algorithm in combination with the speed cost function further comprises:
And adjusting the weight coefficient of the speed cost function, and calculating the optimal speeds of a plurality of nodes of the robot to be tested under the weight coefficients of different proportions.
7. The method for calculating the speed of a robot according to any one of claims 5 to 6, wherein the speed cost function is specifically:
；
wherein cost is a speed cost function, which represents the optimal speed of the robot to be tested at the ith node,speed of the robot under test for the ith node,/->For the maximum speed of the robot to be tested, +.>Acceleration of the robot under test for the ith node, +.>For the maximum acceleration of the robot to be measured, < >>For the path distance of the robot to be measured from the ith node to the (i+1) th node,/for the robot to be measured>For speed smoothing item +.>Weight coefficient for velocity smoothing term, +.>For acceleration soft constraint term, ++>The weight coefficient of the acceleration soft constraint term can be calculated by any gradient-based optimization algorithm, wherein the weight coefficient is the weight coefficient of the acceleration soft constraint term>For acceleration smoothing term, ++>Weight coefficient for acceleration smoothing term, +.>For the shortest time item->For the weight coefficient of the shortest term, n is the total number of divided nodes-1, i.e. n+1 is the total number of divided nodes, and i and n are integers.
8. A robot speed calculation apparatus for planning a speed of a robot, comprising:
the acquisition module is used for acquiring speed information of the robot to be detected and start and stop information of a given path;
the dividing module is used for dividing the given path into node paths of a plurality of nodes according to the start and end information;
the determining module is used for acquiring Cartesian coordinates and orientation angles of the robot to be detected in the plurality of nodes and determining a relational expression among the curvature, the speed and the angular speed of the robot to be detected;
the establishing module is used for establishing a speed constraint condition based on the speed information, the relational expression, the Cartesian coordinates and the orientation angle;
and the calculation module is used for calculating the optimal speeds of a plurality of nodes of the robot to be measured by combining the speed constraint condition, the speed information, the relational expression, the Cartesian coordinates and the orientation angle through a gradient-based optimization algorithm.
9. An electronic device comprising a processor and a memory, the memory storing a computer program executable by the processor, when executing the computer program, running the steps of the robot speed calculation method according to any one of claims 1-7.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, runs the steps in the robot speed calculation method according to any one of claims 1-7.
CN202310972254.9A 2023-08-03 2023-08-03 Robot speed calculation method and device, electronic equipment and storage medium Active CN116701821B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310972254.9A CN116701821B (en) 2023-08-03 2023-08-03 Robot speed calculation method and device, electronic equipment and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310972254.9A CN116701821B (en) 2023-08-03 2023-08-03 Robot speed calculation method and device, electronic equipment and storage medium

Publications (2)

Publication Number Publication Date
CN116701821A true CN116701821A (en) 2023-09-05
CN116701821B CN116701821B (en) 2024-03-19

Family

ID=87832489

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310972254.9A Active CN116701821B (en) 2023-08-03 2023-08-03 Robot speed calculation method and device, electronic equipment and storage medium

Country Status (1)

Country Link
CN (1) CN116701821B (en)

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107943034A (en) * 2017-11-23 2018-04-20 南开大学 Complete and Minimum Time Path planing method of the mobile robot along given path
CN109683615A (en) * 2018-12-25 2019-04-26 上海新时达机器人有限公司 The speed look-ahead approach and robot controller in the path that robot continuously moves
CN110109449A (en) * 2019-03-20 2019-08-09 文远知行有限公司 Speed planning method, apparatus, computer equipment and storage medium
CN110187706A (en) * 2019-05-28 2019-08-30 上海钛米机器人科技有限公司 A kind of speed planning method, apparatus, electronic equipment and storage medium
CN110196590A (en) * 2019-04-23 2019-09-03 华南理工大学 A kind of time optimal trajectory planning system and method for robot path tracking
CN111024085A (en) * 2019-12-18 2020-04-17 四川大学 Unmanned aerial vehicle track planning method with end point direction and time constraints
CN112078594A (en) * 2020-08-31 2020-12-15 纵目科技（上海）股份有限公司 Curvature continuous parking path planning device and method for intelligent parking system
WO2021142793A1 (en) * 2020-01-17 2021-07-22 华为技术有限公司 Path planning method and path planning apparatus
CN114859941A (en) * 2022-07-05 2022-08-05 上海仙工智能科技有限公司 Speed planning method under curved smooth path and storage medium
CN115008450A (en) * 2021-03-05 2022-09-06 京东科技信息技术有限公司 Robot control method and device, electronic equipment and storage medium
WO2023024279A1 (en) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 Dynamic obstacle avoidance method and apparatus for robot, and robot
WO2023115784A1 (en) * 2021-12-22 2023-06-29 北京航空航天大学杭州创新研究院 Robot motion information planning method and related apparatus
WO2023124339A1 (en) * 2021-12-29 2023-07-06 灵动科技（北京）有限公司 Path planning method, motion control method and computer program product

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107943034A (en) * 2017-11-23 2018-04-20 南开大学 Complete and Minimum Time Path planing method of the mobile robot along given path
CN109683615A (en) * 2018-12-25 2019-04-26 上海新时达机器人有限公司 The speed look-ahead approach and robot controller in the path that robot continuously moves
CN110109449A (en) * 2019-03-20 2019-08-09 文远知行有限公司 Speed planning method, apparatus, computer equipment and storage medium
CN110196590A (en) * 2019-04-23 2019-09-03 华南理工大学 A kind of time optimal trajectory planning system and method for robot path tracking
US20220107647A1 (en) * 2019-05-28 2022-04-07 Shanghai Taimi Robotics Technology Co., Ltd. Speed planning method and apparatus, electronic device and storage medium
CN110187706A (en) * 2019-05-28 2019-08-30 上海钛米机器人科技有限公司 A kind of speed planning method, apparatus, electronic equipment and storage medium
CN111024085A (en) * 2019-12-18 2020-04-17 四川大学 Unmanned aerial vehicle track planning method with end point direction and time constraints
WO2021142793A1 (en) * 2020-01-17 2021-07-22 华为技术有限公司 Path planning method and path planning apparatus
CN112078594A (en) * 2020-08-31 2020-12-15 纵目科技（上海）股份有限公司 Curvature continuous parking path planning device and method for intelligent parking system
CN115008450A (en) * 2021-03-05 2022-09-06 京东科技信息技术有限公司 Robot control method and device, electronic equipment and storage medium
WO2023024279A1 (en) * 2021-08-24 2023-03-02 深圳市优必选科技股份有限公司 Dynamic obstacle avoidance method and apparatus for robot, and robot
WO2023115784A1 (en) * 2021-12-22 2023-06-29 北京航空航天大学杭州创新研究院 Robot motion information planning method and related apparatus
WO2023124339A1 (en) * 2021-12-29 2023-07-06 灵动科技（北京）有限公司 Path planning method, motion control method and computer program product
CN114859941A (en) * 2022-07-05 2022-08-05 上海仙工智能科技有限公司 Speed planning method under curved smooth path and storage medium

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
PAUL GLOGOWSKI ET.AL: "Robot Speed Adaption in Multiple Trajectory Planning and Integration in a Simulation Tool for Human-Robot Interaction", 《JOURNAL OF INTELLIGENT & ROBOTIC SYSTEMS (2021) 102: 25》, pages 1 - 20 *
傅国辉 等: "基于自适应S 曲线算法的巡检机器人速度规划", 《数字技术与应用》, vol. 40, no. 9, pages 20 - 23 *

Also Published As

Publication number Publication date
CN116701821B (en) 2024-03-19

Similar Documents

Publication Publication Date Title
CN104020665B (en) Mechanical arm minimum jerk track optimizing method based on multi-objective particle swarm algorithm
CN106767780B (en) The extension ellipsoid set-membership filtering method approached based on Chebyshev polynomial interopolations
CN109828579B (en) Mobile robot path planning method for target incremental movement
CN113074739A (en) UWB/INS fusion positioning method based on dynamic robust volume Kalman
CN113359718B (en) Method and equipment for fusing global path planning and local path planning of mobile robot
CN107592671B (en) Networked multi-agent active variable topology autonomous cooperative positioning method
CN113962030B (en) Method for updating disturbance domain of overlapped grids of multi-body separation simulation of aircraft
CN110744543A (en) Improved PRM obstacle avoidance motion planning method based on UR3 mechanical arm
CN111522341A (en) Multi-time-varying formation tracking control method and system for network heterogeneous robot system
CN111814286B (en) Lane-level map geometric model building method for automatic driving
Safavi et al. An opportunistic linear–convex algorithm for localization in mobile robot networks
CN114705196B (en) Self-adaptive heuristic global path planning method and system for robot
CN109877838A (en) Time optimal mechanical arm method for planning track based on cuckoo searching algorithm
CN110632932A (en) Local path planning algorithm based on membrane calculation and particle swarm optimization
Janapati et al. Enhancement of localized routing using CDPSO in WSN
CN116701821B (en) Robot speed calculation method and device, electronic equipment and storage medium
CN112484733B (en) Reinforced learning indoor navigation method based on topological graph
CN112067007B (en) Map generation method, computer storage medium, and electronic device
CN116736219B (en) Passive TDOA-FDOA combined positioning and optimizing station arrangement method based on improved particle swarm algorithm
CN111679269B (en) Multi-radar fusion track state estimation method based on variation
CN115167466B (en) Mobile robot standard control set local path planning method and system
CN116380038A (en) Multisource navigation information fusion method based on online incremental scale factor graph
CN109541541A (en) A kind of interior triangulation location precision correcting method and device
CN115278870A (en) Improved genetic ant colony hybrid positioning method and device based on TDOA selection
CN108681621A (en) RTS Kalman smoothing methods are extended based on Chebyshev orthogonal polynomials

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant