CN109916400B - An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method - Google Patents
An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO MethodDownload PDFInfo
- Publication number
- CN109916400B CN109916400BCN201910283660.8ACN201910283660ACN109916400BCN 109916400 BCN109916400 BCN 109916400BCN 201910283660 ACN201910283660 ACN 201910283660ACN 109916400 BCN109916400 BCN 109916400B
- Authority
- CN
- China
- Prior art keywords
- speed
- obstacle
- unmanned boat
- gradient
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
Translated fromChinese技术领域technical field
本发明属于无人艇技术领域，具体涉及一种基于梯度下降算法与VO法相结合的无人艇避障方法。The invention belongs to the technical field of unmanned boats, and in particular relates to an obstacle avoidance method for unmanned boats based on a combination of a gradient descent algorithm and a VO method.
背景技术Background technique
水面无人艇，简称无人艇，是一种轻型智能水面运载工具，具有体积小、造价低、速度快、机动性强等特点。随着控制技术、传感技术、无线通信技术的进步，水面无人艇获得了很大的发展。通过搭载不同的设备，无人艇可以应用在不同的领域，比如，当搭载单波束、多波束、浅底层剖面仪等声呐设备时，可以用于海底测绘、探雷反潜等；当搭载水质采样或检测设备时，可以用于环境检测；当搭载武器时，可以用于区域巡逻、海岸保护、护航、作战等任务。Surface unmanned boat, referred to as unmanned boat for short, is a light intelligent surface vehicle with the characteristics of small size, low cost, high speed and strong maneuverability. With the advancement of control technology, sensing technology and wireless communication technology, surface unmanned boats have achieved great development. By carrying different equipment, unmanned boats can be applied in different fields. For example, when carrying single-beam, multi-beam, shallow bottom profiler and other sonar equipment, it can be used for seabed mapping, mine detection and anti-submarine detection, etc.; when carrying water quality sampling Or detection equipment, it can be used for environmental detection; when carrying weapons, it can be used for regional patrol, coast protection, escort, combat and other tasks.
要想保证无人艇能够在海洋中正常安全地航行，无人艇必须能够对航行过程中遇到的岛屿、暗礁、灯塔、浮标和航行的船只等其他障碍物进行自主避障。这是无人艇航行的重中之重，而避障算法又是安全航行的重点。所以无人艇要想安全航行的前提是要有一个优秀的避障算法。目前无人艇通常采用的避障算法有很多，如人工势场法、VelocityObstacle方法(速度障碍法，简称VO法)等，与其他避障算法相比，VO法更加可靠。VO法的前提是设定障碍物的速度和方向是保持不变的，并且将无人艇的形状大小结合到障碍物层面，然后将无人艇等效为一个质点，将障碍物的形状视为圆形，即将障碍物与无人艇的碰撞问题等效为质点与圆盘的接触问题。VO法的核心就是在通过无人艇的位置与障碍物的相对位移位置，依据障碍物和无人艇的综合形状，形成一个锥形区域，该区域简称为VO区域，如图1所示，图1中阴影填充部分为VO区域，在如果无人艇相对于障碍物的相对速度落在VO区域内，则在将来的一定时间内导致无人艇和障碍物发生碰撞，属于不安全速度区，如果无人艇相对于障碍物的相对速度位于VO区域之外，则无人艇与障碍物不会发生碰撞，属于安全速度区。VO法是一种基于速度空间选择非VO区域以外最优解的算法，即在VO区域以外，基于导航避障任务获取最优的避障速度大小和舵角，并将获取的最优速度大小和舵角输入到无人艇底层控制模块，控制无人艇按照获取的最优避障速度大小和舵角行驶，避开障碍物。In order to ensure the normal and safe navigation of unmanned boats in the ocean, unmanned boats must be able to autonomously avoid obstacles such as islands, reefs, lighthouses, buoys, and sailing ships encountered during navigation. This is the top priority of unmanned boat navigation, and the obstacle avoidance algorithm is the focus of safe navigation. Therefore, the premise of the safe navigation of unmanned boats is to have an excellent obstacle avoidance algorithm. At present, there are many obstacle avoidance algorithms commonly used by unmanned boats, such as artificial potential field method, Velocity Obstacle method (velocity obstacle method, VO method for short), etc. Compared with other obstacle avoidance algorithms, VO method is more reliable. The premise of the VO method is to set the speed and direction of the obstacle to remain unchanged, and combine the shape and size of the unmanned boat to the obstacle level, and then use the unmanned boat as a particle, and consider the shape of the obstacle as a particle. It is a circle, that is, the collision problem between obstacles and unmanned boats is equivalent to the contact problem between particle and disk. The core of the VO method is to form a conical area between the position of the passing unmanned boat and the relative displacement position of the obstacle, according to the comprehensive shape of the obstacle and the unmanned boat. This area is referred to as the VO area, as shown in Figure 1. The shaded part in Figure 1 is the VO area. If the relative speed of the unmanned boat relative to the obstacle falls within the VO area, it will cause the unmanned boat and the obstacle to collide within a certain period of time in the future, which belongs to the unsafe speed area. , if the relative speed of the unmanned boat relative to the obstacle is outside the VO area, the unmanned boat and the obstacle will not collide and belong to the safe speed area. The VO method is an algorithm that selects the optimal solution outside the non-VO area based on the speed space, that is, outside the VO area, the optimal obstacle avoidance speed and rudder angle are obtained based on the navigation obstacle avoidance task, and the obtained optimal speed is calculated. And the rudder angle is input to the bottom control module of the unmanned boat to control the unmanned boat to drive according to the obtained optimal obstacle avoidance speed and rudder angle to avoid obstacles.
对于在海洋航行的无人艇，不但要保证无人艇的安全避障，而且还要保证无人艇的平稳航行，我们在实时避障的过程中要保证上层导航避障模块输出给底层控制模块的速度大小和舵角是连续变化的，因为如果相邻两次的导航避障输出存在大角度变化容易导致无人艇的侧翻，而且避障过程中，如果舵角输出发生间断变化，容易导致船体的左右摇摆，所以保证无人艇避障输出的稳定性变化对于无人艇的稳定性航行至关重要。VO法是基于VO区域获取全局最优解(最优的避障速度大小和舵角)，由于VO法获取的VO区域会随着无人艇和障碍物的相对位置发生的变化而变化，当VO区域发生变化，那么基于VO区域获取的全局最优解也将会发生变化，最优算法会寻找全局最优，而VO法对应的极小值不止一个，从而引起前后两次获取的最优解对应不同的极小值，致使前后两次获取的最优解发生突变(即前后两次避障输出的速度大小和舵角发生大范围变化)，进而导致无人艇的不稳定性航行(参见图2，图2为VO法避障仿真实验相邻两帧时刻对应的代价函数在速度空间的分布曲线图，通过该图我们可以发现代价函数空间存在两个极值点，在代价函数发生改变的过程中最优值会在两个极值点跳动进而导致对应避障输出的角度和速度跳动)。For unmanned boats sailing in the ocean, not only the safety and obstacle avoidance of the unmanned boat should be ensured, but also the smooth navigation of the unmanned boat. In the process of real-time obstacle avoidance, we must ensure that the upper-layer navigation and obstacle-avoidance module is output to the bottom control. The speed and rudder angle of the module change continuously, because if there is a large angle change in the two adjacent navigation obstacle avoidance outputs, it is easy to cause the unmanned boat to roll over, and during the obstacle avoidance process, if the rudder angle output changes intermittently, It is easy to cause the hull to sway left and right, so ensuring the stability of the obstacle avoidance output of the unmanned boat is very important for the stable navigation of the unmanned boat. The VO method is based on the VO area to obtain the global optimal solution (optimal obstacle avoidance speed and rudder angle). Since the VO area obtained by the VO method will change with the relative position of the UAV and the obstacle, when If the VO area changes, the global optimal solution obtained based on the VO area will also change. The optimal algorithm will find the global optimal solution, and the VO method has more than one minimum value, resulting in the optimal solution obtained twice before and after. The solutions correspond to different minimum values, resulting in a sudden change in the optimal solutions obtained twice before and after (that is, the speed and rudder angle of the two obstacle avoidance outputs change in a large range), which in turn leads to the unstable navigation of the unmanned boat ( Referring to Figure 2, Figure 2 is the distribution curve of the cost function corresponding to the two adjacent frames of the VO method obstacle avoidance simulation experiment in the speed space. From this figure, we can find that there are two extreme points in the cost function space. When the cost function occurs In the process of changing, the optimal value will jump at two extreme points, which will cause the angle and speed of the corresponding obstacle avoidance output to jump).
发明内容SUMMARY OF THE INVENTION
针对现有技术中存在的问题和不足，本发明的目的是提供一种基于梯度下降算法与VO法相结合的无人艇避障方法。Aiming at the problems and deficiencies in the prior art, the purpose of the present invention is to provide an obstacle avoidance method for an unmanned boat based on the combination of the gradient descent algorithm and the VO method.
为实现发明目的，本发明采用的技术方案如下：For realizing the purpose of the invention, the technical scheme adopted in the present invention is as follows:
一种基于梯度下降算法与VO法相结合的无人艇避障方法，包括以下步骤：An obstacle avoidance method for unmanned boats based on the combination of gradient descent algorithm and VO method, including the following steps:
(1)获取无人艇当前的位置、运动以及姿态信息，获取障碍物当前的位置、运动和尺寸信息，获取无人艇当前导航输出的速度和舵角(vlos,θlos)；(1) Obtain the current position, motion and attitude information of the unmanned boat, obtain the current position, motion and size information of the obstacle, and obtain the speed and rudder angle (vlos , θlos ) of the current navigation output of the unmanned boat;
(2)判断无人艇与障碍物之间是否存在碰撞风险，如果无人艇与障碍物之间不存在碰撞风险，则无人艇按导航输出的速度和舵角继续航行，如果无人艇与障碍物之间存在碰撞风险，则执行下一步操作；(2) Determine whether there is a collision risk between the unmanned boat and the obstacle. If there is no collision risk between the unmanned boat and the obstacle, the unmanned boat will continue to sail according to the speed and rudder angle output by the navigation. If there is a risk of collision with an obstacle, proceed to the next step;
(3)以无人艇当前导航输出的速度和舵角(vlos,θlos)作为初始值输入梯度下降算法程序，采用梯度下降算法程序在初始值的上下左右四个方向进行基于梯度的迭代搜索，获取初始值在上下左右四个方向的参考变量(vi+1,θj)、(vi-1,θj)、(vi,θj+1)和(vi,θj-1)；将获取的四个参考变量输入梯度值函数公式中，计算每一个参考变量对应的梯度值，然后判断每一个参考变量对应的梯度值是否满足梯度下降算法程序的迭代循环终止条件；经判断发现任意一个参考变量对应的梯度值满足迭代循环终止条件，则终止迭代循环，以满足迭代循环终止条件的梯度值对应的参考变量作为无人艇避障输出的速度和舵角传输至无人艇的控制模块；经判断发现每一个参考变量对应的梯度值均不满足迭代循环终止条件，则以四个参考变量作为梯度下降算法程序的初始值，继续在四个参考变量上、下、左、右四个方向进行基于梯度的迭代搜索，直至找到满足迭代循环终止条件的梯度值。(3) Use the speed and rudder angle (vlos , θlos ) of the current navigation output of the UAV as the initial value to input the gradient descent algorithm program, and use the gradient descent algorithm program to perform gradient-based iteration in the four directions of the initial value, up, down, left, and right Search to obtain the reference variables (vi+1 , θj ), (vi-1 , θj ), (vi , θj+ 1) and (vi, θj- 1 ); Input the obtained four reference variables into the gradient value function formula, calculate the gradient value corresponding to each reference variable, and then judge whether the gradient value corresponding to each reference variable satisfies the iterative loop termination condition of the gradient descent algorithm program; It is judged that the gradient value corresponding to any reference variable satisfies the termination condition of the iteration loop, then the iteration loop is terminated, and the reference variable corresponding to the gradient value that satisfies the termination condition of the iteration loop is transmitted as the speed and rudder angle of the obstacle avoidance output of the unmanned boat to the unmanned aerial vehicle. The control module of the boat; it is judged that the gradient value corresponding to each reference variable does not meet the termination condition of the iterative cycle, then the four reference variables are used as the initial value of the gradient descent algorithm program, and the four reference variables are continued to be up, down and left. The gradient-based iterative search is performed in the right four directions until the gradient value that satisfies the termination condition of the iterative loop is found.
根据上述的无人艇避障方法，优选地，步骤(3)中所述梯度下降算法程序的步长为：速度大小为0.1，角度大小为0.5°；梯度下降算法程序的迭代循环终止条件为：速度大小梯度值小于设定的最小速度梯度阈值0.01，同时速度方向(速度方向即舵角)梯度值小于设定的最小方向梯度阈值0.05。According to the above-mentioned obstacle avoidance method for unmanned boats, preferably, the step size of the gradient descent algorithm program in step (3) is: the speed size is 0.1, the angle size is 0.5°; the iterative loop termination condition of the gradient descent algorithm program is: : The speed gradient value is less than the set minimum speed gradient threshold of 0.01, and the speed direction (speed direction is the rudder angle) gradient value is less than the set minimum direction gradient threshold of 0.05.
根据上述的无人艇避障方法，优选地，步骤(3)中所述梯度值函数公式的具体构建步骤为：According to the above-mentioned unmanned boat obstacle avoidance method, preferably, the specific construction steps of the gradient value function formula described in step (3) are:
1)将速度空间进行离散，离散后的速度二维变量为(vi,θj)，其中，vi为速度大小，θj为速度方向，vi的范围为[0,20m/s]，θj的范围为[0,360°]；1) Discrete the velocity space, and the discrete velocity two-dimensional variable is (vi, θj ), where vi is the magnitude of the velocity, θj is the direction of the velocity, and the range of v iis [0,20m/s] , the range of θj is [0,360°];
2)将速度空间的速度二维变量与无人艇导航输出的速度二维变量的偏差作为导航任务的代价值，其取值区间为[0,0.5]；2) The deviation between the speed two-dimensional variable in the speed space and the speed two-dimensional variable output by the UAV navigation is taken as the cost value of the navigation task, and its value interval is [0, 0.5];
3)根据速度障碍法，位于VO区域的速度二维变量对应的代价值为1，位于VO区域以外的速度二维变量对应的代价值为0；3) According to the speed obstacle method, the cost value corresponding to the speed two-dimensional variable located in the VO area is 1, and the cost value corresponding to the speed two-dimensional variable located outside the VO area is 0;
4)海事规则约束的代价值存在灵活性，根据速度障碍法，在VO区域以外存在最优的避障速度二维变量时，将海事规则约束对应的速度二维变量排除在最优的避障速度二维变量之外，当VO区域之外不存在最优的避障速度二维变量时，以海事规则约束对应的速度二维变量作为最优的避障速度二维变量输出，因此，海事规则约束对应的代价值的取值区间为[0.5,1]；4) There is flexibility in the cost value of maritime rule constraints. According to the speed obstacle method, when there is an optimal two-dimensional obstacle avoidance speed variable outside the VO area, the speed two-dimensional variable corresponding to the maritime rule constraint is excluded from the optimal obstacle avoidance. In addition to the two-dimensional variable of speed, when there is no optimal two-dimensional variable of obstacle avoidance speed outside the VO area, the two-dimensional variable of speed corresponding to the constraints of maritime rules is used as the output of the two-dimensional variable of optimal obstacle avoidance speed. The value range of the cost value corresponding to the rule constraint is [0.5, 1];
5)根据导航任务、速度障碍法、海事规则约束三者的代价值取值区间和速度空间的速度二维变量范围之间的关系，构建得到目标代价函数计算公式，如式(I)所示：5) According to the relationship between the value interval of the cost value of the navigation task, the speed obstacle method and the maritime rule constraint and the range of the speed two-dimensional variable in the speed space, the calculation formula of the objective cost function is constructed and obtained, as shown in formula (I) :
6)将目标代价函数J对vi和θj分别求偏导，得到对应的偏导函数，即为梯度值函数，所述梯度值函数包括速度大小梯度值函数和速度方向梯度值函数，其中，目标代价函数J对vi求偏导得到的偏导函数为速度大小梯度值函数，如式(II)所示，目标代价函数J对θj求偏导得到的偏导函数为速度方向梯度值函数，如式(III)所示：6) Calculate the partial derivative of the objective cost function J with respect to vi and θj respectively, and obtain the corresponding partial derivative function, which is the gradient value function, and the gradient value function includes the velocity magnitude gradient value function and the velocity direction gradient value function, wherein , the partial derivative function obtained by the partial derivative of the objective cost function J with respect to vi is the velocity magnitude gradient value function, as shown in formula (II), the partial derivative function obtained by the partial derivative of the objective cost function J with respect to θj is the velocity direction gradient The value function, as shown in formula (III):
根据上述的无人艇避障方法，优选地，步骤(2)中判断无人艇与障碍物之间是否存在碰撞风险的具体操作为：计算无人艇与障碍物的最近会遇时间TCPA和最近会遇距离DCPA，当TCPA≤tmax且DCPA≤dmin时，其中tmax、dmin均为已知的参数，无人艇与障碍物之间存在碰撞风险，如果TCPA、DCPA不能同时满足上述条件，则无人艇与障碍物之间不存在碰撞风险；所述TCPA、DCPA的计算公式为：According to the above-mentioned obstacle avoidance method of the unmanned boat, preferably, in step (2), the specific operation of judging whether there is a collision risk between the unmanned boat and the obstacle is: calculating the latest meeting time TCPA and the time of the unmanned boat and the obstacle. The distance DCPA will be encountered recently. When TCPA≤tmax and DCPA≤dmin , where tmax and dmin are known parameters, there is a risk of collision between the unmanned boat and the obstacle. If TCPA and DCPA cannot meet the requirements at the same time Under the above conditions, there is no risk of collision between the unmanned boat and the obstacle; the calculation formulas of the TCPA and DCPA are:
DCPA＝||(PA+vA·TCPA)-(PB+vBTCPA)|| (V)DCPA=||(PA +vA ·TCPA)-(PB +vB TCPA)|| (V)
其中，PB和PA分别表示障碍物和无人艇的位置矢量，vB和vA分别表示在大地坐标系下的障碍物和无人艇的速度矢量。Among them, PB and PA represent the position vector of the obstacle and the unmanned boat, respectively, and vB and vA represent the velocity vector of the obstacle and the unmanned boat in the geodetic coordinate system, respectively.
根据上述的无人艇避障方法，优选地，步骤(1)的具体操作为：通过GPS和惯导传感器获取无人艇的位置、运动和姿态信息；通过无人艇自身携带的多传感器数据融合进行环境建模得到障碍物的位置，运动和尺寸信息，并且根据障碍物的不同尺寸将障碍物建模为大小不同的圆形状障碍物；根据LOS算法获取无人艇当前导航输出的速度和舵角(vlos,θlos)。According to the above-mentioned obstacle avoidance method for the unmanned boat, preferably, the specific operations of step (1) are: obtaining the position, motion and attitude information of the unmanned boat through GPS and inertial navigation sensors; using multi-sensor data carried by the unmanned boat itself Integrate the environment modeling to obtain the position, motion and size information of the obstacles, and model the obstacles as circular obstacles of different sizes according to the different sizes of the obstacles; obtain the speed and the current navigation output of the unmanned boat according to the LOS algorithm. Rudder angle (vlos , θlos ).
与现有技术相比，本发明取得的有益效果为：Compared with the prior art, the beneficial effects obtained by the present invention are:
(1)本发明采用梯度下降算法寻找无人艇避障输出的最优避障速度和舵角，梯度下降算法是一种寻找局部最优解的算法，给定初始值，梯度下降算法会在初始值附近进行搜索，获取初始值附近的参考变量，通过将参考变量输入梯度值函数公式计算得到其梯度值，然后判断梯度值是否满足为最近局部极小值，如果是局部极小值，则结束迭代循环，将梯度值对应的参考变量作为无人艇避障输出的最优速度和舵角传输至无人艇的控制模块，控制无人艇按避障输出的最优速度和舵角行驶，避开障碍物。因此，梯度下降算法通过寻找局部最优解，能够使无人艇以更加平滑的轨迹避开障碍物，解决了现有VO法在寻找最优解过程中由于VO区域发生变化导致前后两次最优解发生突变致使无人艇的不稳定性航行的问题；而且，梯度下降算法原理简单，控制参数少，易于实现，能够极大地加快算法的搜索速度。(1) The present invention uses the gradient descent algorithm to find the optimal obstacle avoidance speed and rudder angle of the UAV obstacle avoidance output. The gradient descent algorithm is an algorithm for finding a local optimal solution. Given the initial value, the gradient descent algorithm will Search near the initial value to obtain the reference variable near the initial value, calculate the gradient value by entering the reference variable into the gradient value function formula, and then judge whether the gradient value satisfies the nearest local minimum value, if it is a local minimum value, then End the iterative loop, transmit the reference variable corresponding to the gradient value as the optimal speed and rudder angle of the UAV obstacle avoidance output to the control module of the UAV, and control the UAV to travel at the optimal speed and rudder angle of the obstacle avoidance output. , avoid obstacles. Therefore, by finding the local optimal solution, the gradient descent algorithm can make the UAV avoid obstacles with a smoother trajectory, which solves the problem of the two optimal solutions caused by the change of the VO area in the existing VO method in the process of finding the optimal solution. The optimal solution is the problem of unstable navigation of unmanned boats caused by sudden changes. Moreover, the gradient descent algorithm is simple in principle, has few control parameters, and is easy to implement, which can greatly speed up the search speed of the algorithm.
(2)本发明综合考虑无人艇导航任务、VO法和海事规则约束三方面的因素，构建了无人艇避障的合理目标代价函数，该代价函数的优势在于充分考虑现实任务特点，存在极大的灵活性，具体体现在导航任务是避障可行解获取依据，其代价值区间为[0,0.5]，VO法避障任务则是布尔特性，其代价值非0即1，处于最优先等级，灵活性体现在海事规则约束在导航区域不存在可行解的情况下，可以在不碰撞条件下，一定程度上适当违反海事规则，代价函数中体现为其代价值区间为[0.5,1]；而且导航任务是最优先获取代价值，然后是海事规则约束区域。将目标代价函数分别对vi和θj求偏导，得到了速度大小梯度值函数和速度方向梯度值函数，将二维变量的目标代价函数变成一维的梯度值函数，使计算更加简单，而且梯度方向是函数值变化最快的方向，能够提高搜索速度，最快搜索到最优解。(2) The present invention comprehensively considers the three factors of unmanned boat navigation task, VO method and maritime rule constraints, and constructs a reasonable target cost function for unmanned boat obstacle avoidance. The great flexibility is embodied in the fact that the navigation task is the basis for obtaining feasible solutions for obstacle avoidance, and its cost value interval is [0, 0.5]. The VO method obstacle avoidance task is a Boolean characteristic, and its cost value is either 0 or 1, and it is in the most Priority level, flexibility is reflected in the maritime rules constraint that there is no feasible solution in the navigation area, and the maritime rules can be violated to a certain extent under the condition of no collision. ]; and the navigation task is the highest priority to obtain the cost value, and then the area restricted by the maritime rules. Taking the partial derivative of the objective cost function with respect to vi and θj respectively, the velocity magnitude gradient value function and the velocity direction gradient value function are obtained, and the objective cost function of the two-dimensional variable is transformed into a one-dimensional gradient value function, which makes the calculation simpler , and the gradient direction is the direction in which the function value changes the fastest, which can improve the search speed and find the optimal solution as quickly as possible.
(3)本发明中梯度下降算法作为独立算法，不需要和导航避障算法进行耦合，只需要提供梯度值函数公式即可通过梯度下降算法获取最优解，保证了算法的独立性和可移植性；而且梯度下降算法不但可以解决一维变量的局部最优解问题，还可以解决多维的局部最优解问题，所以对于复杂的速度和舵角的变化，梯度下降算法也可以很快寻找到最优解。(3) The gradient descent algorithm in the present invention is an independent algorithm, which does not need to be coupled with the navigation obstacle avoidance algorithm. It only needs to provide the gradient value function formula to obtain the optimal solution through the gradient descent algorithm, which ensures the independence and portability of the algorithm. Moreover, the gradient descent algorithm can not only solve the local optimal solution problem of one-dimensional variables, but also solve the multi-dimensional local optimal solution problem, so for complex changes in speed and rudder angle, the gradient descent algorithm can also quickly find the solution. Optimal solution.
附图说明Description of drawings
图1是VO法形成的VO区域分布图；Fig. 1 is the VO area distribution diagram formed by the VO method;
图2是VO法避障仿真实验相邻两帧时刻对应的代价函数在速度空间的分布曲线图；Fig. 2 is the distribution curve of the cost function corresponding to two adjacent frame moments in the VO method obstacle avoidance simulation experiment in the speed space;
图3是本发明基于梯度下降算法与VO法相结合的无人艇避障方法流程图；Fig. 3 is the flow chart of the unmanned boat obstacle avoidance method based on the combination of gradient descent algorithm and VO method of the present invention;
图4是单障碍物的避障仿真结果；Figure 4 is the simulation result of obstacle avoidance for a single obstacle;
图5为多障碍物的避障仿真结果；Figure 5 shows the simulation results of obstacle avoidance with multiple obstacles;
图6本发明基于梯度下降算法与VO法相结合的无人艇避障方法避障速度大小和角度输出变化曲线图。FIG. 6 is a graph showing the change of the obstacle avoidance speed and the angle output of the UAV obstacle avoidance method based on the combination of the gradient descent algorithm and the VO method of the present invention.
具体实施方式Detailed ways
以下通过具体实施例对本发明作进一步详细说明，但并不限制本发明的范围。The present invention will be described in further detail below through specific examples, but the scope of the present invention is not limited.
实施例1：Example 1:
一种基于梯度下降算法与VO法相结合的无人艇避障方法，如图3所示，包括以下步骤：An obstacle avoidance method based on the combination of gradient descent algorithm and VO method, as shown in Figure 3, includes the following steps:
(1)通过GPS和惯导传感器获取无人艇的位置、运动和姿态信息；通过无人艇自身携带的雷达、视觉、激光雷达、声呐等传感器获得环境信息，进行数据融合与环境建模得到障碍物的位置，运动和尺寸信息，并且根据障碍物的不同尺寸将障碍物建模为大小不同的圆形状障碍物；根据LOS算法获取无人艇当前导航输出的速度和舵角(vlos,θlos)。(1) Obtain the position, motion and attitude information of the unmanned boat through GPS and inertial navigation sensors; obtain environmental information through radar, vision, lidar, sonar and other sensors carried by the unmanned boat itself, and perform data fusion and environmental modeling to obtain The position, motion and size information of the obstacles, and the obstacles are modeled as circular obstacles of different sizes according to the different sizes of the obstacles; according to the LOS algorithm, the speed and rudder angle of the current navigation output of the unmanned boat (vlos , θlos ).
(2)判断无人艇与障碍物之间是否存在碰撞风险，如果无人艇与障碍物之间不存在碰撞风险，则以无人艇当前导航输出的速度和舵角作为无人艇避障输出的速度和舵角传输至无人艇控制模块(即无人艇按导航输出的速度和舵角继续航行)；如果无人艇与障碍物之间存在碰撞风险，则执行下一步操作。(2) Determine whether there is a collision risk between the unmanned boat and the obstacle. If there is no collision risk between the unmanned boat and the obstacle, the speed and rudder angle of the current navigation output of the unmanned boat are used as the obstacle avoidance of the unmanned boat. The output speed and rudder angle are transmitted to the unmanned boat control module (that is, the unmanned boat continues to sail according to the speed and rudder angle output by the navigation); if there is a risk of collision between the unmanned boat and the obstacle, the next step is performed.
其中，判断无人艇与障碍物之间是否存在碰撞风险的具体操作为：计算无人艇与障碍物的最近会遇时间TCPA和最近会遇距离DCPA，当TCPA≤tmax且DCPA≤dmin时，其中tmax、dmin均为已知的参数，无人艇与障碍物之间存在碰撞风险，如果TCPA、DCPA不能同时满足上述条件，则无人艇与障碍物之间不存在碰撞风险；所述TCPA、DCPA的计算公式为：Among them, the specific operation of judging whether there is a collision risk between the unmanned boat and the obstacle is: calculating the nearest encounter time TCPA and the nearest encounter distance DCPA between the unmanned boat and the obstacle, when TCPA≤tmax and DCPA≤dmin , where tmax and dmin are known parameters, there is a risk of collision between the unmanned boat and the obstacle. If TCPA and DCPA cannot meet the above conditions at the same time, there is no collision risk between the unmanned boat and the obstacle. ; The calculation formula of described TCPA, DCPA is:
DCPA＝||(PA+vA·TCPA)-(PB+vBTCPA)|| (V)DCPA=||(PA +vA ·TCPA)-(PB +vB TCPA)|| (V)
PB和PA分别表示障碍物和无人艇的位置矢量，vB和vA分别表示在大地坐标系下的障碍物和无人艇的速度矢量。PB and PA represent the position vector of the obstacle and the unmanned boat, respectively, and vB and vA represent the velocity vector of the obstacle and the unmanned boat in the geodetic coordinate system, respectively.
(3)以无人艇当前导航输出的速度和舵角(vlos,θlos)作为初始值输入梯度下降算法程序，采用梯度下降算法程序在初始值的上下左右四个方向进行基于梯度的迭代搜索，获取初始值在上下左右四个方向的参考变量(vi+1,θj)、(vi-1,θj)、(vi,θj+1)和(vi,θj-1)；将获取的四个参考变量输入梯度值函数公式中，计算每一个参考变量对应的梯度值，然后判断每一个参考变量对应的梯度值是否满足梯度下降算法程序的迭代循环终止条件；经判断发现任意一个参考变量对应的梯度值满足迭代循环终止条件(此时满足迭代循环终止条件的梯度值是离初始值最近的极值点，即局部最优解)，则终止迭代循环，以满足迭代循环终止条件的梯度值对应的参考变量作为无人艇避障输出的速度和舵角传输至无人艇的控制模块，使无人艇按照避障输出的速度和舵角进行行驶避开障碍物；经判断发现每一个参考变量对应的梯度值均不满足迭代循环终止条件，则以四个参考变量作为梯度下降算法程序的初始值，继续在四个参考变量上、下、左、右四个方向进行基于梯度的迭代搜索，直至找到满足迭代循环终止条件的梯度值。其中，所述梯度下降算法程序的步长为：速度大小为0.1，角度大小为0.5°；梯度下降算法程序的迭代循环终止条件为速度大小梯度值小于设定的最小速度梯度阈值0.01，同时速度方向(速度方向即舵角)梯度值小于设定的最小方向梯度阈值0.05。(3) Use the speed and rudder angle (vlos , θlos ) of the current navigation output of the UAV as the initial value to input the gradient descent algorithm program, and use the gradient descent algorithm program to perform gradient-based iteration in the four directions of the initial value, up, down, left, and right Search to obtain the reference variables (vi+1, θj ), (vi-1, θj ), (vi , θj +1) and (vi, θj -1)whose initial values are in the four directions of up, down, left and right ; Input the obtained four reference variables into the gradient value function formula, calculate the gradient value corresponding to each reference variable, and then judge whether the gradient value corresponding to each reference variable satisfies the iterative loop termination condition of the gradient descent algorithm program; If the gradient value corresponding to any reference variable satisfies the iterative loop termination condition (at this time, the gradient value satisfying the iterative loop termination condition is the extreme point closest to the initial value, that is, the local optimal solution), then the iterative loop is terminated to satisfy the iterative loop The reference variable corresponding to the gradient value of the termination condition is transmitted to the control module of the unmanned boat as the speed and rudder angle of the obstacle avoidance output of the unmanned boat, so that the unmanned boat can avoid obstacles according to the speed and rudder angle of the obstacle avoidance output; After judgment, it is found that the gradient value corresponding to each reference variable does not meet the iterative loop termination condition, then the four reference variables are used as the initial value of the gradient descent algorithm program, and the four reference variables continue to be up, down, left and right in the four directions. An iterative gradient-based search is performed until a gradient value that satisfies the iterative loop termination condition is found. Wherein, the step size of the gradient descent algorithm program is: the speed size is 0.1, the angle size is 0.5°; the iterative loop termination condition of the gradient descent algorithm program is that the speed size gradient value is less than the set minimum speed gradient threshold 0.01, while the speed The direction (speed direction, ie rudder angle) gradient value is less than the set minimum direction gradient threshold of 0.05.
所述梯度值函数公式的具体构建步骤为：The specific construction steps of the gradient value function formula are:
1)将速度空间进行离散，离散后的速度二维变量为(vi,θj)，其中，vi为速度大小，θj为速度方向，由于无人艇的动力学限制，vi的范围为0～20m/s，θj的范围为0～360°；1) Discrete the velocity space, and the two-dimensional velocity variable after discretization is (vi, θj ), where vi is themagnitude of the velocity, and θ jis the direction of the velocity. The range is 0～20m/s, and the range ofθj is 0～360°;
2)将速度空间的速度二维变量与无人艇导航输出的速度二维变量的偏差作为导航任务的代价值，其取值区间为[0,0.5]；2) The deviation between the speed two-dimensional variable in the speed space and the speed two-dimensional variable output by the UAV navigation is taken as the cost value of the navigation task, and its value interval is [0, 0.5];
3)根据速度障碍法，位于VO区域的速度二维变量对应的代价值为1，位于VO区域以外的速度二维变量对应的代价值为0；3) According to the speed obstacle method, the cost value corresponding to the speed two-dimensional variable located in the VO area is 1, and the cost value corresponding to the speed two-dimensional variable located outside the VO area is 0;
4)海事规则约束的代价值存在灵活性，根据速度障碍法，在VO区域以外存在最优的避障速度二维变量时，将海事规则约束对应的速度二维变量排除在最优的避障速度二维变量之外(即不考虑海事规则约束对应的速度二维变量)，当VO区域之外不存在最优的避障速度二维变量时，以海事规则约束对应的速度二维变量作为最优的避障速度二维变量输出，因此，海事规则约束对应的代价值的取值区间为[0.5,1]；4) There is flexibility in the cost value of maritime rule constraints. According to the speed obstacle method, when there is an optimal two-dimensional obstacle avoidance speed variable outside the VO area, the speed two-dimensional variable corresponding to the maritime rule constraint is excluded from the optimal obstacle avoidance. In addition to the two-dimensional speed variables (that is, the two-dimensional speed variables corresponding to the constraints of maritime rules are not considered), when there is no optimal two-dimensional variable of obstacle avoidance speed outside the VO area, the two-dimensional speed variables corresponding to the constraints of maritime rules are used as The optimal obstacle avoidance speed two-dimensional variable output, therefore, the value range of the cost value corresponding to the maritime rule constraint is [0.5, 1];
5)根据导航任务、速度障碍法避障任务、海事规则约束三者的代价值取值区间和速度空间的速度二维变量范围之间的关系，构建得到目标代价函数计算公式，如式(I)所示：5) According to the relationship between the value interval of the substitution value of the navigation task, the obstacle avoidance task of the speed obstacle method, and the constraints of the maritime rules and the range of the speed two-dimensional variable in the speed space, the calculation formula of the objective cost function is constructed and obtained, such as formula (I ) as shown:
6)将目标代价函数J对vi和θj分别求偏导，得到对应的偏导函数，即为梯度值函数，所述梯度值函数包括速度大小梯度值函数和速度方向梯度值函数，其中，目标代价函数J对vi求偏导得到的偏导函数为速度大小梯度值函数，如式(II)所示，目标代价函数J对θj求偏导得到的偏导函数为速度方向梯度值函数，如式(III)所示：6) Calculate the partial derivative of the objective cost function J with respect to vi and θj respectively, and obtain the corresponding partial derivative function, which is the gradient value function, and the gradient value function includes the velocity magnitude gradient value function and the velocity direction gradient value function, wherein , the partial derivative function obtained by the partial derivative of the objective cost function J with respect to vi is the velocity magnitude gradient value function, as shown in formula (II), the partial derivative function obtained by the partial derivative of the objective cost function J with respect to θj is the velocity direction gradient The value function, as shown in formula (III):
(4)获取避障仿真效果图，同时打印避障输出曲线图，通过避障输出曲线图中曲线变化的连续性验证梯度下降算法避障的稳定性和有效性。图4为单障碍物场景下无人艇避障仿真的效果图(图中纵坐标和横坐标均表示距离，单位为m)，由图4可以看出，在单障碍物场景下，采用本发明的避障方法可以有效避免无人艇与障碍物的碰撞。图5为多障碍物场景下无人艇避障仿真效果图(图中纵坐标和横坐标均表示距离，单位为m)，由图5可以看出，在多障碍物的复杂场景下，本发明的避障方法依然能够实现无人艇的避障。图6为无人艇避障输出速度的变化曲线图和避障输出角度的变化曲线图，由图6可以看出，采用本发明的避障方法，无人艇避障输出速度变化曲线和角度变化曲线均为连续性曲线，由此说明，本发明的避障方法能够使无人艇以更加平滑的轨迹避开障碍物，解决了现有VO法在寻找最优解过程中由于VO区域发生变化导致前后两次最优解发生突变致使无人艇的不稳定性航行的问题。(4) Obtain the simulation effect diagram of obstacle avoidance, and print the output curve of obstacle avoidance at the same time, and verify the stability and effectiveness of the gradient descent algorithm for obstacle avoidance through the continuity of the curve changes in the output curve of obstacle avoidance. Figure 4 is the effect diagram of the UAV obstacle avoidance simulation in the single obstacle scene (the ordinate and the abscissa in the figure both represent the distance, the unit is m). It can be seen from Figure 4 that in the single obstacle scene, this The invented obstacle avoidance method can effectively avoid the collision between the unmanned boat and the obstacle. Figure 5 is the simulation effect diagram of the UAV obstacle avoidance in the scene of multiple obstacles (both the ordinate and abscissa in the figure represent the distance, the unit is m). It can be seen from Figure 5 that in the complex scene of multiple obstacles, this The invented obstacle avoidance method can still realize the obstacle avoidance of the unmanned boat. Fig. 6 is the change curve graph of the output speed of UAV obstacle avoidance and the change curve graph of the output angle of obstacle avoidance, it can be seen from Fig. The change curves are all continuous curves, which shows that the obstacle avoidance method of the present invention can enable the unmanned boat to avoid obstacles with a smoother trajectory, and solves the problem that the existing VO method is in the process of finding the optimal solution due to the occurrence of the VO area. The change leads to the sudden change of the two optimal solutions before and after, resulting in the unstable navigation of the unmanned boat.
Claims (4)
Translated fromChinese
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910283660.8ACN109916400B (en) | 2019-04-10 | 2019-04-10 | An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910283660.8ACN109916400B (en) | 2019-04-10 | 2019-04-10 | An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109916400A CN109916400A (en) | 2019-06-21 |
| CN109916400Btrue CN109916400B (en) | 2020-08-25 |
Family
ID=66969288
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910283660.8AActiveCN109916400B (en) | 2019-04-10 | 2019-04-10 | An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109916400B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111984006B (en)* | 2020-07-24 | 2021-07-06 | 哈尔滨工程大学 | Multi-target encounter and collision avoidance method for unmanned boats incorporating the effects of ocean currents and scale differences |
| CN112306090B (en)* | 2020-10-26 | 2024-08-20 | 中国人民解放军军事科学院国防科技创新研究院 | Unmanned aerial vehicle instant and rapid obstacle avoidance method based on relative displacement and speed vector synthesis |
| CN114167880A (en)* | 2021-12-02 | 2022-03-11 | 大连海事大学 | A time-optimized multi-underwater glider path planning system |
| CN116540696A (en)* | 2023-04-13 | 2023-08-04 | 北京工业大学 | A multi-modal obstacle avoidance system based on multi-sensor fusion of unmanned ships based on ROS |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6462739B1 (en)* | 1999-03-18 | 2002-10-08 | Abode Systems Incorporated | Curve smoothing without shrinking |
| US8849483B2 (en)* | 2011-04-13 | 2014-09-30 | California Institute Of Technology | Target trailing with safe navigation with colregs for maritime autonomous surface vehicles |
| JP6171976B2 (en)* | 2014-02-25 | 2017-08-02 | 株式会社デンソー | Vehicle behavior control device |
| US10019006B2 (en)* | 2015-04-08 | 2018-07-10 | University Of Maryland, College Park | Surface vehicle trajectory planning systems, devices, and methods |
| FI130355B (en)* | 2016-01-29 | 2023-07-17 | Rolls Royce Oy Ab | Autonomous operation of a vessel |
| CN105589464B (en)* | 2016-03-28 | 2019-02-26 | 哈尔滨工程大学 | A UUV Dynamic Obstacle Avoidance Method Based on Velocity Obstacle Method |
| CN108073176B (en)* | 2018-02-10 | 2020-08-18 | 西安交通大学 | An Improved D*Lite Vehicle Dynamic Path Planning Method |
| CN108664020A (en)* | 2018-04-11 | 2018-10-16 | 上海大学 | A kind of unmanned boat dynamic obstacle avoidance algorithm based on Speed Obstacles method and dynamic window method |
| CN109298712A (en)* | 2018-10-19 | 2019-02-01 | 大连海事大学 | Unmanned ship autonomous collision avoidance decision method based on adaptive navigation situation learning |
| CN109543225B (en)* | 2018-10-19 | 2022-12-16 | 东软集团股份有限公司 | Vehicle control scheme generation method and device, storage medium and electronic equipment |
- 2019
- 2019-04-10CNCN201910283660.8Apatent/CN109916400B/enactiveActive
Also Published As
| Publication number | Publication date |
|---|---|
| CN109916400A (en) | 2019-06-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113093804B (en) | Formation control method and control system of unmanned boat based on inversion sliding mode control | |
| CN110906934B (en) | Unmanned ship obstacle avoidance method and system based on collision risk coefficient | |
| CN109916400B (en) | An Obstacle Avoidance Method for Unmanned Vehicle Based on the Combination of Gradient Descent Algorithm and VO Method | |
| CN111580518B (en) | A Layered Obstacle Avoidance Method for Unmanned Vehicles Based on Improved Drosophila Optimization and Dynamic Window Method | |
| CN110196598B (en) | Dynamic collision avoidance method for unmanned ship | |
| CN106959698B (en) | A Path Tracking and Obstacle Avoidance Guidance Method | |
| CN110262492A (en) | A Real-time Collision Avoidance and Target Tracking Method for Unmanned Vehicles | |
| CN108445879A (en) | A kind of unmanned boat barrier-avoiding method based on prediction collision risk region | |
| CN107168335A (en) | A kind of unmanned surface vehicle path trace method of guidance for considering mixing multiple target avoidance | |
| Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
| CN107037809A (en) | A kind of unmanned boat collision prevention method based on improvement ant group algorithm | |
| CN111123923B (en) | Unmanned ship local path dynamic optimization method | |
| CN108681321A (en) | An underwater detection method for cooperative formation of unmanned ships | |
| CN109784201A (en) | AUV dynamic obstacle avoidance method based on four-dimensional risk assessment | |
| CN113759939A (en) | A method and device for intelligent navigation in restricted waters | |
| CN104881045A (en) | Bionic robot fish three-dimensional tracking method based on embedded visual guidance | |
| CN115903800B (en) | A strict safety control method for multi-unmanned vessel formation based on leader collaboration | |
| CN110837255A (en) | Autonomous danger avoiding method suitable for high-speed water surface unmanned ship | |
| CN111984012A (en) | Unmanned ship autonomous obstacle avoidance and air route planning method | |
| CN111412918A (en) | Unmanned ship global safety path planning method | |
| CN116954232A (en) | Multi-ship collision avoidance decision-making method and system for unmanned ships based on reinforcement learning | |
| CN109765914A (en) | A collision avoidance method for surface unmanned boats based on sliding window particle swarm | |
| CN112650246A (en) | Ship autonomous navigation method and device | |
| CN117519138A (en) | Path planning method based on improved DWA for dense obstacles and ocean current interference | |
| CN117369441A (en) | Self-adaptive intelligent ship path planning method considering ship kinematics and COLLEGs |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |