WO2022152283A1 - Local path planning method for unmanned vehicle - Google Patents

Local path planning method for unmanned vehicle Download PDF

Info

Publication number
WO2022152283A1
WO2022152283A1 PCT/CN2022/072247 CN2022072247W WO2022152283A1 WO 2022152283 A1 WO2022152283 A1 WO 2022152283A1 CN 2022072247 W CN2022072247 W CN 2022072247W WO 2022152283 A1 WO2022152283 A1 WO 2022152283A1
Authority
WO
WIPO (PCT)
Prior art keywords
point
vehicle
planning
obstacle
target
Prior art date
Application number
PCT/CN2022/072247
Other languages
French (fr)
Chinese (zh)
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 WO2022152283A1 publication Critical patent/WO2022152283A1/en

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/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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Definitions

  • the invention relates to the field of path planning, in particular to a local path planning method for unmanned vehicles integrating TangentBug and Dubins curves.
  • the path planning of intelligent vehicles refers to planning an effective path without collision and safely reaching the target point based on a certain environment model and given the starting point and the target point according to the performance index.
  • path planning can be divided into graph search algorithm, sampling algorithm, potential field method and intelligent bionic algorithm according to different algorithms.
  • Graph search and sampling algorithms are the most common and fast, but require path smoothing to adapt to the motion characteristics of the vehicle, and are often used when the global map is known.
  • the potential field method has good adaptability to obstacles, but it is easy to fall into the local optimal solution.
  • Intelligent bionic algorithms refer to the behavior patterns and characteristics of living beings, and can obtain better results than other algorithms.
  • Lumelsky et al. refer to the behavior patterns of crawler (Bug), and for the situation where only local maps and positioning information can be obtained, they propose Bug1 and Bug2 algorithms.
  • Bug1 and Bug2 are aimed at robots that can move in all directions, and solve their local path planning problems on a two-dimensional plane, requiring robots to be equipped with contact sensors to detect surrounding obstacles and have accurate real-time positioning.
  • Both Bug1 and Bug2 consist of two states of moving towards the target and surrounding obstacles, but the total length of the planned path in Bug2 may be shorter in simple scenarios, while Bug1 is more robust in complex scenarios.
  • the Alg1 algorithm with a shorter total length of the planned path in some scenarios is derived, the TangentBug algorithm using the ranging sensor, the FuzzyBug algorithm based on fuzzy logic, the InsertBug algorithm using vectorized paths, and so on. Because the TangentBug algorithm solves the problem that the bug algorithm must contact the obstacle through the ranging sensor and the local tangent graph, it has been widely used in practice.
  • the existing local path planning algorithms based on TangentBug algorithm still have the following shortcomings:
  • the size of the controlled object is not fully considered.
  • the offset distance of the obstacle boundary points in the local tangent graph is too conservative, and the local tangent graph is easily restricted by the accuracy of the sensor and the interference of the environment.
  • the error of the positioning system is not considered.
  • the localization error in practical application has a great influence on the convergence of the Bug-like algorithm.
  • the purpose of the present invention is to provide a local path planning method for an unmanned vehicle.
  • the present invention provides the following scheme:
  • a local path planning method for an unmanned vehicle comprising:
  • the obstacle information is obtained by the ranging sensor, and a planning reference point set and a local map are constructed.
  • the boundary point of the obstacle is the tangent point between the line passing through the coordinate point of the vehicle and the edge of the obstacle, and the local map is a grid map describing the situation that the space near the vehicle is occupied by obstacles;
  • Collision detection is performed on the planned path; the planned path through collision detection is used as the reference path for motion control.
  • a set of planning reference points is constructed by using the obstacle information obtained by the ranging sensor, which specifically includes:
  • d(b,o) ⁇ max ⁇ ;
  • the obstacle boundary point e cw the set composed of is E cw ;
  • the obstacle boundary point e ccw the set formed by it is E ccw ;
  • E ccw ⁇ e ccw ⁇ S′
  • ⁇ d e ,j 1,2,...,m ⁇ , ⁇ max is the maximum detection distance of the ranging sensor, s n ⁇ S', s n-1 ,s n-2 ,...,s nm represents s n adjacent m points in the clockwise direction, s n+1 ,s n+2 ,...,s n+m means s n adjacent m points in the counterclockwise direction, atan2(a,b) Represents the azimuth of point a relative to point b; d(a, b) represents the distance between point a and point b, is the angular resolution of the ranging sensor, o is the position point of the vehicle in the vehicle coordinate system, is
  • the elements in the planning reference point set are selected as the planning end points, and the Dubins curve of the planning path is generated, which specifically includes:
  • the vehicle transitions to a state of moving toward the target: 1) the vehicle is at the initial moment or 2) the vehicle is in a state of surrounding obstacles, and there is an element in the set F that is greater than the distance from the determined local closest point M to the target point is smaller, and the Dubins curve generated based on the element can pass collision detection, wherein the local closest point M is the point detected by the ranging sensor and located on the obstacle closest to the target position;
  • Planning the Dubins curve Select the element with the least cost in the current set F, take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
  • the elements in the set of planning reference points are selected as the planning end points, and the Dubins curve of the planning path is generated, which specifically includes:
  • the vehicle transitions to the state of surrounding obstacles: the vehicle is moving towards the target, and the distance between the vehicle position and the target point fed back by the positioning system at the current moment and the closest distance between the vehicle and the target point recorded before the current moment The difference is greater than the set distance threshold;
  • the obstacle In the surrounding obstacle state, if the target position when the vehicle is converted from the moving state toward the target to the surrounding obstacle state is on the right side of the current position of the vehicle, the obstacle is clockwise around the obstacle, and the elements in the set F' Sort by azimuth angle from small to large, otherwise circle the obstacles in the counterclockwise direction, and sort the elements in the set F' according to the azimuth angle from large to small;
  • Planning the Dubins curve select the top element in the set F', take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
  • Dubins curve cannot pass the collision detection, exclude the current reference element in the set F', and jump to the step of planning the Dubins curve, until the planned Dubins curve passes the collision detection or there is no Dubins curve that can pass the collision detection. .
  • the method further includes:
  • the method further includes:
  • a Dubins curve is generated according to the specified heading at the target point and collision detection is performed; otherwise, the step of planning a Dubins curve in the moving state toward the target is performed and collision detection is performed.
  • the present invention discloses the following technical effects:
  • the local path planning method for an unmanned vehicle is oriented to the wheeled vehicle using Ackermann steering, integrates the TangentBug algorithm and the Dubins curve, takes into account the geometric shape of the vehicle and the errors of the positioning system and the ranging sensor, and satisfies the minimum requirement of the wheeled vehicle. Motion limit for turning radius. While ensuring the real-time performance and convergence of the local path planning method, the security and robustness are improved.
  • FIG. 1 is a framework diagram of an automatic driving system provided by an embodiment of the present invention
  • FIG. 2 is an overall flowchart of a decision planning layer of a local path planning method provided by an embodiment of the present invention
  • FIG. 3 is a schematic diagram of a planning result in an embodiment of the present invention.
  • FIG. 4 is a flow chart of a state of moving toward a target in an embodiment of the present invention.
  • FIG. 5 is a schematic diagram of collision detection along a planned path in an embodiment of the present invention.
  • FIG. 6 is a flowchart of a state of surrounding an obstacle in an embodiment of the present invention.
  • this embodiment provides a local path planning method for an unmanned vehicle.
  • the method is divided into two layers: environment modeling and decision planning according to the architecture.
  • the inputs are ranging sensors and positioning systems.
  • the data is output as a reference path.
  • the obstacle information received by the environment modeling layer is used to construct a set of planning reference points and a local map, and then the decision planning layer uses the above information to generate a reference path in combination with the pose of the vehicle itself and the target.
  • the specific method is as follows:
  • the set of planning reference points includes points located on the detection boundary of the ranging sensor, points rotated and sampled based on obstacle boundary points, and target points , the boundary point of the obstacle is the tangent point between the line passing through the coordinate point of the vehicle and the edge of the obstacle, and the local map is a grid map describing the situation that the space near the vehicle is occupied by obstacles. details as follows:
  • d(a,b) represents the Euclidean distance between point a and point b on the two-dimensional plane
  • o represents the position of the vehicle in the vehicle coordinate system
  • ⁇ is the scale coefficient
  • WO i represents the first i obstacles.
  • the obstacle distance information obtained by it is expressed as:
  • ⁇ max is the maximum detection distance of the ranging sensor
  • n is the scale factor
  • the detected distance is The point of s n is defined as the set S' formed by the point s n as:
  • Atan2(a,b) represents the azimuth angle of point a relative to point b.
  • the set B formed by it is defined as:
  • the present invention divides it into two cases: clockwise sampling and counterclockwise sampling.
  • clockwise sampling For any point s n in the set S', let s n-1 , s n-2 ,..., s nm represent m points adjacent to the point in the clockwise direction, let s n+1 , s n +2 ,...,s n+m represents m points adjacent to this point in the counterclockwise direction.
  • E cw is the set of boundary points sampled clockwise
  • E ccw is the set of boundary points sampled counterclockwise
  • E cw and E ccw are respectively expressed as follows:
  • E cw ⁇ e cw ⁇ S′
  • ⁇ d e , j 1, 2, ..., m ⁇
  • E ccw ⁇ e ccw ⁇ S′
  • ⁇ d e , j 1, 2, ..., m ⁇
  • the point e cw is an element in the set E cw
  • the point e ccw is an element in the set E ccw
  • j is an element subscript.
  • ⁇ safe and ⁇ sample can be calculated but not limited to the following formulas:
  • k safe is the safety factor
  • r safe is the safety distance
  • k sample is the sampling scale factor
  • the set of planning reference points there are three types of planning reference points: one is the point that is not blocked by obstacles and is located on the detection boundary of the ranging sensor; the other is the obstacle boundary point after rotation sampling; the third is the point within the detection range of the ranging sensor target point T.
  • the point f is the element in the set F
  • the point f' is the element in the set F'.
  • the environment modeling layer uses the obstacle information obtained by the ranging sensor to construct a local map. Firstly, based on the space segmentation method, the surrounding environment is decomposed into grid cells, then it is determined whether each grid cell is occupied by obstacles according to the obstacle information, and finally the distance from each grid cell to the nearest occupied grid is calculated.
  • the methods for decomposing the surrounding environment into grid cells include but are not limited to a uniform decomposition method, a hierarchical decomposition method, and the like.
  • each grid cell is occupied by an obstacle, if it is occupied, it is an obstacle grid, otherwise it is a free grid.
  • the distance between each grid cell and its nearest obstacle grid is calculated, and the distance includes but is not limited to Euclidean distance and Manhattan distance.
  • the decision planning layer generates a reference path by combining the local map and planning reference point set constructed by the environment modeling layer, as well as the vehicle and target poses provided by the positioning system.
  • the local path planning method proposed by the present invention integrates TangentBug and Dubins curves, and adds collision detection along the planned path and state transition rules considering positioning errors. details as follows:
  • the overall flow chart of the decision planning layer is shown in Figure 2.
  • the input is the local map and the set of planning reference points generated by the environment modeling layer, as well as the vehicle and target poses provided by the positioning system, and the output is a reference path.
  • FIG. 3 The schematic diagram of the planning result of the present invention is shown in FIG. 3 , in which the vehicle starts from the starting point S and moves toward the target until the vehicle detects the obstacle WO 1 at the collision point H 1 , and then the vehicle starts to move along the edge of the obstacle until at the collision point H 1 .
  • the vehicle Drive away from the obstacle WO 1 at point D 1 , then the vehicle detects the obstacle WO 2 at the collision point H 2 , and the vehicle starts to advance along the edge of the obstacle again until the vehicle transitions to the state transition point SW 2
  • the vehicle starts to circle around the obstacle in a counterclockwise direction until it finds the stop around reference point L R2 at the stop around point L 2 that meets the conditions for safely driving away from the obstacle WO 2 and transitions Move back to the target state until it finally stops at the target point T.
  • the target point When moving towards the target, first determine whether the difference between the distance between the vehicle position fed back by the positioning system at the current moment and the target point and the closest distance between the vehicle and the target point recorded before the current moment is greater than the set distance threshold d m , If so, it will switch to the state of surrounding obstacles, and record the relative position of the target point at this time. Then determine whether the target point is within the detection range of the ranging sensor. If it is "Yes”, try to generate a Dubins curve with the pose of the target point as the end point and perform collision detection. If it is "No”, calculate the planning reference point. The cost of the element, try to generate the corresponding Dubins curve with the pose of the planning reference point as the end point and perform collision detection. At this time, the heading of the planning reference point is the azimuth angle of the planning reference point relative to the vehicle pose.
  • the cost c fi of any element fi in the set F can be calculated by, but not limited to, the distance from the vehicle itself to the element plus the Euclidean distance from the element to the target point, namely:
  • the Dubins curve is generated and the collision detection is performed in turn. Once the collision detection is passed, the Dubins curve is used as the local reference path, otherwise it is considered that there is no safe path and the vehicle stops.
  • the schematic diagram of collision detection along the planned path is shown in Figure 5.
  • the vehicle is surrounded by a covering circle, the center of which is located at the center of the vehicle, the center of the front axle and the center of the rear axle, and the radius of the covering circle is r safe .
  • the radius of the covering circle is r safe .
  • the vehicle position reference point is selected with a fixed step along the curve, the reference point coincides with the center point of the rear axle of the vehicle, and the direction of the tangent vector at the reference point is the heading of the vehicle. If the planned path from the start point to the end point q fi of the planned Dubins curve passes the collision detection, then the planned path is considered to be safe; otherwise, there is a risk of collision.
  • the manner in which the covering circle surrounds the vehicle is not limited to the method shown in FIG. 5 .
  • d m a distance threshold d m , if i, d i (P, T)-d′ i-1 (P, T) > d m is established at a certain time, then it is considered that there is a local closest point Mi , and then it is converted into a surrounding Obstacle status.
  • d m should take a small positive value in combination with the error of the positioning system.
  • FIG. 6 The flowchart of the state of surrounding obstacles in this embodiment is shown in FIG. 6 .
  • the local closest point Mi is first updated.
  • the direction of the surrounding obstacles is determined by the target orientation recorded during the state transition. If atan2(t,o) ⁇ 0 at this time, the surrounding direction is clockwise, otherwise it is counterclockwise.
  • the vehicle starts to circle around the obstacle counterclockwise from the state transition point SW 2 until it runs away from the obstacle at point L 2 .
  • the set F' that only considers the information in front of itself is used as the set of planning reference points.
  • the elements in the set F' are sorted in descending order of azimuth, and in clockwise order, they are sorted in descending order. Dubins curves are generated in turn and collision detection is performed. Once passed, this is used as a reference path, otherwise it is considered that there is no safe path and the vehicle stops.
  • the present invention sets the physical meaning of the variables involved as shown in Table 1:
  • the present invention proposes a local path planning method by fusing TangentBug and Dubins curves, aiming at the situation where only local map and positioning information can be obtained, which only needs to obtain local map information and the pose of the vehicle itself and the target point, without the need for global map and other information.
  • the present invention proposes a method for constructing a local planning reference point set without establishing a local tangent graph. There is no high requirement for the accuracy of the ranging sensor, and no requirement for the shape of the obstacle.
  • the present invention uses the Dubins curve as the planning path, which can meet the motion restriction of the vehicle with the minimum turning radius and the heading requirement at the target point.
  • the present invention takes the geometric shape of the vehicle into consideration, and performs strict collision detection along the planned path, which can meet the safety requirements of the planned path.
  • the present invention proposes a behavior-state transition rule adapting to the positioning system error in combination with the proof result of TangentBug, which improves the robustness of the algorithm of the present invention to the positioning error.

Landscapes

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

Abstract

A local path planning method for an unmanned vehicle. The method comprises: acquiring obstacle information by means of a distance measuring sensor, and constructing a planning reference point set and a local map, the planning reference point set comprising points located on a detection boundary of the distance measuring sensor, points rotationally sampled based on obstacle boundary points, and target points, the obstacle boundary points being tangent points between straight lines passing through a coordinate point of the vehicle and edges of obstacles, and the local map being a grid map describing the occupancy of the space near the vehicle by the obstacles; selecting elements in the planning reference point set as planning end points to generate a Dubins curve of a planned path; performing collision detection on the planned path; and using the planned path subjected to the collision detection as a reference path for movement control. According to the local path planning method for an unmanned vehicle, a TangentBug algorithm and the Dubins curve are fused, the vehicle geometry and errors of a positioning system and the distance measuring sensor are considered, and the movement constraints for the minimum steering radius of wheeled vehicles are met.

Description

一种无人车辆局部路径规划方法A local path planning method for unmanned vehicles
本申请要求于2021年01月15日提交中国专利局、申请号为202110051442.9、发明名称为“一种无人车辆局部路径规划方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。This application claims the priority of the Chinese patent application filed on January 15, 2021 with the application number 202110051442.9 and the invention titled "A method for local path planning for an unmanned vehicle", the entire contents of which are incorporated herein by reference Applying.
技术领域technical field
本发明涉及路径规划领域，特别是涉及一种融合TangentBug和Dubins曲线的无人车辆局部路径规划方法。The invention relates to the field of path planning, in particular to a local path planning method for unmanned vehicles integrating TangentBug and Dubins curves.
背景技术Background technique
随着汽车智能化的发展，自动驾驶技术成为智能车辆领域研究的重要方向，而路径规划是其关键技术之一。智能车辆的路径规划是指在一定环境模型的基础上，给定起点与目标点后，按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。With the development of intelligent vehicles, autonomous driving technology has become an important research direction in the field of intelligent vehicles, and path planning is one of its key technologies. The path planning of intelligent vehicles refers to planning an effective path without collision and safely reaching the target point based on a certain environment model and given the starting point and the target point according to the performance index.
目前路径规划按不同算法可分为图搜索类算法、采样类算法、势场法和智能仿生类算法等。图搜索和采样类算法最为常见，运算速度较快，但需要路径平滑处理以适应车辆的运动特性，并常用于已知全局地图的情况。势场法对障碍物的适应性较好，但容易陷入局部最优解。智能仿生类算法参考了生物的行为模式和特性，相比其他算法可以得出更优的结果，其中Lumelsky等参考爬虫(Bug)的行为模式，针对仅能获取局部地图和定位信息的情况，提出了Bug1和Bug2算法。At present, path planning can be divided into graph search algorithm, sampling algorithm, potential field method and intelligent bionic algorithm according to different algorithms. Graph search and sampling algorithms are the most common and fast, but require path smoothing to adapt to the motion characteristics of the vehicle, and are often used when the global map is known. The potential field method has good adaptability to obstacles, but it is easy to fall into the local optimal solution. Intelligent bionic algorithms refer to the behavior patterns and characteristics of living beings, and can obtain better results than other algorithms. Among them, Lumelsky et al. refer to the behavior patterns of crawler (Bug), and for the situation where only local maps and positioning information can be obtained, they propose Bug1 and Bug2 algorithms.
Bug1和Bug2面向可全向运动的机器人，解决其在二维平面上的局部路径规划问题，要求机器人装有接触式传感器以探测周围障碍物，并有精确的实时定位。Bug1和Bug2均由朝目标移动和环绕障碍物两种状态组成，但Bug2在简单场景下规划路径的总长度可能更短，而在复杂场景时Bug1更为稳健。基于Bug1和Bug2，衍生出了在某些场景下规划路径总长度更短的Alg1算法，使用了测距传感器的TangentBug算法，基于模糊逻辑的FuzzyBug算法，采用向量化路径的InsertBug算法等等。由于TangentBug算法通过测距传感器并结合局部切线图，解决了Bug类算法必须接触障碍物的问题，在实际中得到了较为广泛的应用。然而，现有的基于TangentBug算法的局部路径规划算法仍 存在以下不足：Bug1 and Bug2 are aimed at robots that can move in all directions, and solve their local path planning problems on a two-dimensional plane, requiring robots to be equipped with contact sensors to detect surrounding obstacles and have accurate real-time positioning. Both Bug1 and Bug2 consist of two states of moving towards the target and surrounding obstacles, but the total length of the planned path in Bug2 may be shorter in simple scenarios, while Bug1 is more robust in complex scenarios. Based on Bug1 and Bug2, the Alg1 algorithm with a shorter total length of the planned path in some scenarios is derived, the TangentBug algorithm using the ranging sensor, the FuzzyBug algorithm based on fuzzy logic, the InsertBug algorithm using vectorized paths, and so on. Because the TangentBug algorithm solves the problem that the bug algorithm must contact the obstacle through the ranging sensor and the local tangent graph, it has been widely used in practice. However, the existing local path planning algorithms based on TangentBug algorithm still have the following shortcomings:
1、对被控对象的外形尺寸没有充分考虑。为保证规划路径的安全性，对局部切线图中障碍物边界点的偏移距离过于保守，且局部切线图易受传感器精度的制约和环境的干扰。1. The size of the controlled object is not fully considered. In order to ensure the safety of the planned path, the offset distance of the obstacle boundary points in the local tangent graph is too conservative, and the local tangent graph is easily restricted by the accuracy of the sensor and the interference of the environment.
2、没有考虑定位***的误差。实际应用中的定位误差对Bug类算法的收敛性有较大的影响。2. The error of the positioning system is not considered. The localization error in practical application has a great influence on the convergence of the Bug-like algorithm.
3、目前Bug类算法大多要求被控对象可全向运动，而对于采用Ackermann转向的轮式车辆，其存在最小转向半径的运动限制。3. At present, most bug-like algorithms require the controlled object to be able to move in all directions, while for wheeled vehicles that use Ackermann steering, there is a movement limit of the minimum steering radius.
发明内容SUMMARY OF THE INVENTION
本发明的目的是提供一种无人车辆局部路径规划方法。The purpose of the present invention is to provide a local path planning method for an unmanned vehicle.
为实现上述目的，本发明提供了如下方案：For achieving the above object, the present invention provides the following scheme:
一种无人车辆局部路径规划方法，包括：A local path planning method for an unmanned vehicle, comprising:
通过测距传感器获取障碍物信息，构建规划参考点集合和局部地图，所述规划参考点集合包括位于测距传感器探测边界上的点、基于障碍物边界点旋转采样后的点以及目标点，所述障碍物边界点为经过车辆坐标点的直线与障碍物边缘的切点，所述局部地图为描述车辆附近空间被障碍物占据情况的栅格地图；The obstacle information is obtained by the ranging sensor, and a planning reference point set and a local map are constructed. The boundary point of the obstacle is the tangent point between the line passing through the coordinate point of the vehicle and the edge of the obstacle, and the local map is a grid map describing the situation that the space near the vehicle is occupied by obstacles;
选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线；Select the elements in the planning reference point set as the planning end point, and generate the planning path Dubins curve;
对规划路径进行碰撞检测；通过碰撞检测的规划路径作为运动控制的参考路径。Collision detection is performed on the planned path; the planned path through collision detection is used as the reference path for motion control.
可选地，通过测距传感器获取的障碍物信息，构建规划参考点集合，具体包括：Optionally, a set of planning reference points is constructed by using the obstacle information obtained by the ranging sensor, which specifically includes:
测距传感器在各个方向上获取的点的集合
Figure PCTCN2022072247-appb-000001
Figure PCTCN2022072247-appb-000002
集合S'中包括测距传感器探测边界上的点b，其构成的集合为B＝{b∈S'|d(b,o)＝ρ max}；障碍物边界点e cw，其构成的集合为E cw；障碍物边界点e ccw，其构成的集合为E ccw；其中， A collection of points acquired by the ranging sensor in all directions
Figure PCTCN2022072247-appb-000001
Figure PCTCN2022072247-appb-000002
The set S' includes the point b on the detection boundary of the ranging sensor, and the set composed of it is B={b∈S'|d(b,o)=ρ max }; the obstacle boundary point e cw , the set composed of is E cw ; the obstacle boundary point e ccw , the set formed by it is E ccw ; where,
Figure PCTCN2022072247-appb-000003
Figure PCTCN2022072247-appb-000003
E ccw＝{e ccw∈S′||d(s n-j,o)-d(e ccw,o)|<d e,|d(s n+j,o)-d(e ccw,o)|≥d e,j＝1,2,…,m}，ρ max为测距传感器的最大探测距离，s n∈S'，s n-1,s n-2,...,s n-m表示s n顺时针方向上相邻的m个点，s n+1,s n+2,...,s n+m表示s n逆时针方向上相邻的m个点，atan2(a,b)表示点a相对于点b的方位角；d(a,b)表示点a与点b之间的距离，
Figure PCTCN2022072247-appb-000004
为测距传感器的角度分辨率，o为车辆在车辆坐标系下的位置点，
Figure PCTCN2022072247-appb-000005
为实际应用中测距传感器获取的障碍物距离，
Figure PCTCN2022072247-appb-000006
为n倍
Figure PCTCN2022072247-appb-000007
的角度值，d e为用于障碍物边界点判断的距离阈值； E ccw ={e ccw ∈S′||d(s nj ,o)-d(e ccw ,o)|<d e ,|d(s n+j ,o)-d(e ccw ,o)| ≥d e ,j=1,2,...,m}, ρ max is the maximum detection distance of the ranging sensor, s n ∈ S', s n-1 ,s n-2 ,...,s nm represents s n adjacent m points in the clockwise direction, s n+1 ,s n+2 ,...,s n+m means s n adjacent m points in the counterclockwise direction, atan2(a,b) Represents the azimuth of point a relative to point b; d(a, b) represents the distance between point a and point b,
Figure PCTCN2022072247-appb-000004
is the angular resolution of the ranging sensor, o is the position point of the vehicle in the vehicle coordinate system,
Figure PCTCN2022072247-appb-000005
is the obstacle distance obtained by the ranging sensor in practical applications,
Figure PCTCN2022072247-appb-000006
n times
Figure PCTCN2022072247-appb-000007
The angle value of d e is the distance threshold used to judge the boundary point of the obstacle;
基于所述障碍物边界点进行旋转采样，得到顺时针采样点集合E' cw和逆时针采样点集合E' ccw，其中，
Figure PCTCN2022072247-appb-000008
Figure PCTCN2022072247-appb-000009
Figure PCTCN2022072247-appb-000010
e' cw为集合E' cw中的元素；e' ccw为集合E' ccw中的元素；θ为旋转角度；R为旋转矩阵，R(θ)表示逆时针旋转θ角度，R(-θ)表示顺时针旋转θ角度；θ safe为旋转角度的下限；θ sample为旋转角度的上限；h为旋转采样的步长； Rotation sampling is performed based on the obstacle boundary points to obtain a clockwise sampling point set E' cw and a counterclockwise sampling point set E' ccw , wherein,
Figure PCTCN2022072247-appb-000008
Figure PCTCN2022072247-appb-000009
Figure PCTCN2022072247-appb-000010
e' cw is the element in the set E'cw;e' ccw is the element in the set E'ccw; θ is the rotation angle; R is the rotation matrix, R(θ) represents the counterclockwise rotation θ angle, R(-θ) Represents a clockwise rotation of θ angle; θ safe is the lower limit of the rotation angle; θ sample is the upper limit of the rotation angle; h is the step size of the rotation sampling;
确定规划参考点集合F和仅考虑车辆前方信息的规划参考点集合F'，其中，F＝{f|f∈B∪E′ ccw∪E′ cw∪T′}，
Figure PCTCN2022072247-appb-000011
T'为目标点的集合，若目标点T位于测距传感器探测范围内，记目标点T在车辆坐标系下的坐标点为t，则T'＝{t}，否则
Figure PCTCN2022072247-appb-000012
Determine the planning reference point set F and the planning reference point set F' that only considers the information ahead of the vehicle, where F={f|f∈B∪E′ ccw ∪E′ cw ∪T′},
Figure PCTCN2022072247-appb-000011
T' is the set of target points. If the target point T is within the detection range of the ranging sensor, mark the coordinate point of the target point T in the vehicle coordinate system as t, then T'={t}, otherwise
Figure PCTCN2022072247-appb-000012
可选地，所述选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线，具体包括：Optionally, the elements in the planning reference point set are selected as the planning end points, and the Dubins curve of the planning path is generated, which specifically includes:
当满足以下条件时，车辆转换为朝目标移动状态：1)车辆处于初始时刻或2)车辆处于环绕障碍物状态，且集合F中存在一元素比已确定的局部最近点M到目标点的距离更小，且基于所述元素生成的Dubins曲线能够通过碰撞检测，其中，所述局部最近点M为测距传感器探测到的位于障碍物上距离目标位置最近的点；When the following conditions are met, the vehicle transitions to a state of moving toward the target: 1) the vehicle is at the initial moment or 2) the vehicle is in a state of surrounding obstacles, and there is an element in the set F that is greater than the distance from the determined local closest point M to the target point is smaller, and the Dubins curve generated based on the element can pass collision detection, wherein the local closest point M is the point detected by the ranging sensor and located on the obstacle closest to the target position;
在所述朝目标移动状态中：In the move-to-goal state:
确定集合F中各元素的代价，所述代价根据所述元素到车辆当前位置的距离以及所述元素到目标位置的距离确定；Determine the cost of each element in the set F, the cost is determined according to the distance from the element to the current position of the vehicle and the distance from the element to the target position;
规划Dubins曲线：选取当前集合F中代价最小的元素，以车辆的位姿为起点，以选取元素的位姿为终点，规划Dubins曲线；其中，终点的航向为选取元素相对车辆的方位角；Planning the Dubins curve: Select the element with the least cost in the current set F, take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
如果规划出的Dubins曲线不能通过碰撞检测，则在集合F中排除当前参考元素，跳转至规划Dubins曲线步骤，直至规划出的Dubins曲线通过碰撞检测时或不存在能够通过碰撞检测的Dubins曲线时停止跳转。If the planned Dubins curve cannot pass the collision detection, exclude the current reference element in the set F and jump to the planning Dubins curve step until the planned Dubins curve passes the collision detection or when there is no Dubins curve that can pass the collision detection Stop jumping.
可选地，所述选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线，具体包括：Optionally, the elements in the set of planning reference points are selected as the planning end points, and the Dubins curve of the planning path is generated, which specifically includes:
当满足以下条件时，车辆转换为环绕障碍物状态：车辆处于朝目标移动状态，且当前时刻定位***反馈的车辆位置与目标点间的距离和当前时刻之前记录的车辆与目标点间的最近距离之差大于设定的距离阈值；When the following conditions are met, the vehicle transitions to the state of surrounding obstacles: the vehicle is moving towards the target, and the distance between the vehicle position and the target point fed back by the positioning system at the current moment and the closest distance between the vehicle and the target point recorded before the current moment The difference is greater than the set distance threshold;
在所述环绕障碍物状态中，若车辆由朝目标移动状态转换为环绕障碍物状态时的目标位置处于车辆当前位置的右侧，则按顺时针方向环绕障碍物，对集合F'中的元素按方位角由小到大排序，否则按逆时针方向环绕障碍物，对集合F'中的元素按方位角由大到小排序；In the surrounding obstacle state, if the target position when the vehicle is converted from the moving state toward the target to the surrounding obstacle state is on the right side of the current position of the vehicle, the obstacle is clockwise around the obstacle, and the elements in the set F' Sort by azimuth angle from small to large, otherwise circle the obstacles in the counterclockwise direction, and sort the elements in the set F' according to the azimuth angle from large to small;
规划Dubins曲线：选取集合F'中排序最前的元素，以车辆的位姿为起点，以选取元素的位姿为终点，规划Dubins曲线；其中，终点的航向为选取元素相对车辆的方位角；Planning the Dubins curve: select the top element in the set F', take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
如果Dubins曲线不能通过碰撞检测，则在集合F'中排除当前参考元素，跳转至规划Dubins曲线步骤，直至规划出的Dubins曲线通过检测碰撞或不存在能够通过碰撞检测的Dubins曲线时停止跳转。If the Dubins curve cannot pass the collision detection, exclude the current reference element in the set F', and jump to the step of planning the Dubins curve, until the planned Dubins curve passes the collision detection or there is no Dubins curve that can pass the collision detection. .
可选地，所述方法还包括：在朝目标移动状态时，除按目标点的位姿规划出的Dubins曲线通过碰撞检测外，根据d' 0(P,T)＝d 0(P,T),i＝0、
Figure PCTCN2022072247-appb-000013
Figure PCTCN2022072247-appb-000014
i∈N *确定当前时刻i之前车辆P与目标点T之间欧式距离的最小值d' i(P,T)，其中，d i(P,T)为i时刻定位***反馈的车辆位置P与目标位置T之间的距离，d 0(P,T)为初始时刻定位***反馈的车辆位置P与 目标位置T之间的距离，N *表示正整数。 Optionally, the method further includes: when moving toward the target, in addition to passing collision detection on the Dubins curve planned according to the pose of the target point, according to d' 0 (P, T)=d 0 (P, T ), i=0,
Figure PCTCN2022072247-appb-000013
Figure PCTCN2022072247-appb-000014
i∈N * Determine the minimum value d' i (P, T) of the Euclidean distance between the vehicle P and the target point T before the current time i, where d i (P, T) is the vehicle position P fed back by the positioning system at time i The distance from the target position T, d 0 (P, T) is the distance between the vehicle position P fed back by the positioning system at the initial moment and the target position T, and N * represents a positive integer.
可选地，所述方法还包括：Optionally, the method further includes:
在所述环绕障碍物状态中，当检测到车辆环绕障碍物一周时，确定目标不可到达。In the obstacle-surrounding state, when it is detected that the vehicle circles around the obstacle, it is determined that the target is unreachable.
可选地，所述方法还包括：Optionally, the method further includes:
若目标位置位于测距传感器的探测范围内，则按目标点处规定的航向生成Dubins曲线并进行碰撞检测，否则执行所述朝目标移动状态中规划Dubins曲线步骤并进行碰撞检测。If the target position is within the detection range of the ranging sensor, a Dubins curve is generated according to the specified heading at the target point and collision detection is performed; otherwise, the step of planning a Dubins curve in the moving state toward the target is performed and collision detection is performed.
根据本发明提供的具体实施例，本发明公开了以下技术效果：According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:
本发明提供的无人车辆局部路径规划方法，面向采用Ackermann转向的轮式车辆，融合了TangentBug算法与Dubins曲线，考虑了车辆几何外形以及定位***和测距传感器的误差，满足了轮式车辆最小转向半径的运动限制。在保证局部路径规划方法的实时性和收敛性的同时，提升了安全性和鲁棒性。The local path planning method for an unmanned vehicle provided by the present invention is oriented to the wheeled vehicle using Ackermann steering, integrates the TangentBug algorithm and the Dubins curve, takes into account the geometric shape of the vehicle and the errors of the positioning system and the ranging sensor, and satisfies the minimum requirement of the wheeled vehicle. Motion limit for turning radius. While ensuring the real-time performance and convergence of the local path planning method, the security and robustness are improved.
说明书附图Instruction drawings
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the accompanying drawings required in the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some of the present invention. In the embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without any creative effort.
图1为本发明实施例提供的自动驾驶***框架图；FIG. 1 is a framework diagram of an automatic driving system provided by an embodiment of the present invention;
图2为本发明实施例提供的局部路径规划方法决策规划层总体流程图；FIG. 2 is an overall flowchart of a decision planning layer of a local path planning method provided by an embodiment of the present invention;
图3为本发明实施例中的规划结果示意图；3 is a schematic diagram of a planning result in an embodiment of the present invention;
图4为本发明实施例中朝目标移动状态的流程图；4 is a flow chart of a state of moving toward a target in an embodiment of the present invention;
图5为本发明实施例中沿规划路径进行碰撞检测的示意图；5 is a schematic diagram of collision detection along a planned path in an embodiment of the present invention;
图6为本发明实施例中环绕障碍物状态的流程图。FIG. 6 is a flowchart of a state of surrounding an obstacle in an embodiment of the present invention.
具体实施方式Detailed ways
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造 性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts shall fall within the protection scope of the present invention.
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。In order to make the above objects, features and advantages of the present invention more clearly understood, the present invention will be described in further detail below with reference to the accompanying drawings and specific embodiments.
基于自动驾驶***，如图1所示，本实施例提供了一种无人车辆局部路径规划方法，该方法按架构分为环境建模和决策规划两层，其输入为测距传感器和定位***的数据，输出为一条参考路径。环境建模层接收的障碍物信息用于构建规划参考点集合和局部地图，之后决策规划层利用上述信息结合车辆自身和目标的位姿生成参考路径。具体方法如下：Based on the automatic driving system, as shown in FIG. 1 , this embodiment provides a local path planning method for an unmanned vehicle. The method is divided into two layers: environment modeling and decision planning according to the architecture. The inputs are ranging sensors and positioning systems. The data is output as a reference path. The obstacle information received by the environment modeling layer is used to construct a set of planning reference points and a local map, and then the decision planning layer uses the above information to generate a reference path in combination with the pose of the vehicle itself and the target. The specific method is as follows:
1、通过测距传感器获取障碍物信息，构建规划参考点集合和局部地图，所述规划参考点集合包括位于测距传感器探测边界上的点、基于障碍物边界点旋转采样后的点以及目标点，所述障碍物边界点为经过车辆坐标点的直线与障碍物边缘的切点，所述局部地图为描述车辆附近空间被障碍物占据情况的栅格地图。具体如下：1. Obtain obstacle information through the ranging sensor, and construct a set of planning reference points and a local map. The set of planning reference points includes points located on the detection boundary of the ranging sensor, points rotated and sampled based on obstacle boundary points, and target points , the boundary point of the obstacle is the tangent point between the line passing through the coordinate point of the vehicle and the edge of the obstacle, and the local map is a grid map describing the situation that the space near the vehicle is occupied by obstacles. details as follows:
(1)将测距传感器在任意方向上获取的障碍物距离信息表示为：(1) The obstacle distance information obtained by the ranging sensor in any direction is expressed as:
Figure PCTCN2022072247-appb-000015
Figure PCTCN2022072247-appb-000015
其中，
Figure PCTCN2022072247-appb-000016
为车辆处于状态q，在方位角
Figure PCTCN2022072247-appb-000017
上获取的障碍物距离；d(a,b)表示点a与点b在二维平面上的欧氏距离；o表示车辆在车辆坐标系下的位置点；λ为比例系数；WO i表示第i个障碍物。 in,
Figure PCTCN2022072247-appb-000016
for the vehicle in state q, at azimuth
Figure PCTCN2022072247-appb-000017
The obstacle distance obtained from above; d(a,b) represents the Euclidean distance between point a and point b on the two-dimensional plane; o represents the position of the vehicle in the vehicle coordinate system; λ is the scale coefficient; WO i represents the first i obstacles.
对实际应用中存在角度分辨率和最大探测距离的测距传感器，将其获取的障碍物距离信息表示为：For a ranging sensor with angular resolution and maximum detection distance in practical applications, the obstacle distance information obtained by it is expressed as:
Figure PCTCN2022072247-appb-000018
Figure PCTCN2022072247-appb-000018
其中，
Figure PCTCN2022072247-appb-000019
为实际应用中测距传感器获取的障碍物距离，ρ max为测距传感器的最大探测距离，
Figure PCTCN2022072247-appb-000020
为测距传感器的角度分辨率，n为比例系数，
Figure PCTCN2022072247-appb-000021
为n倍
Figure PCTCN2022072247-appb-000022
的角度值。 in,
Figure PCTCN2022072247-appb-000019
is the obstacle distance obtained by the ranging sensor in practical applications, ρ max is the maximum detection distance of the ranging sensor,
Figure PCTCN2022072247-appb-000020
is the angular resolution of the ranging sensor, n is the scale factor,
Figure PCTCN2022072247-appb-000021
n times
Figure PCTCN2022072247-appb-000022
angle value.
设在某一方向
Figure PCTCN2022072247-appb-000023
探测到距离为
Figure PCTCN2022072247-appb-000024
的点为s n，定义点s n构成的集合 S'为： set in a certain direction
Figure PCTCN2022072247-appb-000023
The detected distance is
Figure PCTCN2022072247-appb-000024
The point of s n is defined as the set S' formed by the point s n as:
Figure PCTCN2022072247-appb-000025
Figure PCTCN2022072247-appb-000025
其中，atan2(a,b)表示点a相对于点b的方位角。Among them, atan2(a,b) represents the azimuth angle of point a relative to point b.
对于没有障碍物遮挡，位于传感器探测边界的点b，定义其构成的集合B为：For the point b that is not occluded by obstacles and is located on the detection boundary of the sensor, the set B formed by it is defined as:
B＝{b∈S′|d(b，o)＝ρ max} B={b∈S′|d(b, o)=ρ max }
由于在障碍物的边界附近，距离
Figure PCTCN2022072247-appb-000026
会发生较大的变化，因此，通过对点s n相邻点的距离进行分析，可得到不同类型的障碍物边界点。对于障碍物边界点的旋转采样，本发明将其分为顺时针和逆时针采样两种情况。对于集合S'中的任意一点s n，令s n-1,s n-2,...,s n-m表示该点顺时针方向上相邻的m个点，令s n+1,s n+2,...,s n+m表示该点逆时针方向上相邻的m个点。设用于障碍物边界点判断的距离阈值为d e，E cw为顺时针采样的边界点集合，E ccw为逆时针采样的边界点集合，E cw和E ccw分别如下表示： Since it is near the boundary of the obstacle, the distance
Figure PCTCN2022072247-appb-000026
There will be large changes, so different types of obstacle boundary points can be obtained by analyzing the distance between the adjacent points of point sn . For the rotation sampling of obstacle boundary points, the present invention divides it into two cases: clockwise sampling and counterclockwise sampling. For any point s n in the set S', let s n-1 , s n-2 ,..., s nm represent m points adjacent to the point in the clockwise direction, let s n+1 , s n +2 ,...,s n+m represents m points adjacent to this point in the counterclockwise direction. Suppose the distance threshold for obstacle boundary point judgment is de, E cw is the set of boundary points sampled clockwise, E ccw is the set of boundary points sampled counterclockwise, E cw and E ccw are respectively expressed as follows:
E cw＝{e cw∈S′||d(s n-j，o)-d(e cw，o)|≥d e，|d(s n+j，o)-d(e cw,o)|＜d e，j＝1，2，...，m} E cw ={e cw ∈S′||d(s nj ,o)-d(e cw ,o)|≥d e ,|d(s n+j ,o)-d(e cw ,o)| <d e , j=1, 2, ..., m}
E ccw＝{e ccw∈S′||d(s n-j，o)-d(e ccw，o)|＜d e，|d(s n+j，o)-d(e ccw，o)|≥d e，j＝1，2，...，m} E ccw ={e ccw ∈S′||d(s nj ,o)-d(e ccw ,o)|<d e ,|d(s n+j ,o)-d(e ccw ,o)| ≥d e , j=1, 2, ..., m}
其中，点e cw为集合E cw中的元素，点e ccw为集合E ccw中的元素，j为元素下标。 The point e cw is an element in the set E cw , the point e ccw is an element in the set E ccw , and j is an element subscript.
基于上述边界点进行顺时针旋转采样和逆时针旋转采样，将顺时针采样点的集合定义为E' cw，逆时针采样点的集合定义为E' ccw，那么有： Based on the above boundary points, clockwise rotation sampling and counterclockwise rotation sampling are performed, and the set of clockwise sampling points is defined as E' cw , and the set of counterclockwise sampling points is defined as E' ccw , then there are:
Figure PCTCN2022072247-appb-000027
Figure PCTCN2022072247-appb-000027
Figure PCTCN2022072247-appb-000028
Figure PCTCN2022072247-appb-000028
其中，点e' cw为集合E' cw中的元素；点e' ccw为集合E' ccw中的元素；θ为旋转角 度；R为旋转矩阵，R(θ)表示逆时针旋转θ角度，R(-θ)表示顺时针旋转θ角度；θ safe为旋转角度的下限；θ sample为旋转角度的上限；h为旋转采样的步长。 Among them, the point e' cw is the element in the set E'cw; the point e' ccw is the element in the set E'ccw; θ is the rotation angle; R is the rotation matrix, R(θ) represents the counterclockwise rotation θ angle, R (-θ) represents the clockwise rotation angle of θ; θ safe is the lower limit of the rotation angle; θ sample is the upper limit of the rotation angle; h is the step size of the rotation sampling.
对于θ safe和θ sample的取值，可以但不限于用下式计算： The values of θ safe and θ sample can be calculated but not limited to the following formulas:
Figure PCTCN2022072247-appb-000029
Figure PCTCN2022072247-appb-000029
θ sample＝k sampleθ safe θ sample = k sample θ safe
其中，k safe为安全系数，r safe为安全距离，k sample为采样比例系数。 Among them, k safe is the safety factor, r safe is the safety distance, and k sample is the sampling scale factor.
本实施例中规划参考点有三种：一是不受障碍物遮挡，位于测距传感器探测边界上的点；二是经旋转采样后的障碍物边界点；三是在测距传感器探测范围内的目标点T。对于集合T'，若目标点T位于测距传感器探测范围内，记目标点T在车辆坐标系下的坐标点为t，则T'＝{t}，否则
Figure PCTCN2022072247-appb-000030
当考虑全部方向时，设规划参考点的集合为F，当仅考虑自身前方信息，设规划参考点的集合为F'，那么有： In this embodiment, there are three types of planning reference points: one is the point that is not blocked by obstacles and is located on the detection boundary of the ranging sensor; the other is the obstacle boundary point after rotation sampling; the third is the point within the detection range of the ranging sensor target point T. For the set T', if the target point T is within the detection range of the ranging sensor, mark the coordinate point of the target point T in the vehicle coordinate system as t, then T'={t}, otherwise
Figure PCTCN2022072247-appb-000030
When considering all directions, let the set of planning reference points be F, and when only considering the information in front of oneself, let the set of planning reference points be F', then there are:
F＝{f|f∈B∪E' ccw∪E' cw∪T'} F={f|f∈B∪E' ccw ∪E' cw ∪T'}
Figure PCTCN2022072247-appb-000031
Figure PCTCN2022072247-appb-000031
其中，点f为集合F中的元素，点f’为集合F’中的元素。Among them, the point f is the element in the set F, and the point f' is the element in the set F'.
(2)环境建模层利用测距传感器获取的障碍物信息构建局部地图。首先基于空间分割法，把周围环境分解为栅格单元，然后根据障碍物信息确定每个栅格单元是否被障碍物占据，最后计算每个栅格单元到最近被占据栅格的距离。(2) The environment modeling layer uses the obstacle information obtained by the ranging sensor to construct a local map. Firstly, based on the space segmentation method, the surrounding environment is decomposed into grid cells, then it is determined whether each grid cell is occupied by obstacles according to the obstacle information, and finally the distance from each grid cell to the nearest occupied grid is calculated.
根据测距传感器的最大探测距离确定局部地图的边长s map，保证
Figure PCTCN2022072247-appb-000032
Figure PCTCN2022072247-appb-000033
其中，把周围环境分解为栅格单元的方法包括但不限于均匀分解法和递阶分解法等。 Determine the side length s map of the local map according to the maximum detection distance of the ranging sensor to ensure that
Figure PCTCN2022072247-appb-000032
Figure PCTCN2022072247-appb-000033
The methods for decomposing the surrounding environment into grid cells include but are not limited to a uniform decomposition method, a hierarchical decomposition method, and the like.
基于测距传感器的障碍物信息，判断每个栅格单元是否被障碍物占据，若被占据则为障碍栅格，否则为自由栅格。通过对栅格单元进行遍历，计算每个栅格单元与其最近障碍栅格的距离，该距离包括但不限于欧式距离和曼哈顿距离等。Based on the obstacle information of the ranging sensor, it is determined whether each grid cell is occupied by an obstacle, if it is occupied, it is an obstacle grid, otherwise it is a free grid. By traversing the grid cells, the distance between each grid cell and its nearest obstacle grid is calculated, and the distance includes but is not limited to Euclidean distance and Manhattan distance.
2、决策规划层结合环境建模层构建的局部地图和规划参考点集合，以及定位***提供的车辆和目标位姿，生成一条参考路径。本发明提出的局部路径规划方法融合了TangentBug和Dubins曲线，并加入了沿规划路径的碰撞检测和考虑定位误差的状态转换规则。具体如下：2. The decision planning layer generates a reference path by combining the local map and planning reference point set constructed by the environment modeling layer, as well as the vehicle and target poses provided by the positioning system. The local path planning method proposed by the present invention integrates TangentBug and Dubins curves, and adds collision detection along the planned path and state transition rules considering positioning errors. details as follows:
决策规划层的总体流程图见图2，其输入为环境建模层生成的局部地图和规划参考点集合，以及定位***提供的车辆和目标位姿，其输出为一条参考路径。首先对相关变量进行初始化，其中，目标是否不可到达用旗标unreachable表示，令unreachable＝false，令状态旗标state＝“朝目标移动”，令局部最近点M i到目标点T的欧式距离d min＝∞，令停止环绕参考点L Ri与目标点T间的欧式距离d leave＝d(S,T)。然后对目标是否可以到达和是否接近目标进行判断，其中，若车辆出现环绕障碍物一周的情况，则认为目标不可到达，此时unreachable＝true。是否接近目标由生成的Dubins曲线的长度l d进行判断，对于距离阈值d t，若l d<d t，则认为车辆接近目标点。若均未满足条件则进入朝目标移动或环绕障碍物状态，接着对是否生成安全的参考路径进行判断，最后对车辆与目标点间的距离进行更新，并进入下一程序循环。 The overall flow chart of the decision planning layer is shown in Figure 2. The input is the local map and the set of planning reference points generated by the environment modeling layer, as well as the vehicle and target poses provided by the positioning system, and the output is a reference path. First, initialize the relevant variables, in which, whether the target is unreachable is indicated by the flag unreachable, let unreachable=false, let the state flag state=“move toward the target”, let the local closest point Mi to the target point T The Euclidean distance d min =∞, let stop the Euclidean distance d leave =d(S,T) between the reference point L Ri and the target point T. Then, it is judged whether the target can be reached and whether it is close to the target. If the vehicle circles around the obstacle, it is considered that the target cannot be reached, and at this time unreachable=true. Whether it is close to the target is judged by the length l d of the generated Dubins curve. For the distance threshold d t , if l d <d t , it is considered that the vehicle is close to the target point. If none of the conditions are met, it enters the state of moving towards the target or surrounding obstacles, then judges whether a safe reference path is generated, and finally updates the distance between the vehicle and the target point, and enters the next program loop.
本发明规划结果示意图如图3所示，其中从起点S开始车辆以朝目标移动状态前进，直到在碰撞点H 1处车辆探测到障碍物WO 1，然后车辆开始沿障碍物边缘前进，直到在驶离障碍物点D 1处驶离障碍物WO 1，之后车辆在碰撞点H 2处探测到障碍物WO 2，车辆再次开始沿障碍物边缘前进，直到在状态转换点SW 2处车辆转换为环绕障碍物模式并记录下局部最近点M 2，车辆开始以逆时针方向环绕障碍物，直到在停止环绕点L 2处发现停止环绕参考点L R2满足安全驶离障碍物WO 2的条件并转换回朝目标移动状态，直到最后在目标点T处停止。 The schematic diagram of the planning result of the present invention is shown in FIG. 3 , in which the vehicle starts from the starting point S and moves toward the target until the vehicle detects the obstacle WO 1 at the collision point H 1 , and then the vehicle starts to move along the edge of the obstacle until at the collision point H 1 . Drive away from the obstacle WO 1 at point D 1 , then the vehicle detects the obstacle WO 2 at the collision point H 2 , and the vehicle starts to advance along the edge of the obstacle again until the vehicle transitions to the state transition point SW 2 Around the obstacle mode and record the local closest point M 2 , the vehicle starts to circle around the obstacle in a counterclockwise direction until it finds the stop around reference point L R2 at the stop around point L 2 that meets the conditions for safely driving away from the obstacle WO 2 and transitions Move back to the target state until it finally stops at the target point T.
在本实施例中朝目标移动状态的流程图见图4，其中state为状态旗标，state初始值为“朝目标移动”；d'(P,T)用于记录车辆与目标点的距离；d m为一距离阈值，用于状态转换判断。 The flow chart of the moving state toward the target in this embodiment is shown in Figure 4, where state is a state flag, and the initial value of state is "moving toward the target";d'(P, T) is used to record the distance between the vehicle and the target point; d m is a distance threshold, which is used for state transition judgment.
在朝目标移动状态时，首先判断当前时刻定位***反馈的车辆位置与目标 点间的距离和当前时刻之前记录的车辆与目标点间的最近距离之差，是否大于设定的距离阈值d m，若是则转换为环绕障碍物状态，并记录下此时目标点的相对位置。然后判断目标点是否在测距传感器的探测范围内，若为“是”，则尝试以目标点的位姿为终点生成Dubins曲线并进行碰撞检测，若为“否”，则计算规划参考点中元素的代价，尝试以规划参考点的位姿为终点生成对应的Dubins曲线并进行碰撞检测，此时规划参考点的航向为规划参考点相对车辆位姿的方位角。 When moving towards the target, first determine whether the difference between the distance between the vehicle position fed back by the positioning system at the current moment and the target point and the closest distance between the vehicle and the target point recorded before the current moment is greater than the set distance threshold d m , If so, it will switch to the state of surrounding obstacles, and record the relative position of the target point at this time. Then determine whether the target point is within the detection range of the ranging sensor. If it is "Yes", try to generate a Dubins curve with the pose of the target point as the end point and perform collision detection. If it is "No", calculate the planning reference point. The cost of the element, try to generate the corresponding Dubins curve with the pose of the planning reference point as the end point and perform collision detection. At this time, the heading of the planning reference point is the azimuth angle of the planning reference point relative to the vehicle pose.
设目标点T在车辆坐标系下为点t，集合F中任意元素f i的代价c fi可以但不限于用车辆自身到该元素的距离加该元素到目标点的欧式距离计算，即： Assuming that the target point T is a point t in the vehicle coordinate system, the cost c fi of any element fi in the set F can be calculated by, but not limited to, the distance from the vehicle itself to the element plus the Euclidean distance from the element to the target point, namely:
c fi＝d(o,f i)+d(f i,t) c fi =d( o ,fi )+d(fi , t)
按代价由小到大的顺序，依次生成Dubins曲线并进行碰撞检测，一旦通过碰撞检测，则以此Dubins曲线作为局部参考路径，否则认为没有安全的路径并停车。In the order of the cost from small to large, the Dubins curve is generated and the collision detection is performed in turn. Once the collision detection is passed, the Dubins curve is used as the local reference path, otherwise it is considered that there is no safe path and the vehicle stops.
沿规划路径进行碰撞检测的示意图如图5所示，采用覆盖圆将车辆包围，其圆心分别位于车辆中心，前轴中心和后轴中心，覆盖圆的半径为r safe。根据圆心在栅格地图中对应栅格单元到最近障碍物的距离，判断覆盖圆内是否存在障碍物，若任意覆盖圆内存在障碍物，则认为存在碰撞的风险。从Dubins曲线起点q o开始，沿曲线以固定步长选取车辆位置参考点，该参考点与车辆后轴中心点重合，参考点处切向量的方向为车辆的航向。若从规划出的Dubins曲线的起点到终点q fi均通过了碰撞检测，那么认为该规划路径是安全的，否则认为有碰撞的风险。覆盖圆将车辆包围的方式不限于图5中所示方法。 The schematic diagram of collision detection along the planned path is shown in Figure 5. The vehicle is surrounded by a covering circle, the center of which is located at the center of the vehicle, the center of the front axle and the center of the rear axle, and the radius of the covering circle is r safe . According to the distance between the center of the circle in the grid map corresponding to the grid cell and the nearest obstacle, it is judged whether there is an obstacle in the covering circle. If there is an obstacle in any covering circle, it is considered that there is a risk of collision. Starting from the starting point q o of the Dubins curve, the vehicle position reference point is selected with a fixed step along the curve, the reference point coincides with the center point of the rear axle of the vehicle, and the direction of the tangent vector at the reference point is the heading of the vehicle. If the planned path from the start point to the end point q fi of the planned Dubins curve passes the collision detection, then the planned path is considered to be safe; otherwise, there is a risk of collision. The manner in which the covering circle surrounds the vehicle is not limited to the method shown in FIG. 5 .
对于存在最小转向半径的车辆，即使不受障碍物的影响，也会因为自身状态等原因远离目标点。此外，由于定位***存在误差，即使车辆实际没有远离目标，也会出现距离d(P,T)增大的情况。因此，对于在某一时刻i的距离d i’(P,T)，在朝目标移动状态时，除按目标点航向规划成功外，其取值d i’(P,T)由式d′ 0(P，T)＝d 0(P，T)＝d(S，T)和
Figure PCTCN2022072247-appb-000034
确定，此时d i’(P,T)为当前时刻i之前到目标点距离的最小值。而其他情况下均为d i’(P,T)＝d i(P,T)。 For a vehicle with a minimum turning radius, even if it is not affected by obstacles, it will be far away from the target point due to its own state and other reasons. In addition, due to the errors in the positioning system, the distance d(P, T) will increase even if the vehicle is not actually far from the target. Therefore, for the distance d i '(P, T) of i at a certain time, when moving towards the target, in addition to the successful course planning according to the target point, its value d i '(P, T) is determined by the formula d' 0 (P,T)=d 0 (P,T)=d(S,T) and
Figure PCTCN2022072247-appb-000034
Determine, at this time d i '(P, T) is the minimum value of the distance to the target point before the current time i. In other cases, it is d i '(P,T)=d i (P,T).
设一距离阈值d m，若在某一时刻i，d i(P，T)-d′ i-1(P，T)＞d m成立，那么认为存在局部最近点M i，随即转换为环绕障碍物状态。当在任何情况下均令d i’(P,T)＝d i(P,T)，且d m＝0时，对应定位***无误差的理想情况。d m应结合定位***的误差取一较小的正值。 Assume a distance threshold d m , if i, d i (P, T)-d′ i-1 (P, T) > d m is established at a certain time, then it is considered that there is a local closest point Mi , and then it is converted into a surrounding Obstacle status. When d i '(P, T)=d i (P, T) in any case, and d m =0, it corresponds to an ideal situation that the positioning system has no error. d m should take a small positive value in combination with the error of the positioning system.
本实施例中环绕障碍物状态的流程图见图6。在此状态下首先更新局部最近点M i。设点M i到目标点的距离为d min，在初始化时令d min＝∞。若
Figure PCTCN2022072247-appb-000035
成立，则更新d min＝d(p,t)，并记录点p在惯性坐标系下的坐标为局部最近点M i和此时在惯性坐标系下的车辆坐标为的车辆状态转换点SW i。 The flowchart of the state of surrounding obstacles in this embodiment is shown in FIG. 6 . In this state, the local closest point Mi is first updated. The distance from the point Mi to the target point is set as d min , and d min =∞ during initialization. like
Figure PCTCN2022072247-appb-000035
If established, then update d min =d(p,t), and record the coordinates of point p in the inertial coordinate system as the local closest point Mi and the vehicle state transition point SW i in the inertial coordinate system at this time .
设一集合L，有：L＝{l∈F|d(l，t)＜d min} Suppose a set L, there are: L={l∈F|d(l, t)<d min }
若
Figure PCTCN2022072247-appb-000036
使得从q o到q li生成的Dubins曲线通过碰撞检测，令d leave＝d(l i,t)，那么d leave＜d min成立，即在集合F中存在一元素比局部最近点到目标的距离更小，且生成的Dubins曲线通过碰撞检测。随后车辆转换为朝目标移动状态。记此时在惯性坐标系下的车辆坐标为点L i，l i的坐标为L Ri。在环绕障碍物状态下，d leave需要实时更新，在初始化时d leave＝d(S,T)。 like
Figure PCTCN2022072247-appb-000036
Make the Dubins curve generated from q o to q li pass the collision detection, let d leave = d(li , t), then d leave <d min is established, that is, there is an element in the set F that is closer to the target than the local closest point The distance is smaller and the resulting Dubins curve passes collision detection. The vehicle then transitions to a state of moving towards the target. Note that the vehicle coordinate in the inertial coordinate system at this time is point Li , and the coordinate of li is L Ri . In the state of surrounding obstacles, d leave needs to be updated in real time, and d leave = d(S, T) during initialization.
环绕障碍物的方向由状态转换时记录的目标方位确定，若此时atan2(t,o)<0，则环绕方向为顺时针，否则为逆时针。以逆时针为例，如图3所示，车辆从状态转换点SW 2处开始逆时针环绕障碍物，直到点L 2处驶离障碍物。为防止在环绕障碍物状态下出现规划路径的局部振荡，以仅考虑自身前方信息的集合F’作为规划参考点集合。逆时针环绕时，按方位角由大到小的顺序对集合F'中的 元素进行排序，而顺时针时，按由小到大的顺序排序。依次生成Dubins曲线并进行碰撞检测，一旦通过则以此作为参考路径，否则认为没有安全的路径并停车。 The direction of the surrounding obstacles is determined by the target orientation recorded during the state transition. If atan2(t,o)<0 at this time, the surrounding direction is clockwise, otherwise it is counterclockwise. Taking counterclockwise as an example, as shown in FIG. 3 , the vehicle starts to circle around the obstacle counterclockwise from the state transition point SW 2 until it runs away from the obstacle at point L 2 . In order to prevent the local oscillation of the planned path in the state of surrounding obstacles, the set F' that only considers the information in front of itself is used as the set of planning reference points. When revolving counterclockwise, the elements in the set F' are sorted in descending order of azimuth, and in clockwise order, they are sorted in descending order. Dubins curves are generated in turn and collision detection is performed. Once passed, this is used as a reference path, otherwise it is considered that there is no safe path and the vehicle stops.
对于环绕障碍物一周的情况，由以下规则判断：For the circumstance around the obstacle, it is judged by the following rules:
(1)若存在一车辆位置o，满足d(sw i,o)>d sw且|atan2(sw i,o)|>π/2，则认为车辆驶离状态转换点SW i，其中点sw i为点SW i在车辆坐标系下的位置，d sw为距离阈值。 (1) If there is a vehicle position o that satisfies d(swi , o )>d sw and |atan2(swi , o )|>π/2, then it is considered that the vehicle leaves the state transition point SW i , where the point sw i is the position of the point SW i in the vehicle coordinate system, and d sw is the distance threshold.
(2)在满足条件(1)的情况下，若存在一车辆位置o，满足d(sw i,o)<d sw且|atan2(sw i,o)|<π/2，则认为车辆再次回到状态转换点SW i。 (2) In the case of satisfying the condition (1), if there is a vehicle position o, which satisfies d(sw i ,o)<d sw and |atan2( swi ,o)|<π/2, it is considered that the vehicle is once again Return to the state transition point SW i .
当条件(1)和(2)均满足时，认为车辆绕障碍物一周回到了状态转换点SW i，即得出目标不可到达的结论并停车，此时令unreachable＝true。 When both conditions (1) and (2) are satisfied, it is considered that the vehicle circles the obstacle and returns to the state transition point SW i , that is, it is concluded that the target is unreachable and the vehicle stops. At this time, set unreachable=true.
本发明设涉及到的变量的物理含义如表1所示：The present invention sets the physical meaning of the variables involved as shown in Table 1:
表1变量参照表Table 1 Variable Reference Table
Figure PCTCN2022072247-appb-000037
Figure PCTCN2022072247-appb-000037
Figure PCTCN2022072247-appb-000038
Figure PCTCN2022072247-appb-000038
本发明提供的无人车辆局部路径规划方法具有以下优势：The local path planning method for an unmanned vehicle provided by the present invention has the following advantages:
1、本发明通过融合TangentBug和Dubins曲线，针对仅能获取局部地图和定位信息的情况，提出了一种局部路径规划方法，仅需获取局部地图信息以 及车辆自身和目标点的位姿，无需全局地图等信息。1. The present invention proposes a local path planning method by fusing TangentBug and Dubins curves, aiming at the situation where only local map and positioning information can be obtained, which only needs to obtain local map information and the pose of the vehicle itself and the target point, without the need for global map and other information.
2、本发明提出了一种用于构建局部规划参考点集合的方法，无需建立局部切线图。对测距传感器的精度没有过高的要求，对障碍物的形状没有要求。2. The present invention proposes a method for constructing a local planning reference point set without establishing a local tangent graph. There is no high requirement for the accuracy of the ranging sensor, and no requirement for the shape of the obstacle.
3、基于第2点，本发明以Dubins曲线作为规划路径，可以满足车辆存在最小转向半径的运动限制和目标点处的航向要求。3. Based on the second point, the present invention uses the Dubins curve as the planning path, which can meet the motion restriction of the vehicle with the minimum turning radius and the heading requirement at the target point.
4、基于第3点，本发明考虑了车辆的几何外形，沿规划路径进行严格的碰撞检测，可以满足规划路径的安全性要求。4. Based on the third point, the present invention takes the geometric shape of the vehicle into consideration, and performs strict collision detection along the planned path, which can meet the safety requirements of the planned path.
5、本发明结合TangentBug的证明结果，提出了适应定位***误差的行为状态转换规则，提高了本发明算法对定位误差的鲁棒性。5. The present invention proposes a behavior-state transition rule adapting to the positioning system error in combination with the proof result of TangentBug, which improves the robustness of the algorithm of the present invention to the positioning error.
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments, and the same and similar parts between the various embodiments can be referred to each other.
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。In this paper, specific examples are used to illustrate the principles and implementations of the present invention. The descriptions of the above embodiments are only used to help understand the methods and core ideas of the present invention; meanwhile, for those skilled in the art, according to the present invention There will be changes in the specific implementation and application scope. In conclusion, the contents of this specification should not be construed as limiting the present invention.

Claims (5)

  1. 一种无人车辆局部路径规划方法，其特征在于，包括：A method for local path planning for an unmanned vehicle, comprising:
    通过测距传感器获取障碍物信息，构建规划参考点集合和局部地图，所述规划参考点集合包括位于测距传感器探测边界上的点、基于障碍物边界点旋转采样后的点以及目标点，所述障碍物边界点为经过车辆坐标点的直线与障碍物边缘的切点，所述局部地图为描述车辆附近空间被障碍物占据情况的栅格地图；The obstacle information is obtained by the ranging sensor, and a planning reference point set and a local map are constructed. The boundary point of the obstacle is the tangent point between the line passing through the coordinate point of the vehicle and the edge of the obstacle, and the local map is a grid map describing the situation that the space near the vehicle is occupied by obstacles;
    选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线；Select the elements in the planning reference point set as the planning end point, and generate the planning path Dubins curve;
    对规划路径进行碰撞检测；通过碰撞检测的规划路径作为运动控制的参考路径；Collision detection is performed on the planned path; the planned path through collision detection is used as the reference path for motion control;
    其中，通过测距传感器获取的障碍物信息，构建规划参考点集合，具体包括：Among them, the obstacle information obtained by the ranging sensor is used to construct a set of planning reference points, which specifically includes:
    测距传感器在各个方向上获取的点的集合
    Figure PCTCN2022072247-appb-100001
    Figure PCTCN2022072247-appb-100002
    集合S′中包括测距传感器探测边界上的点b，其构成的集合为B＝{b∈S′|d(b,o)＝ρ max|；障碍物边界点e cw，其构成的集合为E cw；障碍物边界点e ccw，其构成的集合为E ccw；其中，E cw＝{e cw∈S′||d(s n-j,o)-d(e cw,o)|≥d e,|d(s n+j,o)-d(e cw,o)|<d e,j＝1,2,…,m}，E ccw＝{e ccw∈S′||d(s n-j,o)-d(e ccw,o)|<d e,|d(s n+j,o)-d(e ccw,o)|≥d e,j＝1,2,…,m}，ρ max为测距传感器的最大探测距离，s n∈S′，s n-1,s n-2,...,s n-m表示s n顺时针方向上相邻的m个点，s n+1,s n+2,...,s n+m表示s n逆时针方向上相邻的m个点，atan2(a,b)表示点a相对于点b的方位角；d(a,b)表示点a与点b之间的距离，
    Figure PCTCN2022072247-appb-100003
    为测距传感器的角度分辨率，o为车辆在车辆坐标系下的位置点，
    Figure PCTCN2022072247-appb-100004
    为实际应用中测距传感器获取的障碍物距离，
    Figure PCTCN2022072247-appb-100005
    为n倍
    Figure PCTCN2022072247-appb-100006
    的角度值，d e为用于障碍物边界点判断的距离阈值；     A collection of points acquired by the ranging sensor in all directions
    Figure PCTCN2022072247-appb-100001
    Figure PCTCN2022072247-appb-100002
    The set S' includes the point b on the detection boundary of the ranging sensor, and the set formed by it is B={b∈S'|d(b,o)=ρ max |; the obstacle boundary point e cw , the set formed by is E cw ; the obstacle boundary point e ccw , the set formed by it is E ccw ; wherein, E cw ={e cw ∈S′||d(s nj ,o)-d(e cw ,o)|≥d e ,|d(s n+j ,o)-d(e cw ,o)|<d e ,j=1,2,…,m}, E ccw ={e ccw ∈S′||d(s nj ,o)-d(e ccw ,o)|<d e ,|d(s n+j ,o)-d(e ccw ,o)|≥d e ,j=1,2,…,m} , ρ max is the maximum detection distance of the ranging sensor, s n ∈ S′, s n-1 , s n-2 ,..., s nm represent m points adjacent in the clockwise direction of s n , s n +1 ,s n+2 ,...,s n+m represents m points adjacent to s n in the counterclockwise direction, atan2(a,b) represents the azimuth angle of point a relative to point b; d(a ,b) represents the distance between point a and point b,
    Figure PCTCN2022072247-appb-100003
    is the angular resolution of the ranging sensor, o is the position point of the vehicle in the vehicle coordinate system,
    Figure PCTCN2022072247-appb-100004
    is the obstacle distance obtained by the ranging sensor in practical applications,
    Figure PCTCN2022072247-appb-100005
    n times
    Figure PCTCN2022072247-appb-100006
    The angle value of d e is the distance threshold used to judge the boundary point of the obstacle;
    基于所述障碍物边界点进行旋转采样，得到顺时针采样点集合E′ cw和逆时针采样点集合E′ ccw，其中，
    Figure PCTCN2022072247-appb-100007
    Figure PCTCN2022072247-appb-100008
    Figure PCTCN2022072247-appb-100009
    e′ cw为集合E′ cw中的元素；e′ ccw为集合E′ ccw中的元素；θ为旋转角度；R为旋转矩阵，R(θ)表示逆时针旋转θ角度，R(-θ)表示顺时针旋转θ角度；θ safe为旋 转角度的下限；θ sample为旋转角度的上限；h为旋转采样的步长；     Rotation sampling is performed based on the obstacle boundary points to obtain a clockwise sampling point set E′ cw and a counterclockwise sampling point set E′ ccw , wherein,
    Figure PCTCN2022072247-appb-100007
    Figure PCTCN2022072247-appb-100008
    Figure PCTCN2022072247-appb-100009
    e' cw is the element in the set E'cw;e' ccw is the element in the set E'ccw; θ is the rotation angle; R is the rotation matrix, R(θ) represents the counterclockwise rotation θ angle, R(-θ) Represents a clockwise rotation of θ angle; θ safe is the lower limit of the rotation angle; θ sample is the upper limit of the rotation angle; h is the step size of the rotation sampling;
    确定规划参考点集合F和仅考虑车辆前方信息的规划参考点集合F′，其中，F＝{f|f∈B∪E′ ccw∪E′ cw∪T′}，
    Figure PCTCN2022072247-appb-100010
    T′为目标点的集合，若目标点T位于测距传感器探测范围内，记目标点T在车辆坐标系下的坐标点为t，则T′＝{t}，否则
    Figure PCTCN2022072247-appb-100011
    Determine a set of planning reference points F and a set of planning reference points F' that only considers the information ahead of the vehicle, where F={f|f∈B∪E' ccw ∪E' cw ∪T'},
    Figure PCTCN2022072247-appb-100010
    T' is the set of target points. If the target point T is within the detection range of the ranging sensor, mark the coordinate point of the target point T in the vehicle coordinate system as t, then T'={t}, otherwise
    Figure PCTCN2022072247-appb-100011
    所述选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线，具体包括：The elements in the set of planning reference points are selected as the planning end point, and the Dubins curve of the planning path is generated, which specifically includes:
    当满足以下条件时，车辆转换为朝目标移动状态：1)车辆处于初始时刻或2)车辆处于环绕障碍物状态，且集合F中存在一元素比已确定的局部最近点M到目标点的距离更小，且基于所述元素生成的Dubins曲线能够通过碰撞检测，其中，所述局部最近点M为测距传感器探测到的位于障碍物上距离目标位置最近的点；When the following conditions are met, the vehicle transitions to a state of moving toward the target: 1) the vehicle is at the initial moment or 2) the vehicle is in a state of surrounding obstacles, and there is an element in the set F that is greater than the distance from the determined local closest point M to the target point is smaller, and the Dubins curve generated based on the element can pass collision detection, wherein the local closest point M is the point detected by the ranging sensor and located on the obstacle closest to the target position;
    在所述朝目标移动状态中：In the move-to-goal state:
    确定集合F中各元素的代价，所述代价根据所述元素到车辆当前位置的距离以及所述元素到目标位置的距离确定；Determine the cost of each element in the set F, the cost is determined according to the distance from the element to the current position of the vehicle and the distance from the element to the target position;
    规划Dubins曲线：选取当前集合F中代价最小的元素，以车辆的位姿为起点，以选取元素的位姿为终点，规划Dubins曲线；其中，终点的航向为选取元素相对车辆的方位角；Planning the Dubins curve: Select the element with the least cost in the current set F, take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
    如果规划出的Dubins曲线不能通过碰撞检测，则在集合F中排除当前参考元素，跳转至规划Dubins曲线步骤，直至规划出的Dubins曲线通过碰撞检测时或不存在能够通过碰撞检测的Dubins曲线时停止跳转。If the planned Dubins curve cannot pass the collision detection, exclude the current reference element in the set F and jump to the planning Dubins curve step until the planned Dubins curve passes the collision detection or when there is no Dubins curve that can pass the collision detection Stop jumping.
  2. 根据权利要求1所述的无人车辆局部路径规划方法，其特征在于，所述选取规划参考点集合中的元素作为规划终点，生成规划路径Dubins曲线，具体包括：The method for planning a local path of an unmanned vehicle according to claim 1, wherein the selecting an element in the set of planning reference points as the planning end point, and generating the Dubins curve of the planning path, specifically includes:
    当满足以下条件时，车辆转换为环绕障碍物状态：车辆处于朝目标移动状态，且当前时刻定位***反馈的车辆位置与目标点间的距离和当前时刻之前记录的车辆与目标点间的最近距离之差大于设定的距离阈值；When the following conditions are met, the vehicle transitions to the state of surrounding obstacles: the vehicle is moving towards the target, and the distance between the vehicle position and the target point fed back by the positioning system at the current moment and the closest distance between the vehicle and the target point recorded before the current moment The difference is greater than the set distance threshold;
    在所述环绕障碍物状态中，若车辆由朝目标移动状态转换为环绕障碍物状态时的目标位置处于车辆当前位置的右侧，则按顺时针方向环绕障碍物，对集合F′中的元素按方位角由小到大排序，否则按逆时针方向环绕障碍物，对集合F′中的元素按方位角由大到小排序；In the surrounding obstacle state, if the target position when the vehicle is transformed from the moving state toward the target to the surrounding obstacle state is on the right side of the current position of the vehicle, the obstacle is clockwise around the obstacle, and the elements in the set F' Sort by the azimuth angle from small to large, otherwise circle the obstacles in the counterclockwise direction, and sort the elements in the set F' according to the azimuth angle from large to small;
    规划Dubins曲线：选取集合F′中排序最前的元素，以车辆的位姿为起点，以选取元素的位姿为终点，规划Dubins曲线；其中，终点的航向为选取元素相对车辆的方位角；Planning the Dubins curve: Select the top element in the set F', take the pose of the vehicle as the starting point, and take the pose of the selected element as the end point to plan the Dubins curve; the heading of the end point is the azimuth of the selected element relative to the vehicle;
    如果Dubins曲线不能通过碰撞检测，则在集合F′中排除当前参考元素，跳转至规划Dubins曲线步骤，直至规划出的Dubins曲线通过检测碰撞或不存在能够通过碰撞检测的Dubins曲线时停止跳转。If the Dubins curve cannot pass the collision detection, exclude the current reference element from the set F', and jump to the step of planning the Dubins curve, until the planned Dubins curve passes the collision detection or there is no Dubins curve that can pass the collision detection. .
  3. 根据权利要求1所述的无人车辆局部路径规划方法，其特征在于，所述方法还包括：在朝目标移动状态时，除按目标点的位姿规划出的Dubins曲线通过碰撞检测外，根据d′ 0(P,T)＝d 0(P,T),i＝0、
    Figure PCTCN2022072247-appb-100012
    Figure PCTCN2022072247-appb-100013
    i∈N *确定当前时刻i之前车辆P与目标点T之间欧式距离的最小值d′ i(P,T)，其中，d i(P,T)为i时刻定位***反馈的车辆位置P与目标位置T之间的距离，d 0(P,T)为初始时刻定位***反馈的车辆位置P与目标位置T之间的距离，N *表示正整数。     The method for local path planning for an unmanned vehicle according to claim 1, wherein the method further comprises: when moving toward the target, in addition to the collision detection of the Dubins curve planned according to the pose of the target point, according to d′ 0 (P, T)=d 0 (P, T), i=0,
    Figure PCTCN2022072247-appb-100012
    Figure PCTCN2022072247-appb-100013
    i∈N * Determine the minimum value d′ i (P, T) of the Euclidean distance between the vehicle P and the target point T before the current time i, where d i (P, T) is the vehicle position P fed back by the positioning system at time i The distance from the target position T, d 0 (P, T) is the distance between the vehicle position P fed back by the positioning system at the initial moment and the target position T, and N * represents a positive integer.
  4. 根据权利要求2所述的无人车辆局部路径规划方法，其特征在于，所述方法还包括：The method for local path planning for an unmanned vehicle according to claim 2, wherein the method further comprises:
    在所述环绕障碍物状态中，当检测到车辆环绕障碍物一周时，确定目标不可到达。In the obstacle-surrounding state, when it is detected that the vehicle circles around the obstacle, it is determined that the target is unreachable.
  5. 根据权利要求1或2所述的无人车辆局部路径规划方法，其特征在于，所述方法还包括：The method for local path planning for an unmanned vehicle according to claim 1 or 2, wherein the method further comprises:
    若目标位置位于测距传感器的探测范围内，则按目标点处规定的航向生成Dubins曲线并进行碰撞检测，否则执行所述朝目标移动状态中规划Dubins曲线步骤并进行碰撞检测。If the target position is within the detection range of the ranging sensor, a Dubins curve is generated according to the specified heading at the target point and collision detection is performed; otherwise, the step of planning a Dubins curve in the moving state toward the target is performed and collision detection is performed.
