Robot three-dimensional trajectory planning method and system

Abstract

The invention provides a robot three-dimensional trajectory planning method and system, and relates to the technical field of robots, and the method comprises the steps: obtaining the three-dimensional terrain information of a preset region, taking the plane projection position of the i-th predicted position of a robot as the center of a circle, taking the preset moving distance as the radius, determining a circular plane projection, and determining the information of a plurality of to-be-determined positions, determining a route difficulty coefficient and an energy consumption coefficient of the to-be-determined position information so as to select an (i + 1) th prediction position; and obtaining a three-dimensional planning movement track of the robot according to the plurality of predicted positions and the target position. According to the method, when the advancing track of the robot is planned in the three-dimensional terrain, the energy consumption and difficulty of advancing of the robot on the track with a certain gradient can be comprehensively considered, so that the moving track is integrally planned, the energy consumption and difficulty of advancing of the robot are reduced, and the success rate of the robot reaching the target position is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular, to a three-dimensional trajectory planning method and system for robots. Background Art

[0002] In the related art, CN119512126A discloses a local path planning method for an omnidirectional mobile robot with a fixed pose based on an improved DWA algorithm, belonging to the field of path planning. The method includes: determining a starting point and a target point; constructing a motion model of the omnidirectional mobile robot, and determining a local path of the omnidirectional mobile robot through the improved DWA algorithm; controlling the omnidirectional mobile robot to move according to the starting point and the target point, and during the movement, when an obstacle appears, performing speed sampling on the omnidirectional mobile robot according to the motion model, and obtaining a planned path according to the local path and the speed sampling result; evaluating the planned path through the improved DWA algorithm, and obtaining an obstacle avoidance path based on the evaluation result; performing obstacle avoidance movement on the omnidirectional mobile robot according to the obstacle avoidance path, and after the obstacle avoidance movement, moving in the direction of the target point until the target point is reached, which can ensure that the robot can quickly approach the target point and effectively avoid obstacles.

[0003] CN119440026A discloses a multi-objective path planning method for a robot based on a self-learning evolutionary algorithm, which relates to the technical field of mobile robot path planning. The method includes: initializing algorithm parameters; initializing a population; in the employed bee stage, performing path crossover, path mutation, path shortening, and path safety operations on each solution in the population; calculating the objective value of each solution, and dividing the population into a non-dominated solution set and a dominated solution set; in the following bee stage, acting on the non-dominated solution set based on a collaborative learning mechanism, and acting on the dominated solution set based on a domination-guided learning mechanism; combining the non-dominated solution set and the dominated solution set; in the scout bee stage, using an individual restart strategy to act on a solution whose continuous evolution failure times exceed a set maximum threshold; updating the non-dominated solution set, and determining whether the termination condition is reached. The application of this solution can effectively improve the path planning efficiency and enhance the path quality.

[0004] Therefore, in the related art, although the movement path of the robot can be planned to enable the robot to avoid obstacles and improve the path quality, the related art can only plan the planar path. In a three-dimensional terrain, the robot needs to travel on a path with a slope, and the travel difficulty increases, and the energy consumption also increases. The related art cannot consider the travel difficulty and energy consumption of the robot, and thus cannot plan the movement trajectory according to the travel difficulty and energy consumption. Summary of the Invention

[0005] The present invention provides a three-dimensional trajectory planning method and system for a robot, which can solve the technical problem that the related art cannot consider the traveling difficulty and energy consumption of the robot, and thus cannot plan a moving trajectory for the traveling difficulty and energy consumption.

[0006] According to a first aspect of the present invention, there is provided a three-dimensional trajectory planning method for a robot, including:

[0007] Obtain the three-dimensional terrain information of a preset area, where the preset area includes the current position and the target position of the robot;

[0008] Taking the planar projection position of the i-th predicted position of the robot as the center and a preset moving distance as the radius, determine a circular planar projection and a plurality of undetermined position information, where the planar projection positions of the plurality of undetermined position information are evenly distributed on the edge of the circular planar projection. When i = 0, the i-th predicted position is the current position of the robot;

[0009] According to the three-dimensional terrain information of the preset area, determine the route difficulty coefficient from the i-th predicted position to each undetermined position information;

[0010] According to the three-dimensional terrain information of the preset area and the design information of the robot, determine the energy consumption coefficient from the i-th predicted position to each undetermined position information;

[0011] Select the (i + 1)-th predicted position from the plurality of undetermined position information according to the route difficulty coefficient and the energy consumption coefficient;

[0012] Obtain the three-dimensional planned moving trajectory of the robot according to each predicted position and the target position.

[0013] According to a second aspect of the present invention, there is provided a three-dimensional trajectory planning system for a robot, including:

[0014] A three-dimensional terrain module for obtaining the three-dimensional terrain information of a preset area, where the preset area includes the current position and the target position of the robot;

[0015] An undetermined position information module for taking the planar projection position of the i-th predicted position of the robot as the center and a preset moving distance as the radius, determining a circular planar projection and a plurality of undetermined position information, where the planar projection positions of the plurality of undetermined position information are evenly distributed on the edge of the circular planar projection. When i = 0, the i-th predicted position is the current position of the robot;

[0016] A route difficulty coefficient module for determining the route difficulty coefficient from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area;

[0017] The energy consumption coefficient module is used to determine the energy consumption coefficient for reaching each undetermined position information from the i-th predicted position according to the three-dimensional terrain information of a preset area and the design information of the robot;

[0018] The predicted position module is used to select the (i + 1)-th predicted position from multiple undetermined position information according to the route difficulty coefficient and the energy consumption coefficient;

[0019] The trajectory module is used to obtain the three-dimensional planned movement trajectory of the robot according to each predicted position and the target position.

[0020] By adopting the above technical solutions, the present invention can achieve the following technical effects:

[0021] According to the present invention, when planning the movement trajectory of the robot in a three-dimensional terrain, the energy consumption and difficulty of the robot moving on a trajectory with a certain slope can be comprehensively considered to overall plan the movement trajectory, reduce the energy consumption and difficulty of the robot's movement, and improve the success rate of the robot reaching the target position. When determining the route difficulty coefficient, the travel difficulty of the route between two sampling points can be represented by the absolute value of the sine value of the slope angle, so as to objectively represent the travel difficulty of uphill and downhill, and improve the accuracy and objectivity of the route difficulty coefficient. When determining the energy consumption function, the influence of the mechanical efficiency of the robot itself can be considered, a undetermined coefficient is set on the basis of the theoretical value of the robot moving on the ramp, and solved, so that the energy consumption function can more accurately describe the energy consumption of the robot moving on ramps with various slopes. When determining the energy consumption coefficient, the energy consumption of the robot moving on the three-dimensional route between two sampling points and the energy consumption of the robot moving on a flat road can be respectively solved through the energy consumption function, and then the energy consumption coefficient can be obtained, so as to accurately represent the excess energy consumption of the test line corresponding route relative to the flat route without slope. When determining the constraint conditions, the constraint conditions can be set through the average value of the route difficulty coefficients of the routes between the predicted positions and the overall movement direction of the robot, so that the difficulty of the three-dimensional planned movement trajectory of the robot is reduced, and the overall movement direction remains in the direction close to the target position, improving the possibility that the robot can reach the target position smoothly. When determining the objective function, when selecting each predicted position, the overall energy consumption of the three-dimensional planned movement trajectory can be minimized, avoiding falling into a local optimal solution, reducing the overall energy consumption of the robot, so as to save the energy consumption during movement and improve the possibility that the robot can reach the target position smoothly. Description of the Drawings

[0022] Figure 1 Exemplarily shows a schematic flow chart of the robot three-dimensional trajectory planning method according to an embodiment of the present invention;

[0023] Figure 2The block diagram of the robot three-dimensional trajectory planning system according to an embodiment of the present invention is exemplarily shown. Detailed implementation manners

[0024] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are only a part rather than all of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0025] The technical solutions of the present invention will be described in detail below with specific embodiments. The following specific embodiments may be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments.

[0026] Figure 1 The flowchart of the robot three-dimensional trajectory planning method according to an embodiment of the present invention is exemplarily shown. The method includes:

[0027] Step S101: Obtain the three-dimensional terrain information of a preset area, where the preset area includes the current position and the target position of the robot;

[0028] Step S102: Take the planar projection position of the i-th predicted position of the robot as the center of a circle, and take a preset moving distance as the radius to determine a circular planar projection and a plurality of undetermined position information, where the plurality of undetermined position information is evenly distributed on the edge of the circular planar projection. When i = 0, the i-th predicted position is the current position of the robot;

[0029] Step S103: Determine the route difficulty coefficient from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area;

[0030] Step S104: Determine the energy consumption coefficient from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area and the design information of the robot;

[0031] Step S105: Select the (i + 1)-th predicted position from the plurality of undetermined position information according to the route difficulty coefficient and the energy consumption coefficient;

[0032] Step S106: Obtain the three-dimensional planned movement trajectory of the robot according to each predicted position and the target position.

[0033] According to the robot three-dimensional trajectory planning method of an embodiment of the present invention, when planning the traveling trajectory of a robot in a three-dimensional terrain, the energy consumption and difficulty of the robot traveling on a trajectory with a certain slope can be comprehensively considered to perform an overall planning of the moving trajectory, reduce the energy consumption and difficulty of the robot traveling, and improve the success rate of the robot reaching the target position.

[0034] According to an embodiment of the present invention, in step S101, three-dimensional terrain information of a preset area can be obtained. The preset area may include the current position and the target position of the robot. For example, the preset area may be an experimental site in the wild. The ground in this experimental site is not paved with a flat road surface, and the ground in the test site may be uneven. When the robot travels, its route may have a certain slope, thereby increasing the difficulty and energy consumption of its traveling process.

[0035] According to an embodiment of the present invention, in step S102, when planning the moving trajectory of the robot, the moving trajectory can be divided into multiple segments, and the multiple segments of the moving trajectory are respectively planned, and then the multiple segments of the moving trajectory are connected to obtain an overall three-dimensional planned moving trajectory. When planning each segment of the moving trajectory, a circular plane projection can be determined with the plane projection position of the end point of the previous segment of the moving trajectory (i.e., the i-th predicted position) of the robot as the center and the projection length of each segment of the moving trajectory on the two-dimensional plane (i.e., the preset moving distance) as the radius. That is, a circle on the two-dimensional plane. The plane projection position information of the pending position information of the starting point of the next segment of the moving trajectory (i.e., the (i + 1)-th predicted position) is evenly distributed on the edge of the circular plane projection. The most suitable position can be selected from multiple pieces of pending position information as the starting point of the next segment of the moving trajectory, that is, the (i + 1)-th predicted position.

[0036] According to an embodiment of the present invention, in step S103, based on the three-dimensional terrain information of the preset area, the route difficulty coefficient from the end point of the previous segment of the moving trajectory to each piece of pending position information can be determined. For example, the route difficulty coefficient can be determined by the slope of the route from the i-th predicted position to each piece of pending position information.

[0037] According to an embodiment of the present invention, determining the route difficulty coefficient from the i-th predicted position to each piece of pending position information according to the three-dimensional terrain information of the preset area includes: connecting the plane projection position of the i-th predicted position with the plane projection positions of each piece of pending position information to obtain multiple test lines; uniformly sampling the test lines to obtain multiple sampling points on the test lines; determining the height information of the multiple sampling points in the three-dimensional terrain according to the three-dimensional terrain information; and determining the route difficulty coefficient from the i-th predicted position to the pending position information corresponding to the test line according to the height information of the multiple sampling points on the test line.

[0038] According to an embodiment of the present invention, since the circular planar projection is a circular projection on a two-dimensional plane and the to-be-determined position information is distributed on the circular projection on the two-dimensional plane, the test line is a straight line obtained by connecting the planar projection position of the i-th predicted position with the planar projection positions of each to-be-determined position information. The coordinates of each point on this straight line are two-dimensional coordinates. Based on the three-dimensional terrain information, the height information corresponding to each two-dimensional coordinate can be obtained, and the test line can be evenly sampled to obtain multiple sampling points on the test line. Thus, based on the three-dimensional terrain information, the height information of each sampling point in the three-dimensional terrain can be determined.

[0039] According to an embodiment of the present invention, determining the route difficulty coefficient from the i-th predicted position to the to-be-determined position information corresponding to the test line based on the height information of multiple sampling points on the test line includes: determining the route difficulty coefficient E from the i-th predicted position to the to-be-determined position information corresponding to the j-th test line according to formula (1) D,i,j ,

[0040]

[0041] wherein, H i,j,k is the height information of the k-th sampling point on the test line between the i-th predicted position and the j-th to-be-determined position information, H i,j,k-1 is the height information of the k - 1-th sampling point on the test line between the i-th predicted position and the j-th to-be-determined position information. When k - 1 = 0, H i,j,k-1 is the height information of the i-th predicted position, Δl is the interval distance between adjacent sampling points, n is the number of sampling points on the test line, H i,j,n is the height information corresponding to the j-th to-be-determined position information, k ≤ n, and both k and n are positive integers.

