WO2022252869A1 - 移动设备绕障方法、移动设备及存储介质 - Google Patents

移动设备绕障方法、移动设备及存储介质 Download PDF

Info

Publication number
WO2022252869A1
WO2022252869A1 PCT/CN2022/088575 CN2022088575W WO2022252869A1 WO 2022252869 A1 WO2022252869 A1 WO 2022252869A1 CN 2022088575 W CN2022088575 W CN 2022088575W WO 2022252869 A1 WO2022252869 A1 WO 2022252869A1
Authority
WO
WIPO (PCT)
Prior art keywords
control point
mobile device
path
obstacle
distance
Prior art date
Application number
PCT/CN2022/088575
Other languages
English (en)
French (fr)
Inventor
谢晓佳
刘骁
Original Assignee
北京迈格威科技有限公司
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 北京迈格威科技有限公司 filed Critical 北京迈格威科技有限公司
Publication of WO2022252869A1 publication Critical patent/WO2022252869A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Definitions

  • the present application relates to the technical field of control, and in particular to a mobile device obstacle avoidance method, a mobile device and a storage medium.
  • the corresponding tasks are usually performed along the set path, such as storage robots, hotel service robots, etc., but when there is a temporary obstacle on the travel path, the mobile device will stop in front of the obstacle and wait Obstacles left.
  • a more suitable approach is to circumvent obstacles around a set path, and when an obstacle blocks the traveling path, the mobile device can flexibly go around the obstacle without colliding with the obstacle.
  • the embodiments of the present application are proposed to provide a mobile device obstacle avoidance method, a mobile device, and a storage medium that overcome the above problems or at least partially solve the above problems.
  • a mobile device obstacle avoidance method including:
  • a local target path from the current position to the obstacle avoidance position is determined according to the optimized position of the control point.
  • an obstacle avoidance device for a mobile device including:
  • An initial path determination module configured to determine an initial path from the current position to the obstacle avoidance position based on the path planning instruction, and the obstacle avoidance position is located on the global path;
  • An initial path sampling module configured to sample the initial path, and use the path sampling point as the initial position of the control point of the curve to be fitted;
  • a control point optimization module configured to optimize the position of the control point based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point;
  • the obstacle avoidance path determination module is configured to determine a local target path from the current position to the obstacle avoidance position according to the optimized position of the control point.
  • a mobile device including:
  • One or more processors when the computer readable code is executed by the one or more processors, the mobile device executes the mobile device circumvention method as described in the first aspect.
  • a computer program including computer readable codes, when the computer readable codes are run on a mobile device, causing the mobile device to perform the movement described in the first aspect Device bypass method.
  • a computer-readable storage medium in which the computer program as described in the fourth aspect is stored.
  • the mobile device obstacle avoidance method, mobile device and storage medium provided in the embodiments of the present application determine the initial path from the current position to the obstacle avoidance position based on the path planning instruction, sample the initial path, and use the path sampling points as the curve to be fitted
  • the initial position of the control point based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point, optimize the position of the control point, and determine the current position according to the optimized position of the control point
  • the local target path to the obstacle avoidance position is obtained by optimizing the position of the control point to obtain a better solution, which is solved in a continuous space, and the planned trajectory is optimal, and the position of the control point is used as the optimization variable.
  • the optimization parameters are reduced, and the time-consuming solution is reduced, so that the real-time requirements can be met on the low-cost computing unit.
  • FIG. 1 is a flow chart of the steps of a mobile device obstacle avoidance method provided by an embodiment of the present application
  • Fig. 2 is a schematic diagram of determining an obstacle avoidance position in an embodiment of the present application
  • Fig. 3 is a structural block diagram of an obstacle avoidance device for a mobile device provided by an embodiment of the present application
  • Fig. 4 schematically shows a block diagram of a mobile device for performing a method according to the present application.
  • Fig. 5 schematically shows a storage unit for holding or carrying program codes for realizing the method according to the present application.
  • Fig. 1 is a flow chart of the steps of a mobile device obstacle avoidance method provided in an embodiment of the present application. As shown in Fig. 1, the method may include:
  • Step 101 based on a path planning instruction, determine an initial path from a current location to an obstacle avoidance location, where the obstacle avoidance location is located on a global path.
  • the global path is a global path from an initial starting point to a target end point when a mobile device (such as a robot) performs a certain task.
  • a mobile device such as a robot
  • a certain position in the global path may be blocked by obstacles, which requires local circumvention.
  • the mobile device When the mobile device travels along the planned trajectory of the previous path planning cycle, if it is about to collide with an obstacle, or if the mobile device deviates too far from the global path, it needs to re-plan the path and confirm that the path planning command has been received. Based on the path planning instruction, first determine the end point of the local plan, that is, the obstacle avoidance position. A point on the global path that is the first distance from the current position can be used as the obstacle avoidance position of the local plan, so as to obtain an obstacle avoidance position based on the current position and the obstacle avoidance position. The collision's obstacle course.
  • the mobile device 1 when the mobile device 1 travels on the global path 2, it will collide with the first obstacle 3 and the second obstacle 4, and at this time determine a point on the global path 2 that is the first distance from the current position G', as a locally planned obstacle avoidance position, to bypass the first obstacle 3 and the second obstacle 4 .
  • the first distance can be set as required, for example, it can be 8 meters.
  • an initial solution can be obtained through the path search algorithm, that is, based on the navigation map, an initial path from the current position to the obstacle avoidance position can be searched through the path search algorithm.
  • the search may fail. At this time, you can proceed according to the global path and plan again when the next path planning cycle is reached.
  • the path search algorithm may be an A* algorithm or a Hybrid A* algorithm.
  • the A* search is performed on a discrete grid map, and the side length of each grid is generally 5cm.
  • Hybrid A* is performed on a discrete state space, such as steering wheel at -10, 0, 10 degrees and forward 1 meter.
  • the method also includes:
  • the mobile device can include a map module, a positioning module for the pose of the mobile device, a global planning module, an obstacle avoidance module, and a control module.
  • the grid map is updated.
  • the probability grid map is used to represent the probability of obstacles in each grid. According to the probability of the probability grid map, the occupancy status of the specified location can be obtained. The occupancy status can include obstacles, idle or unknown;
  • the map module then performs Euclidean Distance Transform (EDT) on the probability grid map to obtain a navigation map. In the navigation map, the value in each grid represents the distance between the position and the nearest obstacle.
  • the obstacle avoidance module is used to implement the mobile device obstacle avoidance method in the embodiment of the present application.
  • the control module is used to control the mobile device to travel according to the planned trajectory.
  • the mobile device plans the path locally according to the path planning cycle.
  • it is judged whether the distance from the current position to the global path is greater than the first distance threshold. If so, it may be due to control and other reasons.
  • the mobile device deviates too far from the global path and needs to re-plan the path. Make sure that the path planning command is received. If not, continue to judge whether the planned trajectory of the previous path planning cycle will collide with obstacles. If it will not collide with obstacles, That is, there is no obstacle within the first distance in the traveling direction of the planned trajectory of the previous path planning cycle, so there is no need to re-plan the path.
  • the distance between the mobile device and the obstacle in the traveling direction is greater than or equal to the second distance threshold, or whether the distance between the predicted obstacle avoidance position and the obstacle is greater than or equal to the third Distance threshold, if there is an obstacle within the first distance in the direction of travel in the planned trajectory of the previous path planning cycle, the distance between the mobile device and the obstacle in the direction of travel is greater than or equal to the second distance threshold, or the predicted If the distance between the obstacle avoidance position and the obstacle is greater than or equal to the third distance threshold, it is determined that the path needs to be re-planned, that is, if the path planning instruction is received, if there is an obstacle within the first distance in the planned trajectory of the previous path planning cycle In the case of an object, if the distance between the mobile device and the obstacle in the direction of travel is less than the second distance threshold, or the distance between the obstacle avoidance position and the obstacle is less than the third distance threshold, it is determined that there is no need to re-plan the route
  • the determining an initial path from the current position to the obstacle avoidance position based on the path planning instruction includes: determining an area within a second distance from the global path as a search area; In the search area, an initial path from the current position to the obstacle avoidance position is searched through a path search algorithm.
  • search area that is, the area within the second preset distance of the global path is used as the search area, such as the area within the two dotted lines in Figure 2.
  • search area such as the area within the two dotted lines in Figure 2.
  • search algorithm may adopt the Hybrid A* algorithm.
  • the method further includes: if the search area is completely blocked by obstacles, when searching for the initial path from the current position to the obstacle avoidance position through the path search algorithm, determine Search failed.
  • the search for the initial path will fail when searching for the initial path through the path search algorithm, because the obstacles cannot be circumvented in the search area due to large obstacles. At this time, there is no need to go around the obstacle, and the obstacle stopping module can be responsible for stopping the obstacle.
  • Step 102 sampling the initial path, and using the path sampling points as the initial positions of the control points of the curve to be fitted.
  • the initial path can be sampled according to the distance interval or time interval, and the obtained path sampling points can be used as the initial positions of the control points of the curve to be fitted.
  • the curve to be fitted may be a B-spline curve.
  • sampling the initial path includes: calculating the time required to travel on the initial path; sampling the initial path according to a specified time interval to obtain the initial path sampling points on the path.
  • the optimization variable is the control point of the curve to be fitted, it is necessary to fit the initial path to the curve first.
  • the time required for the mobile device to travel on the initial path can be estimated by using the trapezoidal According to the planning and estimation, the time is sampled according to the specified time interval to obtain multiple time sampling points, and the path sampling point that the mobile device arrives at when the mobile device reaches each time sampling point during the travel is estimated to obtain the time sampling point The path sampling point corresponding to the point on the initial path.
  • the curve to be fitted can be a B-spline curve.
  • the optimization variables are reduced, and the B-spline curve is used for parameterization.
  • the optimization variables are n+1 control points, and the optimization
  • the number of variables is 2*(n+1), and usually the number of control points is not too much.
  • B-spline curve has The convex hull property can simplify the representation of boundary constraints.
  • Step 103 based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point, optimize the position of the control point.
  • the velocity parameter may be jerk, and the jerk may also be called jerk or force rate of change, which is the rate of change of acceleration.
  • the position of the control point is optimized so that the distance between the control point and the obstacle is small, the safety of the mobile device is ensured, and the speed parameter of the mobile device at the control point is small. Ensure the smoothness of the curve to be fitted.
  • n+1 is the number of control points
  • Q i is the position of the i-th control point
  • p is the degree of B-spline curve
  • N i,p (t) is the basis function of B-spline curve of degree p.
  • the optimization variables are: [Q p ,Q p+1 ,...,Q np ], the control points at both ends do not participate in the optimization, this is to ensure that the positions of the starting point and the ending point are equal to the set values.
  • the solution may fail.
  • the path planning can be performed after waiting for the next path planning cycle.
  • optimizing the position of the control point based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point includes: based on the initial position, control The distance between the point and the obstacle and the speed parameter of the mobile device at the control point are adjusted iteratively to the position of the control point until the distance between the control point and the obstacle and the speed parameter of the mobile device at the control point converge.
  • the position of the control point is iteratively adjusted to optimize the position of the control point.
  • the initial position is substituted into the The equation of the fitting curve, the curve to be fitted corresponding to the initial position is obtained, the trajectory between the current position and the obstacle avoidance position in the curve to be fitted corresponding to the initial position is determined, and the distance between the control point and the obstacle is determined in the trajectory and
  • the speed parameter of the mobile device at the control point based on the distance between the control point and the obstacle and the speed parameter of the mobile device at the control point, adjust the position of the control point so that the distance between the control point and the obstacle and the distance between the mobile device and the control point
  • the speed parameter at the point decreases, iteratively execute the above-mentioned equation of substituting the position of the control point into the curve to be fitted, obtain the curve to be fitted corresponding to the position of the control point, and adjust the position of the control point until the
  • the safety constraint function is used to ensure that there is no collision on the optimized trajectory
  • the smoothness constraint function is used to ensure the smoothness of the optimized trajectory.
  • the position of the control point is iteratively adjusted based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point until the control point and the obstacle Convergence of the distance and the speed parameter of the mobile device at the control point, and also includes: based on the initial position, the speed of the mobile device on the curve to be fitted during the iteration process and the acceleration of the mobile device on the curve to be fitted during the iteration process , constructing a velocity constraint function; according to the velocity constraint function, iteratively adjusting the positions of the control points until the velocity constraint function converges.
  • the equipment needs to fit the acceleration on the curve, construct the velocity constraint function, and adjust the position of the control point iteratively based on the security constraint function, smoothness constraint function and velocity constraint function until the security constraint function, smoothness
  • the item constraint function and the velocity constraint function converge, and the iterative operation ends, which can ensure the safety of the mobile device, the smoothness of the trajectory, and the velocity and acceleration on the optimized trajectory meet the requirements.
  • the position of the control point is iteratively adjusted based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point until the control point and the obstacle Convergence of the distance and the velocity parameter of the mobile device at the control point, further comprising: constructing a curvature constraint function based on the initial position and the curvature of the curve to be fitted in the iterative process; based on the initial position and the curvature to be fitted in the iterative process The starting point angle and the end point angle of the curve are used to construct an angle constraint function; according to the curvature constraint function and the angle constraint function, the positions of the control points are iteratively adjusted until the curvature constraint function and the angle constraint function converge.
  • the curvature constraint can be constructed based on the initial position and the curvature of the curve to be fitted during the iteration process.
  • the optimization parameters can also include the speed of the mobile device on the optimized trajectory, the speed of the mobile device at the control point.
  • the control point based on the initial position, the distance between the control point and the obstacle and the speed parameter of the mobile device at the control point, iteratively adjust the position of the control point until the distance between the control point and the obstacle Converging with the velocity parameter of the mobile device at the control point, including: weighting the security constraint function, smoothness constraint function, velocity constraint function, acceleration constraint function, curvature constraint function and angle constraint function, and determining the objective function ; Based on the objective function, iteratively adjust the positions of the control points until the objective function converges.
  • f total is the objective function
  • F s is the smoothing term constraint function
  • F c is the safety constraint function
  • F v is the velocity constraint function
  • F a is the acceleration constraint function
  • F k is the curvature constraint function
  • F ⁇ is the angle constraint function
  • ⁇ 1 is the weight of the smooth term constraint function
  • ⁇ 2 is the weight of the safety constraint function
  • ⁇ 3 is the weight of the speed constraint function and acceleration constraint function
  • ⁇ 4 is the weight of the curvature constraint function
  • ⁇ 5 is the angle constraint function the weight of.
  • the smoothness constraint function is used to ensure the smoothness of the optimized trajectory
  • the safety constraint function is used to ensure that there is no collision on the optimized trajectory
  • the velocity constraint function and acceleration constraint function are used to ensure that the speed and acceleration on the optimized trajectory are within the set range
  • the curvature constraint function to ensure that the curvature of the optimized trajectory is within the set range
  • the constraints of the angle constraint function to ensure that the directions of the starting point and the end point of the optimized trajectory are as unchanged as possible.
  • Q i represents the i-th control point
  • F s (i) represents the sum of the squares of the speed parameters at the i-th control point
  • the security constraint function is expressed as follows:
  • F c (d(Q i )) represents the sum of the squares of the distance between the control point and the obstacle, d(Q i ) is the distance between the i-th control point and the obstacle, and d thr represents the distance between the mobile device and the obstacle when a collision occurs the minimum distance of objects;
  • the speed constraint function is expressed as follows:
  • F v (v ⁇ ) represents the speed constraint function
  • v ⁇ represents the speed on the curve to be fitted in the iterative process
  • v max represents the speed threshold
  • the acceleration constraint function is expressed as follows:
  • F a (a ⁇ ) represents the acceleration constraint function
  • a ⁇ represents the acceleration on the curve to be fitted during the iterative process
  • a max represents the acceleration threshold
  • the smooth term constraint function is represented by the sum of the squares of the velocity parameters.
  • the safety constraint function is represented by the sum of squares of distances between control points and obstacles, and d(Q i ) can be obtained directly from the navigation map.
  • velocity constraint function and acceleration constraint function velocity and acceleration can be obtained by difference of control points.
  • the curvature constraint function is used to constrain the curvature of the optimized trajectory within the set curvature range, and the curvature can be calculated from the control points.
  • the angle constraint function is used to constrain the directions of the starting point and the ending point of the optimized trajectory to be as unchanged as possible, and the starting point angle and the ending point angle can be calculated from the control points.
  • the open-source third-party library ipopt or nlopt can be used to optimize and solve the position of the above-mentioned control points.
  • the position of the control point is optimized, including:
  • the parameters of the curve fitting include the speed of the mobile device on the curve to be fitted in the iteration process, the acceleration of the mobile device on the curve to be fitted in the iteration process, the curvature of the curve to be fitted in the iteration process, and the speed to be fitted in the iteration process.
  • One or more of the start and end angles of the fitted curve are the start and end angles of the fitted curve.
  • the initial position is substituted into the equation of the curve to be fitted to obtain the curve to be fitted corresponding to the initial position, and the trajectory between the current position and the obstacle avoidance position in the curve to be fitted corresponding to the initial position is determined.
  • the parameters of the curve to be fitted in the iterative process include the speed of the mobile device on the curve to be fitted in the iterative process, the acceleration of the mobile device on the curve to be fitted in the iterative process, the curve to be fitted in the iterative process
  • One or more of the curvature, the starting point angle and the ending point angle of the curve to be fitted during the iterative process can ensure that the optimized trajectory meets the requirements and improve safety.
  • Step 104 Determine a local target path from the current position to the obstacle avoidance position according to the optimized position of the control point.
  • the position of the optimized control point is substituted into the equation of the curve to be fitted to obtain the curve to be fitted, and the curve segment from the current position to the obstacle avoidance position is intercepted from the curve to be fitted , get the local target path from the current position to the obstacle avoidance position.
  • the curve to be fitted is a B-spline curve
  • the determining the local target path from the current position to the obstacle avoidance position according to the optimized position of the control point includes:
  • the local target path from the current position to the obstacle avoidance position is determined according to the following B-spline curve:
  • n+1 is the number of control points
  • Q i is the position of the i-th control point
  • p is the degree of B-spline curve
  • N i,p (t) is the basis function of B-spline curve of degree p.
  • a cubic B-spline curve may be used to solve the problem, that is, p is 3.
  • the mobile device obstacle avoidance method determines the initial path from the current position to the obstacle avoidance position based on the path planning instruction, samples the initial path, and takes the path sampling point as the initial position of the control point of the curve to be fitted, Based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point, optimize the position of the control point, and determine the local target from the current position to the obstacle avoidance position according to the optimized position of the control point
  • the path because the better solution is obtained by optimizing the position of the control point, is the solution in the continuous space, the planned trajectory is optimal, and the position of the control point is used as the optimization variable, compared with the optimization-based method, the reduction
  • the parameters are optimized, and the time-consuming solution is reduced, so that the real-time requirements can be met on the low-cost computing unit.
  • the mobile device will not collide with obstacles when traveling along the local target path, and the local target path satisfies the angle constraint condition and the curvature constraint condition, then output the partial target path;
  • the angle constraint condition is a condition to be satisfied by the preset start angle and the end point angle
  • the curvature constraint condition is a condition to be satisfied by the curvature of the preset motion trajectory
  • the local target path After determining the local target path from the current position to the obstacle avoidance position, check the local target path to check whether the mobile device will collide with obstacles when traveling along the local target path, and check whether the local target path Satisfy the angle constraints and curvature constraints, if the mobile device will not collide with obstacles when traveling along the local target path, and the local target path satisfies the angle constraints and curvature constraints, then the local target path can be output to the control module , the mobile device is controlled by the control module to travel along the local target path so as to avoid obstacles.
  • the global path will be output to the control module, and the control module will control the mobile device according to The global path to travel.
  • path verification it is ensured that the mobile device travels along a better path, which can ensure the security of the mobile device.
  • the method also includes:
  • the mobile device needs to travel on the global path. At this time, the mobile device determines the distance between the mobile device and the obstacle in real time during the travel process. If If the distance is less than the fourth distance threshold, that is, the mobile device is about to collide with an obstacle, it stops moving, so as to avoid collision with the obstacle.
  • Fig. 3 is a structural block diagram of an obstacle avoidance device for a mobile device provided in an embodiment of the present application. As shown in Fig. 3, the obstacle avoidance device for a mobile device may include:
  • the initial path determination module 301 is configured to determine an initial path from the current position to the obstacle avoidance position based on the path planning instruction, and the obstacle avoidance position is located on the global path;
  • An initial path sampling module 302, configured to sample the initial path, and use the path sampling point as the initial position of the control point of the curve to be fitted;
  • a control point optimization module 303 configured to optimize the position of the control point based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point;
  • the obstacle avoidance path determination module 304 is configured to determine a local target path from the current position to the obstacle avoidance position according to the optimized position of the control point.
  • control point optimization module is specifically used for:
  • control point optimization module includes:
  • a security constraint construction unit configured to construct a security constraint function based on the initial position and the distance between the control point and the obstacle;
  • a smooth item constraint construction unit is used to construct a smooth item constraint function based on the initial position and the velocity parameter of the mobile device at the control point;
  • An iterative adjustment unit configured to iteratively adjust the position of the control point according to the security constraint function and the smoothness constraint function until the security constraint function and the smoothness constraint function converge.
  • control point optimization module also includes:
  • a velocity constraint construction unit configured to construct a velocity constraint function based on the initial position, the velocity of the mobile device on the curve to be fitted during the iterative process, and the acceleration of the mobile device on the curve to be fitted during the iterative process;
  • the iterative adjustment unit is further configured to iteratively adjust the position of the control point according to the speed constraint function until the speed constraint function converges.
  • control point optimization module also includes:
  • a curvature constraint construction unit configured to construct a curvature constraint function based on the initial position and the curvature of the curve to be fitted in the iterative process
  • An angle constraint construction unit is used to construct an angle constraint function based on the initial position and the starting point angle and the end point angle of the curve to be fitted in the iterative process;
  • the iterative adjustment unit is further configured to iteratively adjust the position of the control point according to the curvature constraint function and the angle constraint function until the curvature constraint function and the angle constraint function converge.
  • control point optimization module includes:
  • An objective function determination unit configured to weight the safety constraint function, smoothness constraint function, velocity constraint function, acceleration constraint function, curvature constraint function, and angle constraint function to determine the objective function;
  • An iterative optimization unit configured to iteratively adjust the positions of the control points based on the objective function until the objective function converges.
  • smoothing item constraint function is expressed as follows:
  • Q i represents the i-th control point
  • F s (i) represents the sum of the squares of the speed parameters at the i-th control point
  • the security constraint function is expressed as follows:
  • F c (d(Q i )) represents the sum of the squares of the distance between the control point and the obstacle, d(Q i ) is the distance between the i-th control point and the obstacle, and d thr represents the distance between the mobile device and the obstacle when a collision occurs the minimum distance of objects;
  • the speed constraint function is expressed as follows:
  • F v (v ⁇ ) represents the speed constraint function
  • v ⁇ represents the speed on the curve to be fitted in the iterative process
  • v max represents the speed threshold
  • the acceleration constraint function is expressed as follows:
  • F a (a ⁇ ) represents the acceleration constraint function
  • a ⁇ represents the acceleration on the curve to be fitted during the iterative process
  • a max represents the acceleration threshold
  • control point optimization module is specifically used for:
  • the parameters of the fitting curve include the speed of the mobile device on the curve to be fitted in the iterative process, the acceleration of the mobile device on the curve to be fitted in the iterative process, the curvature of the curve to be fitted in the iterative process, and the to-be-fitted curve in the iterative process.
  • One or more of the start and end angles of the fitted curve are the start and end angles of the fitted curve.
  • the initial path sampling module includes:
  • a time estimation unit for calculating the time required for traveling on the initial path
  • the sampling unit is configured to sample the initial path at specified time intervals to obtain path sampling points on the initial path.
  • the device also includes:
  • a re-planning judging module configured to, when arriving at the path planning cycle, if the distance from the current position to the global path is greater than the first distance threshold, or the planned trajectory of the previous path planning cycle has obstacles within the first distance in the direction of travel and the distance from the obstacle is greater than or equal to the second distance threshold, or, the planned trajectory of the last path planning cycle has obstacles within the first distance in the direction of travel and the distance between the predicted temporary end point and the obstacle is greater than or equal to the third distance threshold, it is determined that the path planning instruction is received.
  • the initial path determination module includes:
  • a search area determining unit configured to determine an area within a second distance from the global path as a search area
  • An initial path search unit configured to search for an initial path from the current position to the obstacle avoidance position through a path search algorithm within the search area.
  • the device also includes:
  • the search failure determination module is configured to determine that the search has failed when searching for an initial path from the current position to the obstacle avoidance position through a path search algorithm if the search area is completely blocked by obstacles.
  • the obstacle avoidance path determination module is specifically used for:
  • the local target path from the current position to the obstacle avoidance position is determined according to the following B-spline curve:
  • n+1 is the number of control points
  • Q i is the position of the i-th control point
  • p is the degree of B-spline curve
  • N i,p (t) is the basis function of B-spline curve of degree p.
  • the device also includes:
  • a path output module configured to output the local target path if the mobile device will not collide with obstacles when traveling along the local target path, and the local target path satisfies the angle constraint condition and the curvature constraint condition; Otherwise, output the global path.
  • the device also includes:
  • An obstacle-stopping module configured to stop traveling if the distance to the obstacle is less than the fourth distance threshold when traveling on the global path.
  • the mobile device obstacle avoidance device determines the initial path from the current position to the obstacle avoidance position based on the path planning instruction, samples the initial path, and takes the path sampling point as the initial position of the control point of the curve to be fitted, Based on the initial position, the distance between the control point and the obstacle, and the speed parameter of the mobile device at the control point, optimize the position of the control point, and determine the local target from the current position to the obstacle avoidance position according to the optimized position of the control point
  • the path because the better solution is obtained by optimizing the position of the control point, is the solution in the continuous space, the planned trajectory is optimal, and the position of the control point is used as the optimization variable, which reduces the optimization compared with the optimization-based method. parameters, reducing the time-consuming solution, so that real-time requirements can be met on low-cost computing units.
  • the description is relatively simple, and for related parts, please refer to the part of the description of the method embodiment.
  • the device embodiments described above are only illustrative, and the units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in One place, or it can be distributed to multiple network elements. Part or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. It can be understood and implemented by those skilled in the art without any creative efforts.
  • the various component embodiments of the present application may be realized in hardware, or in software modules running on one or more processors, or in a combination thereof.
  • a microprocessor or a digital signal processor (DSP) may be used in practice to implement some or all functions of some or all components in the mobile device according to the embodiments of the present application.
  • DSP digital signal processor
  • the present application can also be implemented as an apparatus or apparatus program (eg, computer program and computer program product) for performing a part or all of the methods described herein.
  • Such a program implementing the present application may be stored on a computer-readable medium, or may be in the form of one or more signals.
  • Such a signal may be downloaded from an Internet site, or provided on a carrier signal, or provided in any other form.
  • Figure 4 shows a mobile device that can implement a method according to the present application.
  • the mobile device conventionally includes a processor 410 and a computer program product in the form of a memory 420 or a computer-readable storage medium.
  • Memory 420 may be electronic memory such as flash memory, EEPROM (Electrically Erasable Programmable Read Only Memory), EPROM, hard disk, or ROM.
  • the memory 420 has a storage space 430 of a program code 431 for executing any method steps in the above mobile device obstacle bypassing method.
  • the storage space 430 for program codes may include respective program codes 431 for respectively implementing various steps in the above methods. These program codes can be read from or written into one or more computer program products.
  • Such computer program products include program code carriers such as hard disks, compact disks (CDs), memory cards or floppy disks.
  • Such a computer program product is typically a portable or fixed storage unit as described with reference to FIG. 5 .
  • the storage unit may have storage segments, storage spaces, etc. arranged similarly to the memory 420 in the mobile device of FIG. 4 .
  • the program code can eg be compressed in a suitable form.
  • the memory unit includes computer readable code 431', i.e. code readable by, for example, a processor such as 410, which when executed by the mobile device causes the mobile device to perform the mobile device bypassing described above. steps in the method.
  • embodiments of the embodiments of the present application may be provided as methods, devices, or computer program products. Therefore, the embodiment of the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, embodiments of the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) having computer-usable program code embodied therein.
  • computer-usable storage media including but not limited to disk storage, CD-ROM, optical storage, etc.
  • Embodiments of the present application are described with reference to flowcharts and/or block diagrams of methods, terminal devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each procedure and/or block in the flowchart and/or block diagram, and a combination of procedures and/or blocks in the flowchart and/or block diagram can be realized by computer program instructions. These computer program instructions may be provided to a general purpose computer, special purpose computer, embedded processor or processor of other programmable data processing terminal equipment to produce a machine such that instructions executed by the computer or processor of other programmable data processing terminal equipment Produce means for realizing the functions specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.
  • These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing terminal to operate in a specific manner, such that the instructions stored in the computer-readable memory produce an article of manufacture comprising instruction means, the The instruction means implements the functions specified in one or more procedures of the flowchart and/or one or more blocks of the block diagram.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Optics & Photonics (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种移动设备绕障方法、移动设备及存储介质，方法包括：基于路径规划指令，确定从当前位置到避障位置的初始路径，避障位置位于全局路径上（S101）；对初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置（S102）；基于初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化（S103）；根据优化得到的控制点的位置，确定当前位置到避障位置的局部目标路径（S104）。将控制点的位置作为优化变量，相对于基于优化的方法，减少了优化参数，减少了求解耗时，在低成本的计算单元上满足实时性的需求。

Description

移动设备绕障方法、移动设备及存储介质
本申请要求在2021年6月2日提交中国专利局、申请号为202110616400.5、发明名称为“移动设备绕障方法、装置、移动设备及存储介质”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请涉及控制技术领域，特别是涉及一种移动设备绕障方法、移动设备及存储介质。
背景技术
对于某一类移动设备，通常是沿着设定的路径执行相应的任务，如仓储机器人、酒店服务机器人等，但是当行进路径上存在临时障碍物时，移动设备就会停在障碍物前等待障碍物离开。一种更合适的做法是，在设定的路径周围进行绕障，当障碍物挡住行进道路时，移动设备可以灵活地绕过障碍物并且不与障碍物发生碰撞。
现有技术中，可以采用传统的路径搜索算法(如A*，Hybrid A*)，或者基于采样的算法(如动态窗口法DWA，快速搜索随机树RRT)来实现绕障，但此类方法是在离散空间进行的，而移动设备的控制空间和轨迹空间都是连续的，为了保证实时性，一般在比较粗粒度的分辨率下实现，因此规划出的轨迹并不是最优的。此外，也可以采用基于优化的方法(如时间弹性带算法TEB)来实现绕障，但该算法将轨迹参数化为离散的点集，因此待优化的变量维数较大，无法在低成本的计算单元上满足实时性的需求。
发明内容
鉴于上述问题，提出了本申请实施例以便提供一种克服上述问题或者至少部分地解决上述问题的一种移动设备绕障方法、移动设备及存储介质。
依据本申请实施例的第一方面，提供了一种移动设备绕障方法，包括：
基于路径规划指令，确定从当前位置到所述避障位置的初始路径，所述避障位置位于全局路径上；
对所述初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置；
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化；
根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径。
依据本申请实施例的第二方面，提供了一种移动设备绕障装置，包括：
初始路径确定模块，用于基于路径规划指令，确定从当前位置到所述避障位置的初始路径，所述避障位置位于全局路径上；
初始路径采样模块，用于对所述初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置；
控制点优化模块，用于基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化；
绕障路径确定模块，用于根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径。
依据本申请实施例的第三方面，提供了一种移动设备，包括：
存储器，其中存储有计算机可读代码；
一个或多个处理器，当所述计算机可读代码被所述一个或多个处理器执行时，所述移动设备执行如第一方面所述的移动设备绕障方法。
依据本申请实施例的第四方面，提供了一种计算机程序，包括计算机可读代码，当所述计算机可读代码在移动设备上运行时，导致所述移动设备执行第一方面所述的移动设备绕障方法。
依据本申请实施例的第五方面，提供了一种计算机可读存储介质，其中存储了如第四方面所述的计算机程序。
本申请实施例提供的移动设备绕障方法、移动设备及存储介质，通过基于路径规划指令确定从当前位置到避障位置的初始路径，对初始路径进行采样，将路径采样点作为待拟合曲线的控制点的初始位置，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径，由于通过对控制点的位置进行优化来获取更优的解，是在连续空间的求解，规划出的轨迹最优，而且将控制点的位置作为优化变量，相对于基于优化的方法，减少了优化参数，减少了求解耗时，从而可以在低成本的 计算单元上满足实时性的需求。
上述说明仅是本申请技术方案的概述，为了能够更清楚了解本申请的技术手段，而可依照说明书的内容予以实施，并且为了让本申请的上述和其它目的、特征和优点能够更明显易懂，以下特举本申请的具体实施方式。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本申请的限制。
图1是本申请实施例提供的一种移动设备绕障方法的步骤流程图；
图2是本申请实施例中的确定避障位置的示意图；
图3是本申请实施例提供的一种移动设备绕障装置的结构框图；
图4示意性地示出了用于执行根据本申请的方法的移动设备的框图；以及
图5示意性地示出了用于保持或者携带实现根据本申请的方法的程序代码的存储单元。
具体实施例
下面将参照附图更详细地描述本申请的示例性实施例。虽然附图中显示了本申请的示例性实施例，然而应当理解，可以以各种形式实现本申请而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本申请，并且能够将本申请的范围完整的传达给本领域的技术人员。
图1是本申请实施例提供的一种移动设备绕障方法的步骤流程图，如图1所示，该方法可以包括：
步骤101，基于路径规划指令，确定从当前位置到避障位置的初始路径，所述避障位置位于全局路径上。
其中，全局路径是移动设备(如机器人)执行某项任务时从初始的起点到目标终点的全局的路径。在移动设备行进过程中，全局路径中的某个位置有可能被障碍物遮挡，这就需要进行局部绕障。
在移动设备沿着上一路径规划周期的规划轨迹行进时，如果将要与障 碍物发生碰撞，或者移动设备偏离全局路径太远，则需要重新规划路径，确定接收到路径规划指令。基于路径规划指令，首先确定局部规划的终点，即避障位置，可以将全局路径上距离当前位置第一距离的一点，作为局部规划的避障位置，以根据当前位置和避障位置获得一条没有碰撞的绕障轨迹。例如，如图2所示，移动设备1在全局路径2上行进时，将会与第一障碍物3和第二障碍物4碰撞，这时确定全局路径2上距离当前位置第一距离的一点G’，作为局部规划的避障位置，以绕过第一障碍物3和第二障碍物4。其中，第一距离可以根据需要设定，例如可以是8米。
在确定避障位置后，可以通过路径搜索算法获得一个初始解，即可以基于导航地图，通过路径搜索算法搜索一条从当前位置到避障位置的初始路径。搜索初始路径时，有可能会搜索失败，这时可以按照全局路径进行行进，并在到达下一路径规划周期时再进行规划。其中，路径搜索算法可以为A*算法或者Hybrid A*算法。A*搜索是在离散的栅格地图上进行的，每个格子的边长一般是5cm。Hybrid A*是在离散的状态空间上进行的，比如方向盘分别为-10,0,10度并前进1米。
在本申请的一个实施例中，所述方法还包括：
在到达路径规划周期时，若当前位置偏离所述全局路径的距离大于第一距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离不小于第三距离阈值，则确定接收到路径规划指令。
移动设备中可以包括地图模块、移动设备位姿的定位模块、全局规划模块、绕障模块和控制模块，地图模块根据定位模块输出的移动设备前时刻的位姿、激光点云数据，对概率栅格地图进行更新，概率栅格地图用于表示每个栅格中存在障碍物的概率，根据概率栅格地图的概率可以获得指定位置的占据状态，占据状态可以包括存在障碍物、空闲或未知；地图模块再将概率栅格地图进行欧式距离转换(Euclidean Distance Transform，EDT)获得导航地图，导航地图中，每个栅格内的值表示该位置与最近障碍物的距离。绕障模块用于执行本申请实施例中的移动设备绕障方法。控制模块 用于控制移动设备按照规划的轨迹行进。
移动设备按照路径规划周期来对路径进行局部规划，在到达路径规划周期时，基于导航地图，判断当前位置偏离全局路径的距离是否大于第一距离阈值，如果是，则可能是由于控制等原因导致移动设备偏离全局路径太远，需要重新规划路径，确定接收到路径规划指令，如果不是，继续判断上一路径规划周期的规划轨迹是否将要与障碍物发生碰撞，如果不会与障碍物发生碰撞，即在上一路径规划周期的规划轨迹的行进方向上第一距离内没有障碍物，则不需要重新规划路径，如果将要与障碍物发生碰撞，即在上一路径规划周期的规划轨迹的行进方向上第一距离内有障碍物，则还需要判断行进方向上移动设备与障碍物的距离是否大于或等于第二距离阈值，或者预测得到的避障位置与障碍物的距离是否大于或等于第三距离阈值，如果在上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物的情况下，行进方向上移动设备与障碍物的距离大于或等于第二距离阈值，或者预测得到的避障位置与障碍物的距离大于或等于第三距离阈值，则确定需要重新规划路径，即接收到路径规划指令，如果在上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物的情况下，行进方向上移动设备与障碍物的距离小于第二距离阈值，或者避障位置与障碍物的距离小于第三距离阈值，则确定不需要重新规划路径，因为移动设备距离障碍物较近或者避障位置距离障碍物较近，即使重新规划也会失败，所以这时不需要重新规划路径，按照上一周期的规划轨迹行进距离障碍物较近时可以进行停障。
在本申请的一个实施例中，所述基于路径规划指令，确定从当前位置到所述避障位置的初始路径，包括：确定距离所述全局路径第二距离内的区域为搜索区域；在所述搜索区域内，通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
由于需要在全局路径附近进行绕障，首先需要构造搜索区域，即将全局路径第二预设距离内的区域作为搜索区域，如图2中两条虚线内的区域。在搜索区域内以当前位置为起点，通过路径搜索算法搜索一条到避障位置的路径，作为初始路径。其中，路径搜索算法可以采用Hybrid A*算法。通过在搜索区域内搜索初始路径，可以避免出现初始路径偏离全局路径太远的情况。
在本申请的一个实施例中，所述方法还包括：若所述搜索区域被障碍物完全遮挡，则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时，确定搜索失败。
若在行进方向上，搜索区域被障碍物完全遮挡，即行进路径被大型障碍物遮挡，则通过路径搜索算法搜索初始路径时会搜索失败，因为障碍物较大无法在搜索区域内实现绕障，这时不需要进行绕障，可以由停障模块负责停障。
步骤102，对所述初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置。
可以按照距离间隔或者时间间隔，对初始路径进行采样，将得到的路径采样点作为待拟合曲线的控制点的初始位置。其中，所述待拟合曲线可以是B样条曲线。
在本申请的一个实施例中，对所述初始路径进行采样，包括：计算在所述初始路径上行进所需的时间；按照指定时间间隔，对所述初始路径进行采样，得到所述初始路径上的路径采样点。
由于在非线性优化步骤中，优化变量是待拟合曲线的控制点，因此需要先将初始路径进行曲线的拟合，这时预估移动设备在初始路径上行进所需的时间，可以采用梯形规划预估得到，按照指定时间间隔对所述时间进行采样，得到多个时间采样点，预估移动设备在行进过程中到达每个时间采样点时移动设备所到达的路径采样点，得到时间采样点在初始路径上所对应的路径采样点。通过基于指定时间间隔来对初始路径进行采样获得路径采样点，将得到的路径采样点作为待拟合曲线的控制点的初始位置，可以获得合适的初始控制点，提高待拟合曲线拟合的速度。
待拟合曲线可以是B样条曲线，采用B样条曲线的优点有3点：第一，优化变量减少，采用B样条曲线的方式参数化，优化变量为n+1个控制点，优化变量数目为2*(n+1)，通常控制点的数量不会太多，对于8米的路径20～30个控制点即可，而基于离散点的优化，通常需要更多的点数(如100个，此时平均间隔为8cm)，优化参数越多，问题的规模越大，求解越耗时，从而B样条曲线相对于离散点的优化可以减少优化参数，提高求解速度；第二，B样条具有可微性，可以求得曲线上任意一点的位置、速度和加速度， 而离散点的优化方式只能通过有限差分获得，时间间隔越大越不精确；第三，B样条曲线具有凸包性质，可以简化边界约束的表示。
步骤103，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化。
其中，所述速度参量可以是加加速度，所述加加速度也可以称为急动度或力变率，是加速度的变化率。
基于初始位置以及待拟合曲线，对控制点的位置进行优化，以使得控制点与障碍物的距离较小，保证移动设备的安全性，并使得移动设备在控制点处的速度参量较小，保证待拟合曲线的平滑性。
在得到待拟合的待拟合曲线的初始位置后，通过最小二乘法拟合一条准均匀的B样条曲线：
Figure PCTCN2022088575-appb-000001
其中，n+1为控制点的数量，Q i为第i个控制点的位置，p为B样条曲线的次数，N i,p(t)为p次B样条曲线基函数。
以目标函数和约束条件构建非线性优化问题，将控制点的初始位置作为非线性优化问题的输入，对控制点的位置进行优化，以使得由控制点确定的B样条曲线能够较为平滑并且安全。需要注意的是，在求解时需要增大起点(即当前位置)和终点(即避障位置)的权重，保证该B样条曲线能穿过起点和终点。在求解时，优化变量为：[Q p,Q p+1,…,Q n-p]，两端的控制点不参与优化，这是为了保证起点和终点的位置等于设定的值。在求解该非线性优化问题时，可能会求解失败，这时可以等待下一路径规划周期再进行路径规划。
在本申请的一个实施例中，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，包括：基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
在基于初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，来对控制点的位置进行优化，具体的，将初始位置代入待拟合曲线的方程，得到初始位置对应的待拟合 曲线，确定初始位置对应的待拟合曲线中的当前位置与避障位置之间的轨迹，在该轨迹中确定控制点与障碍物的距离以及移动设备在控制点处的速度参量，基于控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行调整，使得控制点与障碍物的距离以及移动设备在控制点处的速度参量减小，迭代执行上述将控制点的位置代入待拟合曲线的方程，得到控制点的位置对应的待拟合曲线，并对控制点的位置进行调整的操作，直至控制点与障碍物的距离和移动设备在控制点处的速度参量达到最小值，即控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，结束迭代调整，得到优化后控制点的位置。
在本申请的一个实施例中，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，包括：基于所述初始位置和控制点与障碍物的距离，构建安全性约束函数；基于所述初始位置和移动设备在控制点处的速度参量，构建平滑项约束函数；根据所述安全性约束函数、所述平滑项约束函数，对控制点的位置进行迭代调整，直至所述安全性约束函数和所述平滑项约束函数收敛。
通过安全性约束函数保证优化后的轨迹上没有碰撞，通过平滑项约束函数保证优化后的轨迹平滑，在构建安全性约束函数和平滑项约束函数后，可以对控制点的位置进行迭代调整，即将控制点的位置代入待拟合曲线的方程，得到从当前位置到避障位置的轨迹，基于该路径来确定安全性约束函数值和平滑项约束函数值，基于安全性约束函数值和平滑项约束函数值，对控制点的位置进行调整，使得安全性约束函数值和平滑项约束函数值减小，迭代执行上述将控制点的位置代入待拟合曲线的方程以及对控制点的位置进行调整的操作，直至安全性约束函数值和平滑项约束函数值达到最小，即安全性约束函数值和平滑项约束函数值收敛，结束迭代过程。
在本申请的一个实施例中，所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，还包括：基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度，构建速度约束函数； 根据所述速度约束函数，对控制点的位置进行迭代调整，直至所述速度约束函数收敛。
在对控制点的位置进行迭代调整的过程中，还可以保证在迭代过程中的轨迹上的速度满足要求，这时可以基于初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度，构建速度约束函数，并基于安全性约束函数、平滑项约束函数和速度约束函数，对控制点的位置进行迭代调整，直至安全性约束函数、平滑项约束函数和速度约束函数收敛，结束迭代操作，这样可以保证移动设备的安全性、轨迹的平滑性以及在优化后的轨迹上的速度和加速度满足要求。
在本申请的一个实施例中，所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，还包括：基于所述初始位置和迭代过程中待拟合曲线的曲率，构建曲率约束函数；基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度，构建角度约束函数；根据所述曲率约束函数、角度约束函数，对控制点的位置进行迭代调整，直至所述曲率约束函数、角度约束函数收敛。
在对控制点的位置进行迭代调整的过程中，还可以保证在迭代过程中的轨迹上的曲率和角度满足要求，这时可以基于初始位置和迭代过程中待拟合曲线的曲率，构建曲率约束函数，并基于初始位置和迭代过程中待拟合曲线的起点角度和终点角度构建角度约束函数，并基于安全性约束函数、平滑项约束函数、速度约束函数、曲率约束函数和角度约束函数，对控制点的位置进行迭代调整，直至安全性约束函数、平滑项约束函数、速度约束函数、曲率约束函数和角度约束函数收敛，结束迭代操作，这样可以保证移动设备的安全性、轨迹的平滑性、在优化后的轨迹上的速度和加速度满足要求、曲率和角度满足要求。
在对控制点的位置进行优化的过程中，优化参数除了控制点与障碍物的距离以及移动设备在控制点处的速度参量外，还可以包括移动设备在优化后轨迹上的速度、移动设备在优化后轨迹上的加速度、优化后轨迹的曲率、优化后轨迹的起点角度和终点角度，从而可以在保证安全和平滑的基础上，使得移动设备在优化后的轨迹上行进时速度、加速度、曲率、起点 角度和终点角度均能满足要求，进一步提高安全性。
在本申请的一个实施例中，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，包括：对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权，确定所述目标函数；基于所述目标函数，对控制点的位置进行迭代调整，直至所述目标函数收敛。
其中，所述目标函数表示如下：
f total＝ω 1F s+ω 2F c+ω 3(F v+F a)+ω 4F k+ω 5F θ
其中，f total为目标函数，F s为平滑项约束函数，F c为安全性约束函数，F v为速度约束函数，F a为加速度约束函数，F k为曲率约束函数，F θ为角度约束函数，ω 1为平滑项约束函数的权重，ω 2为安全性约束函数的权重，ω 3为速度约束函数和加速度约束函数的权重，ω 4为曲率约束函数的权重，ω 5为角度约束函数的权重。
通过平滑项约束函数保证优化后的轨迹平滑，通过安全性约束函数保证优化后的轨迹上没有碰撞，通过速度约束函数和加速度约束函数，保证优化后的轨迹上的速度和加速度在设定范围内，通过曲率约束函数保证优化后的轨迹的曲率在设定范围内，通过角度约束函数的约束保证优化后的轨迹的起点和终点的方向尽可能不变。在对控制点的位置进行优化时，确定使得目标函数的值最小时的控制点的位置。
在上述目标函数中，所述平滑项约束函数表示如下：
F s(i)＝((Q i+3-2Q i+2+Q i+1)-(Q i+2-2Q i+1+Q i)) 2
其中，Q i表示第i个控制点，F s(i)表示在第i个控制点处的速度参量的平方和；
所述安全性约束函数表示如下：
Figure PCTCN2022088575-appb-000002
其中，F c(d(Q i))表示控制点与障碍物的距离的平方和，d(Q i)为第i个控制点与障碍物的距离，d thr表示发生碰撞时移动设备与障碍物的最小距离；
所述速度约束函数表示如下：
Figure PCTCN2022088575-appb-000003
其中，F v(v μ)表示速度约束函数，v μ表示迭代过程中待拟合曲线上的速度，v max表示速度阈值；
所述加速度约束函数表示如下：
Figure PCTCN2022088575-appb-000004
其中，F a(a μ)表示加速度约束函数，a μ表示迭代过程中待拟合曲线上的加速度，a max表示加速度阈值。
平滑项约束函数通过速度参量的平方和来表示。安全性约束函数通过控制点与障碍物的距离的平方和来表示，d(Q i)可以从导航地图中直接获得。在速度约束函数和加速度约束函数中，速度和加速度可以由控制点差分获得。曲率约束函数用于约束优化后的轨迹的曲率在设定曲率范围内，曲率可以由控制点计算得到。角度约束函数用于约束优化后的轨迹的起点和终点的方向尽可能不变，起点角度和终点角度可以由控制点计算得到。
在构建上述非线性优化问题后，可以采用开源第三方库ipopt或nlopt对上述控制点的位置进行优化求解。
在本申请的一个实施例中，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，包括：
基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，对控制点的位置进行迭代调整，所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
在基于初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，对控制点的位置进行迭代调整，来对控制点的位置进行优化，具体的，将初始位置代入待拟合曲线的方程， 得到初始位置对应的待拟合曲线，确定初始位置对应的待拟合曲线中的当前位置与避障位置之间的轨迹，在该轨迹中确定控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，基于控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，对控制点的位置进行调整，使得控制点与障碍物的距离、移动设备在控制点处的速度参量减小和迭代过程中待拟合曲线的参数减小，迭代执行上述将控制点的位置代入待拟合曲线的方程，得到控制点的位置对应的待拟合曲线，并对控制点的位置进行调整的操作，直至控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数达到最小值，即控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数收敛，结束迭代调整，得到优化后控制点的位置。所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个，这样可以保证优化后的轨迹满足要求，提高安全性。
步骤104，根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径。
在对控制点的位置进行优化后，将优化得到的控制点的位置代入待拟合曲线方程，得到待拟合曲线，并从该待拟合曲线中截取从当前位置到避障位置的曲线段，得到从当前位置到避障位置的局部目标路径。如待拟合曲线为B样条曲线时，将优化后的控制点的位置代入B样条曲线方程，可以得到B样条曲线，从B样条曲线中截取从当前位置到避障位置的曲线段，得到从当前位置到避障位置的局部目标路径。
其中，所述根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径，包括：
根据优化得到的控制点的位置，按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径：
Figure PCTCN2022088575-appb-000005
其中，n+1为控制点的数量，Q i为第i个控制点的位置，p为B样条曲 线的次数，N i,p(t)为p次B样条曲线基函数。
本申请实施例中，可以采用三次B样条曲线来进行求解，即p为3。
本实施例提供的移动设备绕障方法，通过基于路径规划指令确定从当前位置到避障位置的初始路径，对初始路径进行采样，将路径采样点作为待拟合曲线的控制点的初始位置，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径，由于通过对控制点的位置进行优化来获取更优的解，是在连续空间的求解，规划出的轨迹最优，而且将控制点的位置作为优化变量，相对于基于优化的方法，减少了优化参数，减少了求解耗时，从而可以在低成本的计算单元上满足实时性的需求。
在上述技术方案的基础上，在所述确定所述当前位置到所述避障位置的局部目标路径之后，还包括：
若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞，且所述局部目标路径满足角度约束条件和曲率约束条件，则输出所述局部目标路径；
否则，输出所述全局路径。
其中，角度约束条件是预先设置的起点角度和终点角度需满足的条件，曲率约束条件是预先设置的运动轨迹的曲率需满足的条件。
在确定当前位置到避障位置的局部目标路径后，对该局部目标路径进行校验，校验若移动设备沿着局部目标路径行进时是否会与障碍物发生碰撞，并且校验局部目标路径是否满足角度约束条件和曲率约束条件，若移动设备沿着局部目标路径行进时不会与障碍物发生碰撞，而且局部目标路径满足角度约束条件和曲率约束条件，则可以将局部目标路径输出至控制模块，由控制模块控制移动设备按照局部目标路径行进，以绕过障碍物行进。经过校验，若移动设备沿着局部目标路径行进时会与障碍物发生碰撞，或者局部目标路径不满足角度约束条件和曲率约束条件，则输出全局路径至控制模块，由控制模块控制移动设备按照全局路径行进。通过路径校验，保证移动设备沿着较优的路径行进，可以保证移动设备的安全性。
在上述技术方案的基础上，所述方法还包括：
若在所述全局路径上行进时与障碍物的距离小于第四距离阈值，则停止行进。
在上述绕障轨迹规划失败或者距离障碍物较近无法进行路径重新规划时，移动设备需要在全局路径上行进，这时移动设备在行进的过程中，实时确定移动设备与障碍物的距离，如果距离小于第四距离阈值，即移动设备即将要与障碍物发生碰撞，则停止行进，可以避免与障碍物发生碰撞。
需要说明的是，对于方法实施例，为了简单描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本申请实施例并不受所描述的动作顺序的限制，因为依据本申请实施例，某些步骤可以采用其他顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作并不一定是本申请实施例所必须的。
图3是本申请实施例提供的一种移动设备绕障装置的结构框图，如图3所示，该移动设备绕障装置可以包括：
初始路径确定模块301，用于基于路径规划指令，确定从当前位置到所述避障位置的初始路径，所述避障位置位于全局路径上；
初始路径采样模块302，用于对所述初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置；
控制点优化模块303，用于基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化；
绕障路径确定模块304，用于根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径。
可选的，所述控制点优化模块具体用于：
基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
可选的，所述控制点优化模块包括：
安全性约束构建单元，用于基于所述初始位置和控制点与障碍物的距离，构建安全性约束函数；
平滑项约束构建单元，用于基于所述初始位置和移动设备在控制点处 的速度参量，构建平滑项约束函数；
迭代调整单元，用于根据所述安全性约束函数、所述平滑项约束函数，对控制点的位置进行迭代调整，直至所述安全性约束函数和所述平滑项约束函数收敛。
可选的，所述控制点优化模块还包括：
速度约束构建单元，用于基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度，构建速度约束函数；
所述迭代调整单元，还用于根据所述速度约束函数，对控制点的位置进行迭代调整，直至所述速度约束函数收敛。
可选的，所述控制点优化模块还包括：
曲率约束构建单元，用于基于所述初始位置和迭代过程中待拟合曲线的曲率，构建曲率约束函数；
角度约束构建单元，用于基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度，构建角度约束函数；
所述迭代调整单元，还用于根据所述曲率约束函数、角度约束函数，对控制点的位置进行迭代调整，直至所述曲率约束函数、角度约束函数收敛。
可选的，所述控制点优化模块包括：
目标函数确定单元，用于对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权，确定所述目标函数；
迭代优化单元，用于基于所述目标函数，对控制点的位置进行迭代调整，直至所述目标函数收敛。
可选的，所述平滑项约束函数表示如下：
F s(i)＝((Q i+3-2Q i+2+Q i+1)-(Q i+2-2Q i+1+Q i)) 2
其中，Q i表示第i个控制点，F s(i)表示在第i个控制点处的速度参量的平方和；
所述安全性约束函数表示如下：
Figure PCTCN2022088575-appb-000006
其中，F c(d(Q i))表示控制点与障碍物的距离的平方和，d(Q i)为第i个控制点与障碍物的距离，d thr表示发生碰撞时移动设备与障碍物的最小距离；
所述速度约束函数表示如下：
Figure PCTCN2022088575-appb-000007
其中，F v(v μ)表示速度约束函数，v μ表示迭代过程中待拟合曲线上的速度，v max表示速度阈值；
所述加速度约束函数表示如下：
Figure PCTCN2022088575-appb-000008
其中，F a(a μ)表示加速度约束函数，a μ表示迭代过程中待拟合曲线上的加速度，a max表示加速度阈值。
可选的，所述控制点优化模块具体用于：
基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，对控制点的位置进行迭代调整，所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
可选的，所述初始路径采样模块包括：
时间预估单元，用于计算在所述初始路径上行进所需的时间；
采样单元，用于按照指定时间间隔，对所述初始路径进行采样，得到所述初始路径上的路径采样点。
可选的，所述装置还包括：
重新规划判断模块，用于在到达路径规划周期时，若当前位置偏离所述全局路径的距离大于第一距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离大于或等于第三距离 阈值，则确定接收到路径规划指令。
可选的，所述初始路径确定模块包括：
搜索区域确定单元，用于确定距离所述全局路径第二距离内的区域为搜索区域；
初始路径搜索单元，用于在所述搜索区域内，通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
可选的，所述装置还包括：
搜索失败确定模块，用于若所述搜索区域被障碍物完全遮挡，则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时，确定搜索失败。
可选的，所述绕障路径确定模块具体用于：
根据优化得到的控制点的位置，按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径：
Figure PCTCN2022088575-appb-000009
其中，n+1为控制点的数量，Q i为第i个控制点的位置，p为B样条曲线的次数，N i,p(t)为p次B样条曲线基函数。
可选的，所述装置还包括：
路径输出模块，用于若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞，且所述局部目标路径满足角度约束条件和曲率约束条件，则输出所述局部目标路径；否则，输出所述全局路径。
可选的，所述装置还包括：
停障模块，用于若在所述全局路径上行进时与障碍物的距离小于第四距离阈值，则停止行进。
本实施例提供的移动设备绕障装置，通过基于路径规划指令确定从当前位置到避障位置的初始路径，对初始路径进行采样，将路径采样点作为待拟合曲线的控制点的初始位置，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，根据优化得到的控制点的位置确定当前位置到避障位置的局部目标路径，由于通过优化控制点的位置来获取更优的解，是在连续空间的求解，规划出的轨迹最优，而且将控制点的位置作为优化变量，相对于基于优化的方法， 减少了优化参数，减少了求解耗时，从而可以在低成本的计算单元上满足实时性的需求。
对于装置实施例而言，由于其与方法实施例基本相似，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
本申请的各个部件实施例可以以硬件实现，或者以在一个或者多个处理器上运行的软件模块实现，或者以它们的组合实现。本领域的技术人员应当理解，可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本申请实施例的移动设备中的一些或者全部部件的一些或者全部功能。本申请还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如，计算机程序和计算机程序产品)。这样的实现本申请的程序可以存储在计算机可读介质上，或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到，或者在载体信号上提供，或者以任何其他形式提供。
例如，图4示出了可以实现根据本申请的方法的移动设备。该移动设备传统上包括处理器410和以存储器420形式的计算机程序产品或者计算机可读存储介质。存储器420可以是诸如闪存、EEPROM(电可擦除可编程只读存储器)、EPROM、硬盘或者ROM之类的电子存储器。存储器420具有用于执行上述移动设备绕障方法中的任何方法步骤的程序代码431的存储空间430。例如，用于程序代码的存储空间430可以包括分别用于实现上面的方法中的各种步骤的各个程序代码431。这些程序代码可以从一个或者多个计算机程序产品中读出或者写入到这一个或者多个计算机程序产品中。这些计算机程序产品包括诸如硬盘，紧致盘(CD)、存储卡或者软盘之类的程序代码载体。这样的计算机程序产品通常为如参考图5所述的便携式或者固定存储单元。该存储单元可以具有与图4的移动设备中的存储器420类似布 置的存储段、存储空间等。程序代码可以例如以适当形式进行压缩。通常，存储单元包括计算机可读代码431’，即可以由例如诸如410之类的处理器读取的代码，这些代码当由移动设备运行时，导致该移动设备执行上面所描述的移动设备绕障方法中的各个步骤。
本说明书中的各个实施例均采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似的部分互相参见即可。
本领域内的技术人员应明白，本申请实施例的实施例可提供为方法、装置、或计算机程序产品。因此，本申请实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请实施例是参照根据本申请实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
尽管已描述了本申请实施例的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请实施例范围的所有变更和修改。
最后，还需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。
以上对本申请所提供的一种移动设备绕障方法、移动设备及存储介质，进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (18)

  1. 一种移动设备绕障方法，其特征在于，包括：
    在满足路径规划条件时，确定从当前位置到避障位置的初始路径，所述避障位置位于全局路径上；
    对所述初始路径进行采样，并将路径采样点作为待拟合曲线的控制点的初始位置；
    基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化；
    根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径。
  2. 根据权利要求1所述的方法，其特征在于，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，包括：
    基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛。
  3. 根据权利要求2所述的方法，其特征在于，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，包括：
    基于所述初始位置和控制点与障碍物的距离，构建安全性约束函数；
    基于所述初始位置和移动设备在控制点处的速度参量，构建平滑项约束函数；
    根据所述安全性约束函数、所述平滑项约束函数，对控制点的位置进行迭代调整，直至所述安全性约束函数和所述平滑项约束函数收敛。
  4. 根据权利要求2-3任一项所述的方法，其特征在于，所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，还包括：
    基于所述初始位置、移动设备在迭代过程中待拟合曲线上的速度以及移动设备在迭代过程中待拟合曲线上的加速度，构建速度约束函数；
    根据所述速度约束函数，对控制点的位置进行迭代调整，直至所述速度约束函数收敛。
  5. 根据权利要求4所述的方法，其特征在于，所述基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，还包括：
    基于所述初始位置和迭代过程中待拟合曲线的曲率，构建曲率约束函数；
    基于所述初始位置和迭代过程中待拟合曲线的起点角度和终点角度，构建角度约束函数；
    根据所述曲率约束函数、角度约束函数，对控制点的位置进行迭代调整，直至所述曲率约束函数、角度约束函数收敛。
  6. 根据权利要求5所述的方法，其特征在于，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行迭代调整，直至控制点与障碍物的距离和移动设备在控制点处的速度参量收敛，包括：
    对所述安全性约束函数、平滑项约束函数、速度约束函数、加速度约束函数、曲率约束函数和角度约束函数进行加权，确定所述目标函数；
    基于所述目标函数，对控制点的位置进行迭代调整，直至所述目标函数收敛。
  7. 根据权利要求6所述的方法，其特征在于，所述平滑项约束函数表示如下：
    F s(i)＝((Q i+3-2Q i+2+Q i+1)-(Q i+2-2Q i+1+Q i)) 2
    其中，Q i表示第i个控制点，F s(i)表示在第i个控制点处的速度参量的平方和；
    所述安全性约束函数表示如下：
    Figure PCTCN2022088575-appb-100001
    其中，F c(d(Q i))表示控制点与障碍物的距离的平方和，d(Q i)为第i个控制点与障碍物的距离，d thr表示发生碰撞时移动设备与障碍物的最小距离；
    所述速度约束函数表示如下：
    Figure PCTCN2022088575-appb-100002
    其中，F v(v μ)表示速度约束函数，v μ表示迭代过程中待拟合曲线上的速度，v max表示速度阈值；
    所述加速度约束函数表示如下：
    Figure PCTCN2022088575-appb-100003
    其中，F a(a μ)表示加速度约束函数，a μ表示迭代过程中待拟合曲线上的加速度，a max表示加速度阈值。
  8. 根据权利要求1-7任一项所述的方法，其特征在于，基于所述初始位置、控制点与障碍物的距离以及移动设备在控制点处的速度参量，对控制点的位置进行优化，包括：
    基于所述初始位置、控制点与障碍物的距离、移动设备在控制点处的速度参量和迭代过程中待拟合曲线的参数，对控制点的位置进行迭代调整，所述迭代过程中待拟合曲线的参数包括所述移动设备在迭代过程中待拟合曲线上的速度、所移动设备在迭代过程中待拟合曲线上的加速度、迭代过程中待拟合曲线的曲率、迭代过程中待拟合曲线的起点角度和终点角度中的一个或多个。
  9. 根据权利要求1-8任一项所述的方法，其特征在于，对所述初始路径进行采样，包括：
    计算在所述初始路径上行进所需的时间；
    按照指定时间间隔对所述时间进行采样，得到多个时间采样点；
    预估移动设备在所述初始路径上行进过程中到达每个所述时间采样点时所述移动设备所到达的路径采样点，得到所述初始路径上的路径采样点。
  10. 根据权利要求1-8任一项所述的方法，其特征在于，所述路径 规划条件的判断方式包括：
    在到达路径规划周期时，若当前位置偏离所述全局路径的距离大于第一距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上第一距离内有障碍物且距离障碍物的距离大于或等于第二距离阈值，或者，上一路径规划周期的规划轨迹在行进方向上所述第一距离内有障碍物且预测得到的临时终点与障碍物的距离大于或等于第三距离阈值，则确定满足路径规划条件。
  11. 根据权利要求1-8任一项所述的方法，其特征在于，所述基于路径规划指令，确定从当前位置到所述避障位置的初始路径，包括：
    确定距离所述全局路径第二距离内的区域为搜索区域；
    在所述搜索区域内，通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径。
  12. 根据权利要求11所述的方法，其特征在于，还包括：
    若所述搜索区域被障碍物完全遮挡，则在通过路径搜索算法搜索从所述当前位置到所述避障位置的初始路径时，确定搜索失败。
  13. 根据权利要求1-8任一项所述的方法，其特征在于，所述根据优化得到的控制点的位置，确定所述当前位置到所述避障位置的局部目标路径，包括：
    根据优化得到的控制点的位置，按照如下B样条曲线确定所述当前位置到所述避障位置的局部目标路径：
    Figure PCTCN2022088575-appb-100004
    其中，n+1为控制点的数量，Q i为第i个控制点的位置，p为B样条曲线的次数，N i,p(t)为p次B样条曲线基函数。
  14. 根据权利要求1-8任一项所述的方法，其特征在于，在所述确定所述当前位置到所述避障位置的局部目标路径之后，还包括：
    若移动设备在沿着所述局部目标路径行进时不会与障碍物发生碰撞，且所述局部目标路径满足角度约束条件和曲率约束条件，则输出所述局部目标路径；
    否则，输出所述全局路径。
  15. 根据权利要求1-8任一项所述的方法，其特征在于，还包括：
    若在所述全局路径上行进时与障碍物的距离小于第四距离阈值， 则停止行进。
  16. 一种移动设备，其特征在于，包括：
    存储器，其中存储有计算机可读代码；
    一个或多个处理器，当所述计算机可读代码被所述一个或多个处理器执行时，所述移动设备执行如权利要求1-15中任一项所述的移动设备绕障方法。
  17. 一种计算机程序，包括计算机可读代码，其特征在于，当所述计算机可读代码在移动设备上运行时，导致所述移动设备执行根据权利要求1-15中任一项所述的移动设备绕障方法。
  18. 一种计算机可读存储介质，其特征在于，其中存储了如权利要求17所述的计算机程序。
PCT/CN2022/088575 2021-06-02 2022-04-22 移动设备绕障方法、移动设备及存储介质 WO2022252869A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110616400.5 2021-06-02
CN202110616400.5A CN113433936B (zh) 2021-06-02 2021-06-02 移动设备绕障方法、装置、移动设备及存储介质

Publications (1)

Publication Number Publication Date
WO2022252869A1 true WO2022252869A1 (zh) 2022-12-08

Family

ID=77803555

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/088575 WO2022252869A1 (zh) 2021-06-02 2022-04-22 移动设备绕障方法、移动设备及存储介质

Country Status (2)

Country Link
CN (1) CN113433936B (zh)
WO (1) WO2022252869A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116858254A (zh) * 2023-09-01 2023-10-10 青岛中德智能技术研究院 单舵轮agv路径规划方法
CN117472066A (zh) * 2023-12-27 2024-01-30 成都流体动力创新中心 一种航向角速度局部最优的避障控制方法
CN118081743A (zh) * 2024-02-29 2024-05-28 哈尔滨工业大学 一种考虑多维度轨迹样本的机械臂多约束轨迹规划方法

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113433936B (zh) * 2021-06-02 2023-07-25 北京迈格威科技有限公司 移动设备绕障方法、装置、移动设备及存储介质
CN113868818B (zh) * 2021-10-13 2024-04-02 桂林电子科技大学 一种区域数据点栅格化区块大小确定方法及系统
CN115268457A (zh) * 2022-08-10 2022-11-01 美的集团(上海)有限公司 避障机器人及其控制方法、控制装置和可读存储介质
CN118707936A (zh) * 2023-03-23 2024-09-27 珠海一微半导体股份有限公司 基于地图概率信息的机器人绕障控制方法
CN118464028B (zh) * 2024-07-15 2024-09-06 龙门实验室 一种基于分治策略的农机路径规划方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106681335A (zh) * 2017-01-22 2017-05-17 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的避障路径规划及其控制方法
KR20170070488A (ko) * 2015-12-14 2017-06-22 현대모비스 주식회사 주행경로 자동생성방법 및 장치
CN109542106A (zh) * 2019-01-04 2019-03-29 电子科技大学 一种移动机器人多约束条件下的路径规划方法
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN110347151A (zh) * 2019-05-31 2019-10-18 河南科技大学 一种融合Bezier优化遗传算法的机器人路径规划方法
CN110865642A (zh) * 2019-11-06 2020-03-06 天津大学 一种基于移动机器人的路径规划方法
CN111679678A (zh) * 2020-06-30 2020-09-18 安徽海博智能科技有限责任公司 一种横纵向分离的轨迹规划方法、系统及计算机设备
CN112506176A (zh) * 2019-08-26 2021-03-16 上海汽车集团股份有限公司 路径规划方法及装置
CN113433936A (zh) * 2021-06-02 2021-09-24 北京迈格威科技有限公司 移动设备绕障方法、装置、移动设备及存储介质

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110388920A (zh) * 2018-04-15 2019-10-29 张舒怡 机器人的路径规划
CN110488802B (zh) * 2019-08-21 2020-05-12 清华大学 一种网联环境下的自动驾驶车辆动态行为决策方法
CN111694364A (zh) * 2020-06-30 2020-09-22 山东交通学院 一种应用于智能车路径规划的基于改进蚁群算法与动态窗口法的混合算法
CN111813117B (zh) * 2020-07-09 2023-09-01 北京布科思科技有限公司 一种机器人巡线优先导航方法、装置和设备
CN112352530B (zh) * 2020-10-27 2022-05-31 懿力创新(厦门)科技有限公司 一种自动除草机器人的工作路径优化方法
CN112432648A (zh) * 2020-11-17 2021-03-02 中山大学 一种移动型机器人安全运动轨迹的实时规划方法
CN112462785B (zh) * 2020-12-04 2022-06-03 厦门大学 一种移动机器人路径规划方法、装置及存储介质
CN112857385B (zh) * 2021-01-18 2022-09-27 北京理工大学 一种基于非均匀栅格模型的快速无人车局部路径规划方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20170070488A (ko) * 2015-12-14 2017-06-22 현대모비스 주식회사 주행경로 자동생성방법 및 장치
CN106681335A (zh) * 2017-01-22 2017-05-17 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的避障路径规划及其控制方法
CN109542106A (zh) * 2019-01-04 2019-03-29 电子科技大学 一种移动机器人多约束条件下的路径规划方法
CN109799822A (zh) * 2019-01-30 2019-05-24 中国石油大学(华东) 移动机器人全局平滑路径规划方法
CN110347151A (zh) * 2019-05-31 2019-10-18 河南科技大学 一种融合Bezier优化遗传算法的机器人路径规划方法
CN112506176A (zh) * 2019-08-26 2021-03-16 上海汽车集团股份有限公司 路径规划方法及装置
CN110865642A (zh) * 2019-11-06 2020-03-06 天津大学 一种基于移动机器人的路径规划方法
CN111679678A (zh) * 2020-06-30 2020-09-18 安徽海博智能科技有限责任公司 一种横纵向分离的轨迹规划方法、系统及计算机设备
CN113433936A (zh) * 2021-06-02 2021-09-24 北京迈格威科技有限公司 移动设备绕障方法、装置、移动设备及存储介质

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116858254A (zh) * 2023-09-01 2023-10-10 青岛中德智能技术研究院 单舵轮agv路径规划方法
CN117472066A (zh) * 2023-12-27 2024-01-30 成都流体动力创新中心 一种航向角速度局部最优的避障控制方法
CN117472066B (zh) * 2023-12-27 2024-03-26 成都流体动力创新中心 一种航向角速度局部最优的避障控制方法
CN118081743A (zh) * 2024-02-29 2024-05-28 哈尔滨工业大学 一种考虑多维度轨迹样本的机械臂多约束轨迹规划方法

Also Published As

Publication number Publication date
CN113433936B (zh) 2023-07-25
CN113433936A (zh) 2021-09-24

Similar Documents

Publication Publication Date Title
WO2022252869A1 (zh) 移动设备绕障方法、移动设备及存储介质
CN110260867B (zh) 一种机器人导航中位姿确定、纠正的方法、设备及装置
WO2022056770A1 (zh) 一种路径规划方法和路径规划装置
CN113376650B (zh) 移动机器人定位方法及装置、电子设备及存储介质
WO2020011025A1 (zh) 一种车辆的自动换道方法及装置
Kim et al. Real-time path planning with limited information for autonomous unmanned air vehicles
US20220107647A1 (en) Speed planning method and apparatus, electronic device and storage medium
Petrich et al. Map-based long term motion prediction for vehicles in traffic environments
KR101585504B1 (ko) 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치
CN108052102B (zh) 机器人行进路线的确定方法、装置及机器人
CN108021136A (zh) 机器人沿行进路线行进的控制方法、装置及机器人
WO2023024539A1 (zh) 路径导航规划方法、装置、存储介质及电子设备
CN111121812A (zh) 一种路径优化方法、电子设备及存储介质
CN113867347A (zh) 机器人路径规划方法、装置、管理系统及计算机存储介质
WO2023036044A1 (zh) 全局路径规划方法、运动控制方法及计算机程序产品
CN111522335B (zh) 基于改进粒子群算法的机器人路径优化方法及系统
US11383704B2 (en) Enhanced vehicle operation
JPWO2020105189A1 (ja) 経路計画装置、経路計画方法、及びプログラム
JP7235060B2 (ja) 経路計画装置、経路計画方法、及びプログラム
CN111427344A (zh) 自主体轨迹冲突的解决方法、装置、设备和存储介质
CN112711252B (zh) 移动机器人及其路径跟踪方法、计算机可读存储介质
Hassani et al. Control points searching algorithm for autonomous mobile robot navigation with obstacle avoidance
Kang et al. Two-stage iterative finite-memory neural network identification for unmanned aerial vehicles
WO2020250101A1 (en) Methods and systems for path planning in a known environment
CN118226860B (zh) 机器人运动控制方法、装置、机器人和存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22814916

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 28/03/2024)