PCT/CN2022/072247 2021-01-15 2022-01-17 Local path planning method for unmanned vehicle WO2022152283A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202110051442.9 2021-01-15
CN202110051442.9A CN112379679B (en) 2021-01-15 2021-01-15 Unmanned vehicle local path planning method

Publications (1)

Publication Number Publication Date
WO2022152283A1 true WO2022152283A1 (en) 2022-07-21

Family

ID=74581833

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/072247 WO2022152283A1 (en) 2021-01-15 2022-01-17 Local path planning method for unmanned vehicle

Country Status (2)

Country Link
CN (1) CN112379679B (en)
WO (1) WO2022152283A1 (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115451988A (en) * 2022-09-29 2022-12-09 国能宝日希勒能源有限公司 Path planning method, device and navigator thereof
CN115912183A (en) * 2023-03-09 2023-04-04 国网湖北省电力有限公司经济技术研究院 High-voltage transmission line ecological measure inspection method and system and readable storage medium
CN116429145A (en) * 2023-06-07 2023-07-14 福龙马城服机器人科技有限公司 Automatic docking navigation method and system for unmanned vehicle and garbage can under complex scene
CN117472066A (en) * 2023-12-27 2024-01-30 成都流体动力创新中心 Obstacle avoidance control method with locally optimal course angular velocity

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112379679B (en) * 2021-01-15 2021-04-23 北京理工大学 Unmanned vehicle local path planning method
CN113050684B (en) * 2021-03-06 2022-05-24 南京航空航天大学 Emergency threat-oriented unmanned aerial vehicle track planning algorithm
CN113485453B (en) * 2021-08-20 2024-05-10 中国华能集团清洁能源技术研究院有限公司 Method and device for generating inspection flight path of marine unmanned aerial vehicle and unmanned aerial vehicle
CN113819917A (en) * 2021-09-16 2021-12-21 广西综合交通大数据研究院 Automatic driving path planning method, device, equipment and storage medium
CN113985881B (en) * 2021-10-29 2023-07-11 温州大学 Mobile robot path planning method based on bidirectional crawler
TWI828330B (en) * 2022-09-23 2024-01-01 優式機器人股份有限公司 Movement control method and system for an automated guided apparatus
CN115540892B (en) * 2022-11-28 2023-03-24 北京理工大学深圳汽车研究院(电动车辆国家工程实验室深圳研究院) Obstacle-detouring terminal point selection method and system for fixed line vehicle

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121517A1 (en) * 2008-11-10 2010-05-13 Electronics And Telecommunications Research Institute Method and apparatus for generating safe path of mobile robot
CN103278164A (en) * 2013-06-13 2013-09-04 北京大学深圳研究生院 Planning method for simulated path of robot under complex dynamic scene and simulation platform
CN103455034A (en) * 2013-09-16 2013-12-18 苏州大学张家港工业技术研究院 Avoidance path planning method based on closest distance vector field histogram
CN107340768A (en) * 2016-12-29 2017-11-10 珠海市微半导体有限公司 A kind of paths planning method of intelligent robot
CN109540159A (en) * 2018-10-11 2019-03-29 同济大学 A kind of quick complete automatic Pilot method for planning track
CN111301409A (en) * 2020-03-11 2020-06-19 中国第一汽车股份有限公司 Parking path planning method and device, vehicle and storage medium
CN112379679A (en) * 2021-01-15 2021-02-19 北京理工大学 Unmanned vehicle local path planning method

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110346814B (en) * 2018-04-08 2023-04-28 浙江国自机器人技术有限公司 Obstacle detection and obstacle avoidance control method and system based on 3D laser
CN111338352A (en) * 2020-03-24 2020-06-26 云南和富科技有限公司 Robot autonomous path planning method

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121517A1 (en) * 2008-11-10 2010-05-13 Electronics And Telecommunications Research Institute Method and apparatus for generating safe path of mobile robot
CN103278164A (en) * 2013-06-13 2013-09-04 北京大学深圳研究生院 Planning method for simulated path of robot under complex dynamic scene and simulation platform
CN103455034A (en) * 2013-09-16 2013-12-18 苏州大学张家港工业技术研究院 Avoidance path planning method based on closest distance vector field histogram
CN107340768A (en) * 2016-12-29 2017-11-10 珠海市微半导体有限公司 A kind of paths planning method of intelligent robot
CN109540159A (en) * 2018-10-11 2019-03-29 同济大学 A kind of quick complete automatic Pilot method for planning track
CN111301409A (en) * 2020-03-11 2020-06-19 中国第一汽车股份有限公司 Parking path planning method and device, vehicle and storage medium
CN112379679A (en) * 2021-01-15 2021-02-19 北京理工大学 Unmanned vehicle local path planning method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
ZHANG XUDONG, XU FUKANG;ZOU YUAN;GUO NINGYUAN;ZHANG YU: "A Local Path Planning Algorithm for Intelligent Wheeled Vehicle Combining TangentBug and Dubins Path", AUTOMOTIVE ENGINEERING, vol. 43, no. 6, 25 June 2021 (2021-06-25), pages 833 - 841+869, XP055952000, DOI: 10.19562/j.chinasae.qcgc.2021.06.006 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115451988A (en) * 2022-09-29 2022-12-09 国能宝日希勒能源有限公司 Path planning method, device and navigator thereof
CN115451988B (en) * 2022-09-29 2024-05-17 国能宝日希勒能源有限公司 Path planning method, device and navigator
CN115912183A (en) * 2023-03-09 2023-04-04 国网湖北省电力有限公司经济技术研究院 High-voltage transmission line ecological measure inspection method and system and readable storage medium
CN116429145A (en) * 2023-06-07 2023-07-14 福龙马城服机器人科技有限公司 Automatic docking navigation method and system for unmanned vehicle and garbage can under complex scene
CN116429145B (en) * 2023-06-07 2023-08-25 福龙马城服机器人科技有限公司 Automatic docking navigation method and system for unmanned vehicle and garbage can under complex scene
CN117472066A (en) * 2023-12-27 2024-01-30 成都流体动力创新中心 Obstacle avoidance control method with locally optimal course angular velocity
CN117472066B (en) * 2023-12-27 2024-03-26 成都流体动力创新中心 Obstacle avoidance control method with locally optimal course angular velocity

Also Published As

Publication number Publication date
CN112379679B (en) 2021-04-23
CN112379679A (en) 2021-02-19

Similar Documents

Publication Publication Date Title
WO2022152283A1 (en) Local path planning method for unmanned vehicle
JP7060625B2 (en) LIDAR positioning to infer solutions using 3DCNN network in self-driving cars
Liu et al. Situation-aware decision making for autonomous driving on urban road using online POMDP
CN109270933B (en) Unmanned obstacle avoidance method, device, equipment and medium based on conic curve
Li et al. Multivehicle cooperative local mapping: A methodology based on occupancy grid map merging
US20190056743A1 (en) Method for motion planning for autonomous moving objects
Kim et al. Curvilinear-coordinate-based object and situation assessment for highly automated vehicles
CN108444488A (en) Based on etc. steps sample A* algorithms unmanned local paths planning method
CN111811517A (en) Dynamic local path planning method and system
CN103335658A (en) Autonomous vehicle obstacle avoidance method based on arc path
Park et al. Feedback motion planning via non-holonomic RRT* for mobile robots
Sgorbissa Integrated robot planning, path following, and obstacle avoidance in two and three dimensions: Wheeled robots, underwater vehicles, and multicopters
Bresson et al. A cooperative fusion architecture for robust localization: Application to autonomous driving
Chen et al. Path planning for autonomous vehicle based on a two-layered planning model in complex environment
Blaich et al. Probabilistic collision avoidance for vessels
Wu et al. Infrastructure-free hierarchical mobile robot global localization in repetitive environments
Yu et al. Semantic evidential lane grids with prior maps for autonomous navigation
Zhao et al. Online trajectory planning for an industrial tractor towing multiple full trailers
Xu et al. Potential gap: A gap-informed reactive policy for safe hierarchical navigation
Mutz et al. Following the leader using a tracking system based on pre-trained deep neural networks
Hoy A method of boundary following by a wheeled mobile robot based on sampled range information
Yu et al. RDT-RRT: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles
Liu et al. DVL dead-reckoning navigation method based on beam measurements
Yoon et al. High-definition map based motion planning, and control for urban autonomous driving
Hoy Methods for collision-free navigation of multiple mobile robots in unknown cluttered environments

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: 22739158

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22739158

Country of ref document: EP

Kind code of ref document: A1