[0042] According to an embodiment of the present invention, in formula (1), is the sine value of the slope angle between the k-th sampling point and the k - 1-th sampling point. The larger this sine value is, the greater the slope between the two sampling points, and the greater the difficulty for the robot to travel between the three-dimensional positions corresponding to the two sampling points. Also, when the slope angle is positive, the robot needs to perform a climbing motion and needs to increase power to prevent slipping. When the slope angle is negative, the robot needs to perform a downhill motion and needs to reduce power and increase the braking force. Therefore, when the slope angle is positive or negative, the traveling difficulty of the robot will increase, and the greater the absolute value of the slope angle, the greater the traveling difficulty. Thus, the absolute value of the above sine value can be used To represent the traveling difficulty of the route between two sampling points. Further, the traveling difficulties of the routes between multiple sampling points on a test line can be averaged to obtain the route difficulty coefficient for reaching the to-be-determined position information corresponding to the test line from the i-th predicted position, so as to accurately describe the difficulty of reaching the to-be-determined position information corresponding to the test line from the i-th predicted position.

[0043] In this way, the traveling difficulty of the route between two sampling points can be represented by the absolute value of the sine value of the slope angle, so as to objectively represent the traveling difficulties of uphill and downhill, and improve the accuracy and objectivity of the route difficulty coefficient.

[0044] According to an embodiment of the present invention, in step S104, the energy consumption coefficient is related to the design information of the robot, for example, the self-weight information of the robot, the mechanical efficiency of the robot, etc., and is also related to the terrain information of the route. For example, when climbing a slope, the energy consumption will increase, and when going downhill, the energy consumption will decrease. Therefore, the energy consumption law of the robot can be determined first, and then the energy consumption coefficients of the robot traveling on different routes can be determined.

[0045] According to an embodiment of the present invention, based on the three-dimensional terrain information of a preset area and the design information of the robot, determining the energy consumption coefficients for reaching each to-be-determined position information from the i-th predicted position includes: determining the self-weight information of the robot according to the design information of the robot; testing the energy consumption information of the robot when moving on routes with various slopes according to the self-weight information of the robot to obtain an energy consumption function; and determining the energy consumption coefficient for reaching the to-be-determined position information corresponding to the test line from the i-th predicted position according to the energy consumption function and the height information of multiple sampling points on the test line.

[0046] According to an embodiment of the present invention, the energy consumption function can be used to describe the relationship between the slope of the route where the robot walks and the energy consumption of the robot. Testing the energy consumption information of the robot when moving on routes with various slopes according to the self-weight information of the robot to obtain an energy consumption function includes: obtaining the equation of the undetermined coefficients of the energy consumption function according to formula (2),

[0047] W s =α1(MgΔltanθ s +μMgΔl)+α2(2)

[0048] where, W s is the energy consumption of the robot when the planar projection moves a preset distance during the s-th test, θ s$\theta_s$ is the slope angle of the robot's moving route during the $s$-th test, $\mu$ is the resistance coefficient of the ramp, $\Delta l$ is the interval distance between adjacent sampling points, $g$ is the acceleration due to gravity, $M$ is the self-weight information of the robot, and $\alpha_1$ and $\alpha_2$ are undetermined coefficients; according to the energy consumption and slope angle during multiple tests, the undetermined coefficients are solved to obtain the solution values of the undetermined coefficients; according to the solution values of the undetermined coefficients and the undetermined coefficient equation, the energy consumption function is obtained.

[0049] According to an embodiment of the present invention, in formula (2), when the robot travels on a route with a slope angle of $\theta$ s the traction force required to keep it moving is $Mg\sin\theta$ s $+\mu Mg\cos\theta$ s , and when the slope angle is $\theta$ s , if the projection of the robot moves $\Delta l$ on the plane, then the moving distance of the robot on the slope is Therefore, the theoretical energy consumption of the robot is $Mg\Delta l\tan\theta$ s $+\mu Mg\Delta l$. Further, due to the influence of the robot's own mechanical efficiency, the actual energy consumed by the robot is greater than this theoretical value of energy consumption. Therefore, when determining the robot's energy consumption, undetermined coefficients can be set for this theoretical value, that is, $\alpha_1(Mg\Delta l\tan\theta$ s $+\mu Mg\Delta l)+\alpha_2$.

[0050] According to an embodiment of the present invention, the test route can be changed multiple times, that is, the slope angle is changed, and the energy consumption of the robot when traveling on roads with various slopes (the traveling distance of the projection of the robot on the two-dimensional plane is kept at $\Delta l$) is tested, and the energy consumption and slope angle during multiple tests are substituted into the undetermined coefficient equation to solve the undetermined coefficients and obtain the solution values of the undetermined coefficients, thereby obtaining the energy consumption function.

[0051] In this way, when determining the energy consumption function, the influence of the robot's own mechanical efficiency can be considered, undetermined coefficients are set on the basis of the theoretical value when the robot travels on the ramp, and the solution is carried out, so that the energy consumption function can more accurately describe the energy consumption of the robot when traveling on ramps with various slopes.

