Robot, path planning method and computer readable storage medium
By using environment-aware sensors and processors in robots and optimizing path planning based on field of view constraints, the problem of high driving risks of robots in the prior art is solved, and driving safety is significantly improved.
Patent Information
- Application Number
- CN202210891982.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-07-27
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2042-07-27
AI Technical Summary
Existing robot path planning methods pose high risks when driving, especially the possibility of collision with obstacles outside the field of view of the environmental perception sensor is high, resulting in insufficient driving safety.
By introducing environment-aware sensors, memory and processors into the robot, the initial planning path is obtained and the field of view constraints is determined based on the environment-aware data, and input it to the preset objective function solution to determine the target planning path and ensure that the path point is in the field of view of the perception sensor. Reduce the risk of obstacle collision.
It effectively reduces the possibility of collision between the robot and the obstacles outside the field of view of the environmental perception sensor, and improves the safety of the robot's driving and the reliability of path planning.
Smart Images

Figure CN115113627B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of robotics, and in particular to a robot, a path planning method, and a computer-readable storage medium. Background Art
[0002] Currently, when planning the driving path of a robot, the robot's driving path is usually planned based on the information contained in the cost map and the perception data of the sensors installed on the robot.
[0003] However, when the robot moves based on the path obtained by the above driving planning method, there is a high driving risk. Summary of the invention
[0004] Based on this, it is necessary to provide a robot, a path planning method and a computer-readable storage medium that can improve driving safety in response to the above technical problems.
[0005] In a first aspect, the present application provides a robot, which includes an environment perception sensor, a memory, and a processor. The environment perception sensor is used to collect real-time environment perception data. The memory stores a computer program that can be run on the processor. The processor is used to call and execute the computer program to implement the following steps:
[0006] Get the initial planning path;
[0007] Determine a target constraint condition group based on the initial planned path, the target constraint condition group at least includes a field of view constraint, and the field of view constraint is related to the field of view range of the environment perception sensor;
[0008] The target constraint condition group is input into the preset objective function for solution, and the target planning path is determined from the solution value of the preset objective function.
[0009] In one embodiment, determining the target constraint condition group based on the initial planned path includes:
[0010] According to the field of view of the environment perception sensor at the current position, determine the boundary line of the field of view on the map of the space where the robot is located;
[0011] Based on the boundary lines and the initial planned path, the field of view constraints are determined.
[0012] In one of the embodiments, the target constraint condition group further includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint.
[0013] In one embodiment, the target constraint condition group also includes obstacle constraints, and the target constraint condition group is determined based on the initial planned path, including:
[0014] If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information.
[0015] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and based on the boundary line and the initial planned path, determining the field of view constraint includes:
[0016] For each path point in the initial planned path, determining a positional relationship between the path point and the boundary line, the positional relationship including that the path point is between the first boundary line and the second boundary line and that the path point is not between the first boundary line and the second boundary line;
[0017] If the positional relationship is that the path point is not between the first boundary line and the second boundary line, a field of view constraint is constructed based on the distance between the path point and the target boundary line, wherein the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
[0018] In one embodiment, determining the positional relationship between the path point and the boundary line includes:
[0019] Obtaining a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line;
[0020] Calculate a first vector product of a path vector corresponding to the path point and a first boundary vector, and a second vector product of a path vector corresponding to the path point and a second boundary vector, wherein the starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point;
[0021] If the first vector product and the second vector product are both less than 0 or greater than 0, the position relationship is determined to be that the path point is not between the first boundary line and the second boundary line; otherwise, the position relationship is determined to be that the path point is between the first boundary line and the second boundary line.
[0022] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and the determining of the field of view constraint based on the boundary line and the initial planned path includes:
[0023] For each path point in the initial planned path, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point. The target distance is the smaller value of the first distance between the path point and the first boundary line and the second distance between the path points.
[0024] In a second aspect, the present application also provides a path planning method. The method comprises:
[0025] Get the initial planning path;
[0026] Determine a target constraint condition group based on the initial planned path, the target constraint condition group at least includes a field of view constraint, and the field of view constraint is related to the field of view range of the environment perception sensor;
[0027] The target constraint condition group is input into the preset objective function for solution, and the target planning path is determined from the solution value of the preset objective function.
[0028] In a third aspect, the present application also provides a path planning device. The device includes:
[0029] An acquisition module is used to obtain an initial planning path;
[0030] A determination module, configured to determine a target constraint condition group based on an initial planned path, wherein the target constraint condition group includes at least a field of view constraint, and the field of view constraint is related to a field of view range of an environment perception sensor;
[0031] The planning module is used to input the target constraint condition group into the preset target function for solution, and determine the target planning path from the solution value of the preset target function.
[0032] In a fourth aspect, the present application further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the path planning method as described in the second aspect above is implemented.
[0033] The robot, path planning method, device and computer-readable storage medium described above obtain an initial planning path; determine a target constraint condition group based on the initial planning path, the target constraint condition group at least includes a field of view constraint, and the field of view constraint is related to the field of view of the environmental perception sensor; input the target constraint condition group into a preset objective function for solution, and determine the target planning path from the solution value of the preset objective function. In this application embodiment, since the field of view constraint is used as one of the constraints in the target constraint condition group for determining the target planning path, the field of view of the environmental perception sensor is taken into account when determining the target planning path, the possibility of the robot colliding with obstacles outside the field of view of the environmental perception sensor when driving based on the target planning path is greatly reduced, thereby improving the safety of the robot's driving. BRIEF DESCRIPTION OF THE DRAWINGS
[0034] Figure 1 is a schematic diagram of the structure of a robot in one embodiment;
[0035] Figure 2 A flowchart of steps executed by a processor in one embodiment;
[0036] Figure 3 is a flow chart of step 102 in one embodiment;
[0037] Figure 4 A schematic diagram of a map coordinate system constructed based on a map of a robot body and a space in which the robot body is located in one embodiment;
[0038] Figure 5 is a flow chart of step 202 in one embodiment;
[0039] Figure 6 is a flow chart of step 301 in one embodiment;
[0040] Figure 7 A flowchart of steps executed by a processor in another embodiment;
[0041] Figure 8 is a structural block diagram of a path planning device in one embodiment;
[0042] Fig. 9 is a structural block diagram of a path planning device in another embodiment;
[0043] Fig.10 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION
[0044] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.
[0045] See also Figure 1 The embodiment of the present application provides a robot, which includes an environment perception sensor 1, a memory and a processor.
[0046] The environment perception sensor 1 is used to collect real-time environment perception data.
[0047] Optionally, the sensor type corresponding to the environment perception sensor 1 includes one or more of the following sensors: a color camera, a depth camera, a laser sensor, and a radar sensor.
[0048] Optionally, the number of environment perception sensors provided on the robot may be one or more. When the number of environment perception sensors provided on the robot is more than one, the sensor types corresponding to the environment perception sensors may be the same or different. It should be noted that the sensor type and installation position of the environment perception sensor may be adjusted according to actual conditions.
[0049] The memory is used to store a computer program that can be executed on the processor.
[0050] The memory includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium.
[0051] Processor, used to call and execute computer programs to achieve such Figure 2 The steps shown include step 101 to step 103:
[0052] Step 101, obtaining an initial planned path.
[0053] Before driving, the robot will plan a global planning path based on the set starting position, the set end position and the target map information, wherein the target map information is the map information of the spatial area determined based on the set starting position and the set end position; the global planning path can be calculated using a full coverage path planning algorithm, and the full coverage path planning algorithm can be one of "ox-plowing full coverage path planning" and "U-shaped full coverage path planning". During the driving process, the robot will correct the driving path in real time at every time interval to obtain the target planning path corresponding to the corresponding time interval, for example, the time interval can be 10ms-20ms.
[0054] In an optional implementation, the initial planned path is a path segment intercepted from the global planned path based on the position of the current time node.
[0055] In another optional implementation, the initial planned path is the target driving path planned at the previous time point. The time nodes that the robot passes through in sequence during the driving process are t0 (the time point corresponding to when the robot is at the set starting position), t1, ..., t n For example, the initial planning path corresponding to time t0 is the global planning path, and the initial planning path corresponding to time t1 is the target planning path obtained at time t0, ..., t n The initial planning path corresponding to time is t n-1 The target planning path obtained at all times.
[0056] Step 102: determine a target constraint condition group based on the initial planned path.
[0057] The target constraint condition group includes at least a field of view constraint, which is related to the field of view range of the environment perception sensor.
[0058] Optionally, the field of view constraint may be a safety range constraint condition determined based on the field of view, or a speed constraint condition determined based on the field of view. Specifically, the safety range constraint condition may be: determining a safe driving area based on the field of view to ensure that as many path points in the final planned path as possible fall into the safe driving area, and the fewer path points fall into the safe driving area, the smaller the loss value corresponding to the planned path; the speed constraint condition may be: determining a boundary area based on the field of view, and the speed of the path points falling into the boundary area shall not exceed a preset value.
[0059] Optionally, the target constraint condition group also includes constraints of an existing local path planner itself. For example, the local path planner may be a TEB (Time Elastic Band) local path planner.
[0060] Specifically, the target constraint condition group also includes one or more of the following constraints: speed constraint, acceleration constraint, kinematic constraint, obstacle constraint and following constraint. Among them, the speed constraint is used to ensure that the speed corresponding to the planned target planning path is within a preset range; the acceleration constraint is used to ensure that the acceleration corresponding to the planned target planning path is within a preset range; the kinematic constraint is used to ensure that the kinematic specification is met when driving based on the target planning path; the obstacle constraint is used to ensure that the planned target planning path will not collide with obstacles; the following constraint is used to ensure that the planned target planning path will not deviate too far from the global planning path.
[0061] Optionally, the target constraint condition group further includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint and a following constraint. Specifically, the following constraint is determined based on the initial planned path and the global planned path.
[0062] Step 103, input the target constraint condition group into the preset target function for solution, and determine the target planning path from the solution value of the preset target function.
[0063] The target planning path includes the position information of each path point, the posture of the robot at each path point, and the driving speed of the robot at each path point.
[0064] Optionally, assume that the target constraint group includes the following constraints: f1(x,y), f2(x,y), …, f k (x,y),…,f n (x,y), where, f1(x,y), f2(x,y), f k (x,y) and f n(x, y) represent the first constraint, the second constraint, the kth constraint and the nth constraint in the target constraint group, respectively. (x, y) represent the coordinates of the path points in the planned path to be evaluated. The planned path to be evaluated can be the initial planned path or a planned path obtained based on the initial planned path. For the i-th path point (x i ,y i ), using f n (x,y) can get the path point (x i ,y i ) corresponds to the nth loss value, i.e. f n (x i ,y i ).
[0065] Optionally, the preset objective function is expressed as follows: cost = g(f1(x, y), f2(x, y), ..., f n (x, y)), where cost represents the loss value corresponding to the planned path to be evaluated. Specifically, the preset objective function can be expressed as follows: cost = ∑ k ∑ i f k (x i ,y i ), k represents the kth constraint condition, i represents the ith path point in the planned path to be evaluated; or, the preset objective function can also be expressed as follows: cost = ∑ k ∑ i w k f k (x i ,y i ), where w k represents the kth constraint f k The weight corresponding to (x, y). Alternatively, the preset objective function can be expressed as follows: or, Wherein, n is the number of constraints included in the target constraint group, and m is the number of path points included in the planned path to be evaluated. Correspondingly, the solution value of the preset objective function is the loss value corresponding to the planned path to be evaluated.
[0066] Optionally, obtaining the planned path to be evaluated based on the initial planned path includes the following implementation methods:
[0067] The first method is to obtain multiple planning paths to be evaluated that meet the target constraint condition group based on the initial planning path and the target constraint condition group.
[0068] The second method is to obtain the planned path to be evaluated based on the optimization algorithm in the local path planner and the initial planned path, and the optimization algorithm can be a gradient algorithm or an iterative algorithm. Among them, the local path planner can be a TEB (Time Elastic Band) local path planner.
[0069] Specifically, after receiving the initial planning path, the local planner uses the initial planning path as the planning path to be evaluated, and then uses the preset objective function to calculate the loss value of the planning path to be evaluated, and judges whether the planning path to be evaluated meets the preset goal according to the calculated loss value; if it does not meet the preset goal, then re-plan an intermediate path that meets the target constraint condition group based on the initial planning path, and use the intermediate path as the planning path to be evaluated, and then use the preset objective function to calculate the loss value of the planning path to be evaluated again, and judge whether the planning path to be evaluated meets the preset goal according to the calculated loss value; if it does not meet the preset goal, then re-plan a new intermediate path that meets the target constraint condition group based on the initial planning path or the intermediate path, and use the new intermediate path as the planning path to be evaluated, and then use the preset objective function to calculate the loss value of the planning path to be evaluated again, and judge whether the planning path to be evaluated meets the preset goal according to the calculated loss value; repeat the above process repeatedly until the new intermediate path obtained by planning meets the requirements.
[0070] Optionally, for each planning path to be evaluated, determine whether the loss value corresponding to the planning path to be evaluated meets the preset target. If it does not meet the preset target, replace the planning path to be evaluated with a new one until the loss value corresponding to a certain planning path to be evaluated meets the preset target. Then, the planning path to be evaluated whose loss value meets the preset target will be used as the target planning path.
[0071] Among them, the preset target is that the loss value is less than a preset threshold, or the preset target is that the loss value is the minimum.
[0072] In this embodiment, an initial planning path is obtained; a target constraint group is determined based on the initial planning path, the target constraint group includes at least a field of view constraint, and the field of view constraint is related to the field of view of the environmental perception sensor; the target constraint group is input into a preset objective function for solution, and the target planning path is determined from the solution value of the preset objective function. In this application embodiment, since the field of view constraint is used as one of the constraints in the target constraint group for determining the target planning path, the field of view of the environmental perception sensor is taken into account when determining the target planning path, which greatly reduces the possibility of the robot colliding with obstacles outside the field of view of the environmental perception sensor when driving based on the target planning path, thereby improving the safety of the robot's driving.
[0073] Furthermore, in order to reduce the amount of calculation, it can be determined whether the obstacle constraint needs to be added to the target constraint condition group based on the obstacle information before determining the target planning path. Specifically, the target constraint condition group also includes the obstacle constraint. Figure 2 In the illustrated embodiment, the implementation process of determining the target constraint condition group based on the initial planned path in step 102 includes the following steps:
[0074] If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information.
[0075] In an optional embodiment, the target constraint condition group is input into a preset objective function for solution, and all constraints in the target constraint condition group need to be input into the preset objective function and solved synchronously.
[0076] In one embodiment, Figure 3 As shown, based on Figure 2 In the embodiment shown, the implementation process of determining the target constraint condition group based on the initial planned path in step 102 includes steps 201 and 202:
[0077] Step 201, according to the field of view of the environment perception sensor at the current position, determine the boundary line of the field of view on the map of the space where the robot is located.
[0078] Optionally, the current position point is the position of the robot at the current time node; the map may be a cost map.
[0079] Optionally, based on the current position point and the field of view of the environment perception sensor at the current position point, a boundary line of the field of view range on a map of the space where the robot is located is determined, wherein the starting point of the boundary line is the current position point.
[0080] Specifically, a map coordinate system is constructed based on a map of the robot body and the space in which the robot body is located, and the field of view FOV is projected onto the map according to the field of view FOV of the environment perception sensor to obtain the boundary line.
[0081] Figure 4 Schematic diagram of a map coordinate system constructed based on a map of the robot body and the space in which the robot body is located provided in this embodiment. The map coordinate system is a Cartesian coordinate system. Figure 4 As shown, the center projection point of the robot body is set as the coordinate origin, the front of the robot is set as the direction of the positive semi-axis of the y-axis, the x-axis is established based on the left and right directions of the robot, the projection area of the field of view FOV on the map is determined, and the boundary line is obtained.
[0082] Step 202: Determine the field of view constraint based on the boundary line and the initial planned path.
[0083] Optionally, based on the initial planned path, a planned path to be evaluated is obtained; and based on the boundary line, the visual field constraint corresponding to the planned path to be evaluated is determined. The method for obtaining the planned path to be evaluated is the same as the method mentioned in step 103, which will not be described here.
[0084] Optionally, for each planned path to be evaluated, the safe driving area corresponding to the safety range constraint condition determined based on the field of view mentioned above may be that the distance between the path points in the safe driving area and the boundary line is not less than the first preset value, wherein the loss value corresponding to the path points in the safe driving area is less than the loss value corresponding to the path points not in the safe driving area, and the fewer the path points falling into the safe driving area, the smaller the loss value corresponding to the planned path; the boundary area corresponding to the speed constraint condition determined based on the field of view may be that the distance between the path points in the boundary area and the boundary line is less than or equal to the second preset value, and the speed of the path points falling into the boundary area shall not exceed the preset value. The first preset value and the second preset value may be the same or different.
[0085] In this embodiment, the target constraint condition is constructed according to the boundary line of the field of view on the map of the space where the robot is located. The method is simple and easy to implement.
[0086] In one embodiment, the initial planned path includes a plurality of path points, and the boundary line includes a first boundary line and a second boundary line. Figure 3 The embodiment shown, as Figure 5 As shown, the implementation process of determining the field of view constraint based on the boundary line and the initial planning path in step 202 includes steps 301 and 302:
[0087] Step 301, for each path point in the initial planned path, determine the positional relationship between the path point and the boundary line.
[0088] The positional relationship includes that the path point is located between the first boundary line and the second boundary line and that the path point is not located between the first boundary line and the second boundary line.
[0089] When the path point is located on the first boundary line or the second boundary line, the positional relationship may be determined as the path point not being located between the first boundary line and the second boundary line.
[0090] Step 302: If the position relationship is that the path point is not located between the first boundary line and the second boundary line, a field of view constraint is constructed based on the distance between the path point and the target boundary line.
[0091] The target boundary line is a boundary line between the first boundary line and the second boundary line that is close to the path point.
[0092] Optionally, based on a straight line expression of the target boundary line on the map and the coordinates of each path point on the map, the distance between the path point and the target boundary line is obtained.
[0093] by Figure 4 For example, the first boundary line and the second boundary line in the example are as follows: A1x+B1y+C1=0, A2x+B2y+C2=0. For the path point P(x0,y0), the target boundary line is the second boundary line, and the distance D from the path point P(x0,y0) to the target boundary line is:
[0094]
[0095] Optionally, for each path point that is not located between the first boundary line and the second boundary line, the expression of the field of view constraint corresponding to the path point is:
[0096] cost1 = D,
[0097] Among them, cost1 represents the loss value of the path point, and D is the distance between the path point and the target boundary line.
[0098] Optionally, an expression for the field of view constraint is constructed based on the path points of the initial planned path; when subsequently using the preset objective function to calculate the loss value corresponding to the planned path to be evaluated, it is only necessary to substitute the coordinates of the path points included in the planned path to be evaluated into the above-mentioned field of view constraint expression, that is, the coordinates of the path points included in the planned path to be evaluated in the original field of view constraint expression replace the coordinates of the path points of the initial planned path.
[0099] In this embodiment, by judging whether the path point is between the first boundary line and the second boundary line to determine whether the corresponding path is within the field of view, and establishing a field of view constraint for the path outside the field of view, the reliability of the target planned path is improved, thereby improving the driving safety of the robot.
[0100] Further, such as Figure 6 As shown, based on Figure 5 In the embodiment shown, determining the positional relationship between the path point and the boundary line in step 301 includes steps 401, 402, and 403:
[0101] Step 401: Obtain a first boundary vector corresponding to a first boundary line and a second boundary vector corresponding to a second boundary line.
[0102] Optional, reference Figure 4 , the first boundary vector can be set to or The corresponding unit vector, where o represents the origin of the coordinate system and A1 is the point of the first boundary line; the second boundary vector can be set to or The corresponding unit vector, where o represents the origin of the coordinate system and A2 is the point of the second boundary line.
[0103] Step 402 , calculating a first vector product between a path vector corresponding to the path point and a first boundary vector and a second vector product between the path vector corresponding to the path point and a second boundary vector.
[0104] The starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point.
[0105] Optional, reference Figure 4 , for the path point P(x0,y0), the corresponding path vector is or The corresponding unit vector. Specifically, the first vector product k1 can be: The second vector product k2 can be:
[0106] Step 403, if the first vector product and the second vector product are both less than 0 or greater than 0, then the position relationship is determined to be that the path point is not between the first boundary line and the second boundary line, otherwise, then the position relationship is determined to be that the path point is between the first boundary line and the second boundary line.
[0107] Optionally, if k1<0 and k2<0, the position relationship is determined as the path point is not located between the first boundary line and the second boundary line; if k1>0 and k2>0, the position relationship is determined as the path point is not located between the first boundary line and the second boundary line; otherwise, the position relationship is determined as the path point is located between the first boundary line and the second boundary line.
[0108] This embodiment realizes the determination of the positional relationship between the path point and the boundary line. The method is simple and has a small amount of calculation.
[0109] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, based on Figure 3 In the illustrated embodiment, the process of determining the field of view constraint based on the boundary line and the initial planned path in step 202 includes the following steps:
[0110] For each path point in the initial planned path, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point.
[0111] For each of the path points, the target distance is a smaller value between a first distance between the path point and the first boundary line and a second distance between the path point and the second boundary line.
[0112] Optionally, the loss value of the path point obtained based on the field of view constraint is inversely proportional to the target distance, directly proportional to the planned speed, and directly proportional to the braking distance.
[0113] Specifically, for each path point, the expression of the field of view constraint corresponding to the path point is:
[0114] cost2 = k*v / dis,
[0115] Among them, cost2 represents the loss value of the path point, k represents the proportional coefficient determined based on the braking distance corresponding to the path point, v represents the planned speed corresponding to the path point, and dis represents the target distance from the path point to the boundary line. For example, the k value can be determined as 0.5.
[0116] by Figure 4 Take the first boundary line and the second boundary line in as an example, the linear expressions of the first boundary line and the second boundary line are: A1x+B1y+C1=0, A2x+B2y+C2=0. Then the first distance dis1 from the path point P(x0,y0) to the first boundary line is: Similarly, the second distance dis2 from the path point P(x0,y0) to the second boundary line is: Then dis is: dis=min(dis1,dis2).
[0117] In this embodiment, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point, and a speed constraint based on the boundary line is implemented to avoid the failure to brake in time due to the excessively high planned speed of the path point close to the boundary point, thereby improving the reliability of the target planned path and further improving the driving safety of the robot.
[0118] In one embodiment, a robot is provided. The robot includes an environment perception sensor 1, a memory and a processor, wherein the processor is used to implement the following when executing a computer program: Figure 7 Steps shown:
[0119] Step 501, obtaining an initial planned path.
[0120] Step 502: Determine the boundary line of the field of view on the map of the space where the robot is located according to the field of view of the environment perception sensor at the current position.
[0121] The boundary line includes a first boundary line and a second boundary line.
[0122] Step 503: For each path point in the initial planned path, obtain a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line.
[0123] Step 504 , calculating a first vector product between the path vector corresponding to each path point and the first boundary vector and a second vector product between the path vector corresponding to the path point and the second boundary vector.
[0124] The starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point.
[0125] Step 505, based on the first vector product and the second vector product, determine the positional relationship between each path point and the boundary line; if the positional relationship is that the path point is not between the first boundary line and the second boundary line, then construct a first field of view constraint based on the distance between the path point and the target boundary line.
[0126] The positional relationship includes that the path point is located between the first boundary line and the second boundary line and the path point is not located between the first boundary line and the second boundary line.
[0127] Specifically, if the first vector product and the second vector product are both less than 0 or greater than 0, the position relationship is determined to be that the path point is not located between the first boundary line and the second boundary line, otherwise, the position relationship is determined to be that the path point is located between the first boundary line and the second boundary line. The target boundary line is the boundary line of the first boundary line and the second boundary line that is close to the path point. Based on the straight line expression of the target boundary line on the map and the coordinate points of each path point on the map, the distance between the path point and the target boundary line is obtained.
[0128] Step 506, constructing a second field of view constraint based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point.
[0129] For each path point, the target distance is a smaller value between a first distance between the path point and the first boundary line and a second distance between the path point and the second boundary line.
[0130] Step 507: Add the first field of view constraint and the second field of view constraint to the target constraint condition group.
[0131] Step 508: If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information, and the obstacle constraint is added to the target constraint condition group.
[0132] Step 509: Add at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint to the target constraint condition group.
[0133] Step 510, obtaining a preset objective function based on the objective constraint condition group.
[0134] Step 511, obtaining a planned path to be evaluated, and determining a loss value of the planned path to be evaluated based on a preset objective function, wherein the planned path to be evaluated is an initial planned path or a planned path obtained based on the initial planned path.
[0135] Step 512: if the loss value of the planned path to be evaluated meets the preset target, the planned path to be evaluated corresponding to the loss value is used as the target planned path; if the loss value of the planned path to be evaluated does not meet the preset target, the planned path to be evaluated is updated.
[0136] In one embodiment, based on the same inventive concept, the embodiment of the present application provides a path planning method, the execution subject of which can be a path planning device, and the path planning is set on the robot, and can be implemented as part or all of the robot's terminal by software, hardware, or a combination of software and hardware. The terminal can be a personal computer, a laptop, a media player, a smart TV, a smart phone, a tablet computer, and a portable wearable device. The implementation solution provided by the method to solve the problem is similar to the implementation solution recorded in the above-mentioned robot, so the specific limitations in one or more path planning method embodiments provided below can refer to the limitations on the robot above, and will not be repeated here.
[0137] In one embodiment, a path planning method is provided, the method comprising:
[0138] Get the initial planning path;
[0139] Determine a target constraint condition group based on the initial planned path, the target constraint condition group at least includes a field of view constraint, and the field of view constraint is related to the field of view range of the environment perception sensor;
[0140] The target constraint condition group is input into the preset objective function for solution, and the target planning path is determined from the solution value of the preset objective function.
[0141] In one embodiment, the determining of the target constraint condition group based on the initial planned path includes:
[0142] Determine the boundary line of the field of view on the map of the space where the robot is located according to the field of view of the environment perception sensor at the current position and the current position;
[0143] Based on the boundary lines and the initial planned path, the field of view constraints are determined.
[0144] In one embodiment, the target constraint condition group further includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint.
[0145] In one embodiment, the target constraint condition group further includes obstacle constraints, and the target constraint condition group is determined based on the initial planned path, including:
[0146] If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information.
[0147] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and based on the boundary line and the initial planned path, determining the field of view constraint includes:
[0148] For each path point in the initial planned path, determining a positional relationship between the path point and the boundary line, the positional relationship including that the path point is between the first boundary line and the second boundary line and that the path point is not between the first boundary line and the second boundary line;
[0149] If the positional relationship is that the path point is not between the first boundary line and the second boundary line, a field of view constraint is constructed based on the distance between the path point and the target boundary line, wherein the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
[0150] In one embodiment, determining the positional relationship between the path point and the boundary line includes:
[0151] Obtaining a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line;
[0152] Calculate a first vector product of a path vector corresponding to the path point and a first boundary vector, and a second vector product of a path vector corresponding to the path point and a second boundary vector, wherein the starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point;
[0153] If the first vector product and the second vector product are both less than 0 or greater than 0, the position relationship is determined to be that the path point is not between the first boundary line and the second boundary line; otherwise, the position relationship is determined to be that the path point is between the first boundary line and the second boundary line.
[0154] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and based on the boundary line and the initial planned path, determining the field of view constraint includes:
[0155] For each path point in the initial planned path, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point. The target distance is the smaller value of the first distance between the path point and the first boundary line and the second distance between the path points.
[0156] It should be understood that, although the steps in the flowcharts involved in the above embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.
[0157] Based on the same inventive concept, the embodiment of the present application also provides a path planning device for implementing the path planning method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more path planning device embodiments provided below can refer to the limitations of the path planning method above, and will not be repeated here.
[0158] In one embodiment, Figure 8 As shown, a path planning device is provided, including: an acquisition module 600, a determination module 700 and a planning module 800, wherein:
[0159] The acquisition module 600 is used to acquire an initial planned path.
[0160] The determination module 700 is used to determine a target constraint condition group based on the initial planned path. The target constraint condition group at least includes a field of view constraint. The field of view constraint is related to the field of view range of the environment perception sensor.
[0161] The planning module 800 is used to input the target constraint condition group into the preset target function for solution, and determine the target planning path from the solution value of the preset target function.
[0162] In one embodiment, Fig. 9 As shown, the determination module 700 includes a boundary unit 701 and a construction unit 702, wherein:
[0163] The boundary unit 701 is used to determine the boundary line of the field of view on the map of the space where the robot is located according to the field of view of the environment perception sensor at the current position point;
[0164] The construction unit 702 is used to determine the field of view constraint based on the boundary line and the initial planning path.
[0165] In one embodiment, the target constraint condition group further includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint.
[0166] In one embodiment, the target constraint condition group also includes an obstacle constraint, and the determination module 700 is further specifically configured to:
[0167] If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information.
[0168] In one embodiment, the initial planned path includes a plurality of path points, and the boundary line includes a first boundary line and a second boundary line; the construction unit 702 is further specifically configured to:
[0169] For each path point in the initial planned path, determining a positional relationship between the path point and the boundary line, the positional relationship including that the path point is between the first boundary line and the second boundary line and that the path point is not between the first boundary line and the second boundary line;
[0170] If the positional relationship is that the path point is not between the first boundary line and the second boundary line, a field of view constraint is constructed based on the distance between the path point and the target boundary line, wherein the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
[0171] In one embodiment, the construction unit 702 is further specifically configured to:
[0172] Obtaining a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line;
[0173] Calculate a first vector product of a path vector corresponding to the path point and a first boundary vector, and a second vector product of a path vector corresponding to the path point and a second boundary vector, wherein the starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point;
[0174] If the first vector product and the second vector product are both less than 0 or greater than 0, the position relationship is determined to be that the path point is not between the first boundary line and the second boundary line; otherwise, the position relationship is determined to be that the path point is between the first boundary line and the second boundary line.
[0175] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and the construction unit 702 is further specifically configured to:
[0176] For each path point in the initial planned path, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point. The target distance is the smaller value of the first distance between the path point and the first boundary line and the second distance between the path points.
[0177] In one embodiment, a computer device is provided. The computer device may be a terminal, and its internal structure diagram may be as follows: Fig.10 As shown. The computer device includes a processor, a memory and a communication interface connected via a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The communication interface of the computer device is used to communicate with an external terminal in a wired or wireless manner, and the wireless manner can be implemented through WIFI, a mobile cellular network, NFC (near field communication) or other technologies. When the computer program is executed by the processor, a path planning method is implemented.
[0178] Those skilled in the art will understand that Fig.10 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.
[0179] In one embodiment, a computer readable storage medium is provided, on which a computer program is stored, and when the computer program is executed by a processor, the following steps are implemented:
[0180] Get the initial planning path;
[0181] Determine a target constraint condition group based on the initial planned path, the target constraint condition group at least includes a field of view constraint, and the field of view constraint is related to the field of view range of the environment perception sensor;
[0182] The target constraint condition group is input into the preset objective function for solution, and the target planning path is determined from the solution value of the preset objective function.
[0183] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:
[0184] According to the field of view of the environment perception sensor at the current position and the current position, the boundary line of the field of view on the map of the space where the robot is located is determined; based on the boundary line and the initial planned path, the field of view constraint is determined.
[0185] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:
[0186] The target constraint condition group also includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint.
[0187] In one embodiment, the target constraint condition group further includes an obstacle constraint, and when the computer program is executed by a processor, the following steps are further implemented:
[0188] If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, an obstacle constraint is constructed based on the initial planned path and the obstacle information.
[0189] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and the computer program further implements the following steps when executed by a processor:
[0190] For each path point in the initial planned path, the positional relationship between the path point and the boundary line is determined, and the positional relationship includes that the path point is between the first boundary line and the second boundary line and that the path point is not between the first boundary line and the second boundary line; if the positional relationship is that the path point is not between the first boundary line and the second boundary line, then based on the distance between the path point and the target boundary line, a field of view constraint is constructed, wherein the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
[0191] In one embodiment, when the computer program is executed by a processor, the following steps are also implemented:
[0192] Obtain a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line; calculate a first vector product of the path vector corresponding to the path point and the first boundary vector and a second vector product of the path vector corresponding to the path point and the second boundary vector, the starting point of the path vector is the current position point, and the direction of the path vector is the direction from the current position point to the path point; if the first vector product and the second vector product are both less than 0 or greater than 0, then the position relationship is determined to be that the path point is not located between the first boundary line and the second boundary line, otherwise, then the position relationship is determined to be that the path point is located between the first boundary line and the second boundary line.
[0193] In one embodiment, the initial planned path includes a plurality of path points, the boundary line includes a first boundary line and a second boundary line, and the computer program further implements the following steps when executed by a processor:
[0194] For each path point in the initial planned path, a field of view constraint is constructed based on the target distance from each path point to the boundary line, the planned speed corresponding to each path point, and the braking distance corresponding to each path point. The target distance is the smaller value of the first distance between the path point and the first boundary line and the second distance between the path points.
[0195] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited to this. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited to this.
[0196] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0197] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.
Claims
1. A robot, characterized in that: The robot comprises an environment perception sensor, a memory and a processor. The environment perception sensor is used to collect real-time environment perception data. The memory stores a computer program that can be run on the processor. The processor is used to implement the following steps when calling and executing the computer program: Get the initial planning path; Determining a target constraint condition group based on the initial planned path, the target constraint condition group at least including a field of view constraint, and the field of view constraint is related to the field of view range of the environment perception sensor; Inputting the target constraint condition group into a preset target function for solution, and determining the target planning path according to the solution value of the preset target function; The field of view constraint includes a first field of view constraint, and the initial planned path includes a plurality of path points; Determining the first field of view constraint based on the initial planned path includes: According to the field of view of the environment perception sensor at the current position, determining the boundary line of the field of view on the map of the space where the robot is located; the boundary line includes a first boundary line and a second boundary line; For each of the path points in the initial planned path, determining a positional relationship between the path point and the boundary line; If the positional relationship is that the path point is not between the first boundary line and the second boundary line, the first field of view constraint is constructed based on the distance between the path point and the target boundary line; the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
2. The robot according to claim 1, characterized in that: The target constraint condition group also includes at least one of a velocity constraint, an acceleration constraint, a kinematic constraint, and a following constraint.
3. The robot according to claim 1, characterized in that: The target constraint condition group further includes obstacle constraints, and the determining of the target constraint condition group based on the initial planned path includes: If the real-time environment perception data includes obstacle information, and the obstacle information indicates that there is an obstacle on the initial planned path, the obstacle constraint is constructed based on the initial planned path and the obstacle information.
4. The robot according to claim 1, characterized in that: The determining the positional relationship between the path point and the boundary line includes: Obtaining a first boundary vector corresponding to the first boundary line and a second boundary vector corresponding to the second boundary line; Calculate a first vector product of a path vector corresponding to the path point and the first boundary vector and a second vector product of a path vector corresponding to the path point and the second boundary vector, wherein the starting point of the path vector is the current position point and the direction of the path vector is the direction from the current position point to the path point; If the first vector product and the second vector product are both less than 0 or greater than 0, then the positional relationship is determined to be that the path point is not between the first boundary line and the second boundary line; otherwise, the positional relationship is determined to be that the path point is between the first boundary line and the second boundary line.
5. The robot according to claim 1, characterized in that: The field of view constraint further includes a second field of view constraint; and determining the second field of view constraint based on the initial planned path includes: For each of the path points in the initial planned path, the second field of view constraint is constructed based on the target distance from each of the path points to the boundary line, the planned speed corresponding to each of the path points, and the braking distance corresponding to each of the path points, wherein the target distance is the smaller value of the first distance between the path point and the first boundary line and the second distance between the path points.
6. The robot according to any one of claims 1 to 5, characterized in that: The determining of the target planning path according to the solution value of the preset target function includes: Obtaining a planned path to be evaluated, and determining a loss value of the planned path to be evaluated based on a preset objective function, wherein the planned path to be evaluated is an initial planned path or a planned path obtained based on the initial planned path; If the loss value of the planned path to be evaluated meets the preset target, the planned path to be evaluated corresponding to the loss value is used as the target planned path; if the loss value of the planned path to be evaluated does not meet the preset target, the planned path to be evaluated is updated.
7. A path planning method, characterized in that: The method comprises: Get the initial planning path; Determining a target constraint condition group based on the initial planned path, the target constraint condition group at least including a field of view constraint, and the field of view constraint is related to the field of view range of an environment perception sensor; Inputting the target constraint condition group into a preset target function for solution, and determining the target planning path from the solution value of the preset target function; The field of view constraint includes a first field of view constraint, and the initial planned path includes a plurality of path points; and determining the first field of view constraint based on the initial planned path includes: According to the field of view of the environment perception sensor at the current position, determining a boundary line of the field of view on a map of the space where the robot is located; the boundary line includes a first boundary line and a second boundary line; For each of the path points in the initial planned path, determining a positional relationship between the path point and the boundary line; If the positional relationship is that the path point is not between the first boundary line and the second boundary line, the first field of view constraint is constructed based on the distance between the path point and the target boundary line; the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
8. A path planning device, characterized in that: The device comprises: An acquisition module is used to obtain an initial planning path; A determination module, configured to determine a target constraint condition group based on an initial planned path, wherein the target constraint condition group includes at least a field of view constraint, and the field of view constraint is related to a field of view range of an environment perception sensor; A planning module, used to input the target constraint condition group into a preset target function for solution, and determine the target planning path from the solution value of the preset target function; The field of view constraint includes a first field of view constraint, and the initial planned path includes multiple path points; the determination module is also used to: determine the boundary line of the field of view on the map of the space where the robot is located according to the field of view of the environmental perception sensor at the current position point; the boundary line includes a first boundary line and a second boundary line; for each of the path points in the initial planned path, determine the positional relationship between the path point and the boundary line; if the positional relationship is that the path point is not between the first boundary line and the second boundary line, then construct the first field of view constraint based on the distance between the path point and the target boundary line; the target boundary line is the boundary line between the first boundary line and the second boundary line that is close to the path point.
9. A computer device, characterized in that: The method comprises a memory and a processor, wherein the memory stores a computer program, and wherein the processor implements the steps of the method according to claim 7 when executing the computer program.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to claim 7 are implemented.
Citation Information
Patent Citations
Path planning method and device and storage medium
CN111457923A
Method and device for sensing obstacle information by mobile robot
CN112631266A
Local path planning method and device, storage medium, electronic equipment and vehicle
CN112651535A
Path planning method and device, vehicle and readable storage medium
CN113448335A
Global path planning method and device, terminal and readable storage medium
CN113917912A