[0052] According to an embodiment of the present invention, according to the energy consumption function and the height information of multiple sampling points on the test line, determining the energy consumption coefficient from the $i$-th predicted position to the corresponding undetermined position information of the test line includes: determining the energy consumption coefficient $E$ of the route from the $i$-th predicted position to the corresponding undetermined position information of the $j$-th test line according to formula (3) W,i,j ,

[0053]

[0054] where $W(*)$ is the energy consumption function, $H$i,j,k is the height information of the k-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information, H i,j,k-1 is the height information of the (k - 1)-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information. When k - 1 = 0, H i,j,k-1 is the height information of the i-th predicted position, Δl is the interval distance between adjacent sampling points, n is the number of sampling points on the test line, H i,j,n is the height information corresponding to the j-th undetermined position information. k ≤ n, and both k and n are positive integers.

[0055] According to an embodiment of the present invention, in formula (3), the energy consumption function can describe the energy consumption of the robot when traveling on ramps with various slopes. Substituting the slope angle into the energy consumption function, the energy consumption of the robot when traveling on the path with this slope can be obtained. As described above, is the sine value of the slope angle between the k-th sampling point and the (k - 1)-th sampling point. Therefore, is the slope angle of the route between the two sampling points, is the energy consumption of the robot when traveling on the three-dimensional route between the k-th sampling point and the (k - 1)-th sampling point, and W(0) can represent the energy consumption of the robot when traveling Δl on a road with a slope of 0. The ratio of the two is the relative value of the energy consumption of the route between the k-th sampling point and the (k - 1)-th sampling point. The larger this value is, the higher the energy consumption of the route between the two sampling points. Averaging the relative values of the energy consumption between multiple adjacent sampling points on the test line can obtain the energy consumption coefficient from the i-th predicted position to the undetermined position information corresponding to the test line, which can be used to represent the excess energy consumption of the route corresponding to the test line relative to a flat route without slope. Thus, when selecting a route subsequently, a route that minimizes the excess energy consumption can be selected, thereby reducing the overall energy consumption of the robot.

[0056] In this way, the energy consumption of the robot when traveling on the three-dimensional route between two sampling points and the energy consumption of the robot when traveling on a flat road can be solved respectively through the energy consumption function, and then the energy consumption coefficient can be obtained, so as to accurately represent the excess energy consumption of the route corresponding to the test line relative to a flat route without slope.

[0057] According to an embodiment of the present invention, in step S105, the (i + 1)-th predicted position can be determined through a trajectory planning model, and multiple subsequent predicted positions can be continuously determined, so as to obtain a three-dimensional planned movement trajectory. The trajectory planning model can be a genetic algorithm model, a nonlinear programming model, etc. The constraint conditions and objective function of the trajectory planning model can be set, and the constraint conditions and objective function can be related to the selection of each predicted position. Under the limitation of the constraint conditions, an optimal solution that can maximize the achievement of the objective described by the objective function can be obtained. Based on the selection method of the predicted positions corresponding to this optimal solution, the three-dimensional planned movement trajectory can be obtained, that is, each predicted position is connected to obtain the three-dimensional planned movement trajectory.

[0058] According to an embodiment of the present invention, the constraint conditions of the trajectory planning model are determined according to the route difficulty coefficient and the energy consumption coefficient, including: determining the constraint conditions of the trajectory planning model according to formulas (4) and (5).

[0059]

[0060] where E D,i,p is the route difficulty coefficient from the i-th predicted position to the to-be-determined (i + 1)-th predicted position, m is the number of predicted positions, H tar is the height information of the target position, H now is the height information of the current position, ΔL is the horizontal distance between the target position and the current position, P tar is the two-dimensional projection coordinate of the target position, P i+1 is the two-dimensional projection coordinate of the to-be-determined position information of the (i + 1)-th predicted position, P t is the two-dimensional projection coordinate of the to-be-determined position information of the t-th predicted position. When t = 0, P t is the two-dimensional projection coordinate of the current position, t ≤ i, i ≤ m, and t, i, and m are all positive integers.

[0061] According to an embodiment of the present invention, in formula (4), is the average value of the route difficulty coefficients of the routes between multiple predicted positions, which can represent the average difficulty coefficient of the three-dimensional planned movement trajectory, is the absolute value of the sine value of the slope angle of the connection line between the target position and the current position, which can also represent the difficulty of reaching the target position along a straight line from the current position. It can be expressed that when selecting multiple predicted positions, the average difficulty of the selected route is lower than the difficulty of reaching the target position along a straight line, thereby reducing the traveling difficulty of the robot and increasing the possibility of the robot successfully reaching the target position. And when selecting each position, the overall difficulty can be reduced, avoiding falling into a local optimal solution and reducing the overall difficulty of the three-dimensional planned movement trajectory.

[0062] According to an embodiment of the present invention, in formula (5), |P tar -P i+1 | is the two-dimensional plane distance between the target position and the undetermined position information of the (i + 1)-th predicted position, represents the average value of the two-dimensional plane distances between the target position and the undetermined position information of the previous i predicted positions, as well as the two-dimensional plane distance between the target position and the current position, means that when determining the next predicted position, the overall traveling direction of the robot is in a direction close to the target position, that is, when encountering a route with greater traveling difficulty or higher energy consumption, other routes can be selected for detouring, but the overall traveling direction should be maintained in a direction close to the target position, so that the robot can finally reach the target position.

[0063] In this way, the constraint conditions can be set through the average value of the route difficulty coefficients of the routes between the predicted positions and the overall traveling direction of the robot, so that the difficulty of the three-dimensional planned movement trajectory of the robot is reduced, and the overall traveling direction is maintained in a direction close to the target position, enhancing the possibility that the robot can successfully reach the target position.

[0064] According to an embodiment of the present invention, according to the energy consumption coefficient, the objective function of the trajectory planning model is determined, including: determining the objective function of the trajectory planning model according to formula (6),

[0065]

[0066] where E W,i,p is the energy consumption coefficient from the i-th predicted position to the undetermined (i + 1)-th predicted position, and minimize is the minimization function.

[0067] According to an embodiment of the present invention, in formula (6), is the total energy consumption of the routes between multiple predicted positions, and can also represent the total energy consumption of the three-dimensional planned movement trajectory. It can minimize the total energy consumption of the three-dimensional planned movement trajectory, enabling the robot to save energy during travel, enhancing the possibility of successfully reaching the target position, and furthermore, when selecting each predicted position, it can minimize the overall energy consumption of the three-dimensional planned movement trajectory, avoid falling into a local optimal solution, and reduce the overall energy consumption of the robot.

[0068] In this way, when selecting each predicted position, the overall energy consumption of the three-dimensional planned movement trajectory can be minimized, avoiding falling into a local optimal solution, reducing the overall energy consumption of the robot, thereby saving the energy consumption during travel and enhancing the possibility that the robot can successfully reach the target position.

[0069] According to one embodiment of the present invention, based on the above constraints and objective function, an optimal solution that enables the goal described by the objective function to be achieved to the greatest extent can be solved under the constraints, that is, the optimal way of selecting each prediction position to obtain each prediction position.

[0070] According to one embodiment of the present invention, in step S106, the three-dimensional planned movement trajectory of the robot can be obtained based on each predicted position and the target position, that is, each predicted position and the target position are connected in sequence to obtain the three-dimensional planned movement trajectory of the robot, and moving along the three-dimensional planned movement trajectory can reduce the route difficulty, reduce the energy consumption of the robot, and increase the probability that the robot can successfully reach the target position.

[0071] According to the robot three-dimensional trajectory planning method of the embodiment of the present invention, when planning the robot's travel trajectory in a three-dimensional terrain, the energy consumption and difficulty of the robot traveling on a trajectory with a certain slope can be comprehensively considered to plan the mobile trajectory as a whole, reduce the energy consumption and difficulty of the robot's travel, and improve the success rate of the robot reaching the target position. When determining the route difficulty coefficient, the absolute value of the sine value of the slope angle can be used to represent the travel difficulty of the route between two sampling points, thereby objectively representing the travel difficulty of uphill and downhill, and improving the accuracy and objectivity of the route difficulty coefficient. When determining the energy consumption function, the influence of the robot's own mechanical efficiency can be considered, and the undetermined coefficients are set on the basis of the theoretical value of the robot traveling on the ramp, and the solution is performed, so that the energy consumption function can more accurately describe the energy consumption of the robot traveling on ramps of various slopes. When determining the energy consumption coefficient, the energy consumption function can be used to solve the energy consumption of the robot traveling on the three-dimensional route between two sampling points and the energy consumption of the robot traveling on the plane road, and then the energy consumption coefficient can be obtained, thereby accurately representing the excess energy consumption of the route corresponding to the test line relative to the plane route without slope. When determining the constraint conditions, the constraint conditions can be set by predicting the average value of the route difficulty coefficient of the route between the positions and the overall direction of travel of the robot, so that the difficulty of the robot's three-dimensional planned movement trajectory is reduced, and the overall direction of travel is maintained in the direction close to the target position, thereby increasing the possibility that the robot can successfully reach the target position. When determining the objective function, when selecting each predicted position, the overall energy consumption of the three-dimensional planned movement trajectory can be minimized to avoid falling into the local optimal solution and reduce the overall energy consumption of the robot, thereby saving energy consumption during travel and increasing the possibility that the robot can successfully reach the target position.

[0072] Figure 2 A block diagram of a robot three-dimensional trajectory planning system according to an embodiment of the present invention is exemplarily shown, wherein the system comprises:

[0073] 3D terrain module, used to obtain 3D terrain information of a preset area, where the preset area includes the current position and the target position of the robot;

[0074] Pending position information module, used to determine a circular planar projection and multiple pending position information with the planar projection position of the i-th predicted position of the robot as the center and a preset moving distance as the radius, where the planar projection positions of the multiple pending position information are evenly distributed on the edge of the circular planar projection. When i = 0, the i-th predicted position is the current position of the robot;

[0075] Route difficulty coefficient module, used to determine the route difficulty coefficient from the i-th predicted position to each pending position information according to the 3D terrain information of the preset area;

[0076] Energy consumption coefficient module, used to determine the energy consumption coefficient from the i-th predicted position to each pending position information according to the 3D terrain information of the preset area and the design information of the robot;

[0077] Predicted position module, used to select the (i + 1)-th predicted position from multiple pending position information according to the route difficulty coefficient and the energy consumption coefficient;

[0078] Trajectory module, used to obtain the 3D planned movement trajectory of the robot according to each predicted position and the target position.

[0079] The present invention can be a method, device, system, and / or computer program product. The computer program product can include a computer-readable storage medium having computer-readable program instructions thereon for performing various aspects of the present invention.

[0080] Those skilled in the art should understand that the embodiments of the present invention described above and shown in the accompanying drawings are only examples and do not limit the present invention. The object of the present invention has been fully and effectively achieved. The function and structural principle of the present invention have been shown and described in the embodiments. Without departing from the said principle, the embodiments of the present invention can have any deformation or modification.

[0081] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A three-dimensional trajectory planning method for a robot, characterized in that, Including: Obtain the three-dimensional terrain information of a preset area, where the preset area includes the current position and the target position of the robot; Taking the planar projection position of the i-th predicted position of the robot as the center and a preset moving distance as the radius, determine a circular planar projection and multiple undetermined position information, where the planar projection positions of the multiple undetermined position information are evenly distributed on the edge of the circular planar projection. When i = 0, the i-th predicted position is the current position of the robot; According to the three-dimensional terrain information of the preset area, determine the route difficulty coefficients from the i-th predicted position to each undetermined position information; According to the three-dimensional terrain information of the preset area and the design information of the robot, determine the energy consumption coefficients from the i-th predicted position to each undetermined position information; Select the (i + 1)-th predicted position from the multiple undetermined position information according to the route difficulty coefficients and the energy consumption coefficients; Obtain the three-dimensional planned movement trajectory of the robot according to each predicted position and the target position.

2. The robot three-dimensional trajectory planning method according to claim 1, wherein, Determining the route difficulty coefficients from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area includes: Connect the planar projection position of the i-th predicted position with the planar projection positions of each undetermined position information to obtain multiple test lines; Uniformly sample the test lines to obtain multiple sampling points on the test lines; According to the three-dimensional terrain information, determine the height information of the multiple sampling points in the three-dimensional terrain; According to the height information of the multiple sampling points on the test lines, determine the route difficulty coefficients from the i-th predicted position to the undetermined position information corresponding to the test lines.

3. The robot three-dimensional trajectory planning method according to claim 2, characterized in that, Determining the route difficulty coefficients from the i-th predicted position to the undetermined position information corresponding to the test lines according to the height information of the multiple sampling points on the test lines includes: According to the formula Determine the route difficulty coefficient E for reaching the undetermined position information corresponding to the j-th test line from the i-th predicted position D,i,j , where H i,j,k is the height information of the k-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information, and H i,j,k-1 is the height information of the (k - 1)-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information. When k - 1 = 0, H i,j,k-1 is the height information of the i-th predicted position, Δl is the interval distance between adjacent sampling points, n is the number of sampling points on the test line, and H i,j,n is the height information corresponding to the j-th undetermined position information. k ≤ n, and both k and n are positive integers.

4. The three-dimensional trajectory planning method for a robot according to claim 2, wherein Determining the energy consumption coefficients from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area and the design information of the robot includes: Determine the self-weight information of the robot according to the design information of the robot; Test the energy consumption information of the robot when moving on routes with various slopes according to the self-weight information of the robot to obtain an energy consumption function; According to the energy consumption function and the height information of the multiple sampling points on the test lines, determine the energy consumption coefficients from the i-th predicted position to the undetermined position information corresponding to the test lines.

5. The robot three-dimensional trajectory planning method according to claim 4, wherein, Testing the energy consumption information of the robot when moving on routes with various slopes according to the self-weight information of the robot to obtain an energy consumption function includes: According to the formula W s = α1(MgΔltanθ s + μMgΔl) + α2 Obtain the equation for the undetermined coefficients of the energy consumption function, where W s is the energy consumption for the robot to move a preset distance in the planar projection during the s-th test, θ s is the slope angle of the robot's moving route during the s-th test, μ is the resistance coefficient of the ramp, Δl is the interval distance between adjacent sampling points, g is the acceleration due to gravity, M is the self-weight information of the robot, and α1 and α2 are undetermined coefficients; Solve for the undetermined coefficients according to the energy consumption and slope angles during multiple tests to obtain the solution values of the undetermined coefficients; According to the solution values of the undetermined coefficients and the undetermined coefficient equation, obtain the energy consumption function.

6. The robot three-dimensional trajectory planning method according to claim 5, characterized in that, Determining the energy consumption coefficients from the i-th predicted position to the undetermined position information corresponding to the test lines according to the energy consumption function and the height information of the multiple sampling points on the test lines includes: According to the formula Determine the energy consumption coefficient E of the route from the i-th predicted position to the undetermined position information corresponding to the j-th test line W,i,j , where W(*) is the energy consumption function, H i,j,k is the height information of the k-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information, H i,j,k-1 is the height information of the (k - 1)-th sampling point on the test line between the i-th predicted position and the j-th undetermined position information. When k - 1 = 0, H i,j,k-1 is the height information of the i-th predicted position, Δl is the interval distance between adjacent sampling points, n is the number of sampling points on the test line, H i,j,n is the height information corresponding to the j-th undetermined position information, k ≤ n, and both k and n are positive integers.

7. The robot three-dimensional trajectory planning method according to claim 1, wherein Selecting the (i + 1)-th predicted position from the multiple undetermined position information according to the route difficulty coefficients and the energy consumption coefficients includes: Determine the constraint conditions of the trajectory planning model according to the route difficulty coefficients; Determine the objective function of the trajectory planning model according to the energy consumption coefficient; Solve the trajectory planning model according to the constraint conditions and the objective function to determine the (i + 1)-th predicted position.

8. The robot three-dimensional trajectory planning method according to claim 7, wherein, Determine the constraint conditions of the trajectory planning model according to the route difficulty coefficient and the energy consumption coefficient, including: According to the formula Determine the constraint conditions of the trajectory planning model, where E D,i,p is the route difficulty coefficient from the i-th predicted position to the to-be-determined (i + 1)-th predicted position, m is the number of predicted positions, H tar is the height information of the target position, H now is the height information of the current position, ΔL is the horizontal distance between the target position and the current position, P tar is the two-dimensional projection coordinate of the target position, P i+1 is the two-dimensional projection coordinate of the to-be-determined position information of the (i + 1)-th predicted position, P t is the two-dimensional projection coordinate of the to-be-determined position information of the t-th predicted position. When t = 0, P t is the two-dimensional projection coordinate of the current position, t ≤ i, i ≤ m, and t, i, and m are all positive integers.

9. The robot three-dimensional trajectory planning method according to claim 8, characterized in that, Determine the objective function of the trajectory planning model according to the energy consumption coefficient, including: According to the formula Determine the objective function of the trajectory planning model, where E W,i,p is the energy consumption coefficient from the i-th predicted position to the to-be-determined (i + 1)-th predicted position, and minimize is the minimization function.

10. A three-dimensional trajectory planning system for a robot, characterized in that, Including: A three-dimensional terrain module for obtaining three-dimensional terrain information of a preset area, where the preset area includes the current position and the target position of the robot; An undetermined position information module for determining a circular plane projection and a plurality of undetermined position information with the planar projection position of the i-th predicted position of the robot as the center and a preset moving distance as the radius, wherein the planar projection positions of the plurality of undetermined position information are evenly distributed on the edge of the circular plane projection. When i = 0, the i-th predicted position is the current position of the robot; A route difficulty coefficient module for determining the route difficulty coefficient from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area; An energy consumption coefficient module for determining the energy consumption coefficient from the i-th predicted position to each undetermined position information according to the three-dimensional terrain information of the preset area and the design information of the robot; A predicted position module for selecting the (i + 1)-th predicted position from the plurality of undetermined position information according to the route difficulty coefficient and the energy consumption coefficient; A trajectory module for obtaining the three-dimensional planned movement trajectory of the robot according to each predicted position and the target position.

Images