rvo动态避障算法
源码: snape (Jamie Snape) · GitHub
文档: RVO2 Library - Reciprocal Collision Avoidance for Real-Time Multi-Agent Simulation
网友翻译的中文版文档: 导航动态避让算法RVO的优化ORCA(Optimal Reciprocal Collision Avoidance)_u012740992的专栏-CSDN博客_orca算法
本文就rvo中动态避障的算法源码做一个简单分析
代码取自:https://github.com/warmtrue/RVO2-Unity 大约是2021-8-10这个时间取的,
是一个例子, 将rvo整合到unity里
rvo详可见上文链接
这里做一个简单的说明
RVO算法简要说明
运动的物体A与物体B,假设物体A的位置为坐标原点,并将A变成质点,
则B相应的半径增大,新的圆B记录CircleNew
如下图示,以原点为顶点,原点到CircleNew做切线,组成一个锥形区域(图中划横线的)
计算A,B之间的相对速度Va,如果Va位于锥形区域以外,则A B将不会发生碰撞,反之,则会在未来某个时间发生碰撞
进一步计算,假设时间取为t,
物体A的半径记为rA, B的半径记录rB, 位置分别为PA ,PB
CircleNew圆记位于PB - PA 半径为rA + rB,
对整个坐标系进行缩放1/t,这样速度的单位就变成秒
原来的大圆,就缩放为圆心 (PB-PA)/t 半径为(rA+rB)/t,这个新圆被称为截止圆cutoff-circle
可以看出速度的选择范围如以下右图所示,除了灰色圆头锥型区域以外,白色区域都是可选的相对速度Vr=Va-Vb(Va Vb分别为A,B的速度)方向以及大小.如图中的蓝色速度与红色速度
如果Vr速度位于灰色的锥型区,就需要调整速度至空白区域,即给速度加个分量
如下图的蓝色小箭头,这种情况下的相对速度,就需要往旁边调整,锥形区域中的切边,被称为leg
根据速度的方向大小,也有可能往cutoff-circle处调整或另一条leg处调整
如下图,往cutoff-circle处调整相对速度Vr,
这个调整的速度分量我们称之为u,
物体A调整后的速度为 Va-new = Va + u,
令A和B各负责一半的速度调整,则Va-new = Va + 0.5 *u
这样即可保证A B在t时间内不会碰撞
选取了u为调整分量后,可以发现,Va-new的取值是一个半平面,
这个半平面的分割线是以垂直向量u为方向,过点Va+0.5*u的直线,在此处称之为line
在这个半平面的一侧速度都可以为备选速度
如果有多个物体都在做避障处理,就会得到一系列的半平面,这些半平面的交集,就是速度的可选范围
如下图中划斜线的区域
在斜线区域的边缘,取则一个速度,并使这个速度的长度,尽量接近程序中预设物体A的最大速率(Vopt的长度),即可
在选速度时,尽量保证新的速度与Vopt方向相同,长度相同,如果不能保证方向相同,则尽量保证长度不大于Vopt(即舍弃方向与Vopt相同这个约束)
观察RVO代码实现,line取过点Va+0.5*u,保证了交集是一定存在的
可以认为所有的line都以为点0.5*u(每个line的u取值不同),那么所有的line的交集是包含原点的凸多边形
再将整个坐标系做一个平移到Va,所得的交集是一个包含点Va的凸多边形
隐含的假设是其它寻路物体的下一个时刻速度为零,避免程序出现没有交集而无法选取速度的情况,如下,,在每个速度半平面没有交点的情况下(中间那张图),选择让对方的速度为0,从而得到一个交集(最右边的那张图)
RVO认为B C也会做同样的处理,从而达到障碍的目的
致此,RVO算法简要说明结束-------------
RVO动态避障代码分析
动态功能代码主要在这个函数:computeNewVelocity
红框处这个循环,就是处理动态障碍开始
1 计算相对速度,相对位置
代码:
combineRadius就是把A看到质点后,B物体半径相对扩大
2 计算速度调整
代码截图
分析:
2.1 代码中变量对应的几何意义
2.2 dotProduct1
判断是否往cutoff-circle调整的代码:
if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq)
w与relativePosition夹角记做<Awr>
值为 length(w) * length(relativePosition) * cos(<Awr>)
此值小零就意味着可能需要往cutoff-circle处调整相对速度
RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq
条件中的另一个,将不等式两边的wLengthSq去掉,
很明显实际上比较是cos(<Awr>)的平方与cos<角1>的平方,
如果此条件也满足,说明一定是在cutoff-circle上调整速度了
2.3 计算cutoff-circle的u
这个比较简单,知道判断规则,其实就是cutoff-circle圆心与relativeVelocity连线,即w
取w的方向 line.direction自然就要取与w垂直(即对应切线方向)
u的长度要用cutoff-circle的半径减去w的长度wLength
如此就得到了往cutoff-circle调整时,向量u以及对应的半平面分割线的方向
2.4 往leg处调整
leg就是原点与cutoff-circle做的切线
如下图,当速度为红色所示向量时,要按蓝色箭头的方向调整
利用叉积(sin值)判断是位于哪条leg,代码:
RVOMath.det(relativePosition, w)
构造二元一次方程组,求向调整向量u
Rx,Ry为cutoff-circle圆心坐标 leg假设为(x,y)单位向量,则与leg垂直的u为(y,-x)
cutoff-circle的圆心坐标分别在u与leg上投影
方程组如下:
Rx * x + Ry * y = leg leg是圆C_cutoff的切点到A点的长度,用勾股定理就可以求得
Rx * y - Ry * x = Rad/t
代码:
/* Project on left leg. */
line.direction = new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
解此方程组,就可以求得(x,y) leg的单位向量,再根据leg,求得u
代码:
float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
另外还有物体A与物体B已经发生碰撞的情况,看代码处理类似向cutoff-circle调整,此处不特别分析了
3 linearProgramX
对应linearProgram1 linearProgram2 linearProgram3 这三个函数
有了速度备选集合后,要选一个速度newVelocity_,它的方向与长度,应尽量与prefVelocity_相近似,或者完全相同
对所有的动态物体进行处理后,会得到一系列的半平面,
它们的分割线记录在Agent.orcaLines_里面
根据之前的说明,可以很容易推断出,这些line的交集是一个以velocity_为”中心点”的凸多边形
即所有的分割线,都以各自的u方向以及u的长度,相对velocity_拉开
并且,根据代码迭代来看,velocity_初始值是0,之后它的长度一般是不会大于prefVelocity_的
(除非程序有动态改变prefVelocity_的需求)
3.1 linearProgram2
将prefVelocity设置迭代的初始速度 result
判断循环判断result是否在orcaLines_所划分的半平面内
利用result到分割线的距离正负值来判断,从代码推断,在分割线的左边表示在半平面内
代码如下:
if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f)
如果距离为正,说明在半平面外,调用linearProgram1算出来的最接近的速度
3.2 linearProgram1
此函数直观表示如下:
velocity_是当前的速度,它必然在orcaLines_交集组成的凸多边形当中
prefVelocity_可能不在交集中,linearProgram1负责在凸多边形的一条边上,找出最合适的速度例如上图中的newVelocity_
prefVelocity_够不能交集所在区域的情况(这种情况应该是动态改了prefVelocity_)
根据勾股定理判断,dotProduct是一条直角边的长, RVOMash.absSq(lines[lineNo].port)是斜边,将它们与速度的长radius比较,代码如下:
之后,计算以原点为圆心以,以prefVelocity_的长为半径的圆与当前line分割线的两个交点tLeft tRight,
计算让交集凸多边形的其它边与当前line的交点,判断交点是否位于tLeft 与tRight之间,如果是根据情况替换tLeft或tRight
实际上就是prefVelocity_旋转,与当前交集多边形line相交,如果交点在边的两个端点内,就取交点,
如果不是,判断边的端点是否在交点内,即取两个线段的交集
如果两个线段没有交集,也被判断为失败
各个变量的几何意义如下:
tLeft tRight的初始值如上图所示,图中红色lines[lineNo].point表示该点的长度
重线的长度为 length(lines[lineNo].point) - RVOMath.sqr(dotProduc)
如下二图所示,
原点到交集边的垂足P tLeft tRight实际上是当前line上的点到垂足的长度比例,因为不断迭代,tLeft tRight会变化(变小)
紫色即为选取的速度:
取边的两个端点间的某一个点,此时速度方向变了,但速度大小与prefVelocity_一致
取边的端点,此时速度方向变了,速度大小也比prefVelocity_小
RVO代码很处理比较复杂,还是配合linearProgram3处理,实际上就是完成上面这个步骤,
在速度交集多边形上的边上选一个点
再来看linearProgram1的迭代tLeft 与tRight的代码,
循环每条line,与当前处理的line(以下简称为line_cur)求它们的”交点”t(t其实是一个比例)
为交点到line.point距离
注意这三行代码:
float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction);
float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point);
float t = numerator / denominator;
其实是在求两条直线的交点,lines[lineNo].point - lines[i].point隐含了坐标平移
Lines[lineNo]的直线方程就会变成 y=ax
而lines[i]的直线方程在新坐标系中会变成 (y-py)=b(x-px) 其中(px,py)就是lines[lineNo].point - lines[i].point,
为坐标平移后的点
由于t是一个距离点lines[lineNo.point]的比例,所以平移不影响计算结果.
相应的推导过程比较简单,二元一次方程组求解,即可得到
t = numerator / denominator
t是交点到lines[lineNo.point]的距离
根据denominator的值判断t是替换tLeft,还是tRight
过程不太好理解,但原理其实非常简单
最后这段代码,就是在迭代完成之后选取result了,比较简单,没啥好说的
3.3 linearProgram3
如果linearProgram2中有出现失败的现象(tLeft > tRight,或者速度太小够不到交集所在的区域)
会导致linearProgram3的处理
其实笔者认为,直接以原点为圆心,prefVelocity_为半径划个圆,
将速度交集多边形所有的顶点判断是否在圆中,
然后对在圆中的顶点的边进行速度处理,也可以,不知道算法为什么不这么写.
算法是对出现失败前的line进行处理,求出它们与当前line的交点,并重新设置它们的方向
line.direction = RVOMath.normalize(lines[j].direction - lines[i].direction);
代码如下:
红框处是这个算法不太好理解的部分,为什么要这样重新设定半平面的方向
原因1 这样设定会导致交集一定比原来的小(即保证速度的合法性)
原因2 这样设定会让新的速度选择尽量出现在当前的line上
(和之后调用linearProgram2的速度取值有关)
这两点笔者反复思考过,应该就是这段代码的真实意图,可以用图来直观的理解
新的line的方向其实是夹角的角平分线,可以看出,交集会变小
if (linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, ref result) < projLines.Count)
这行代码再次调用linearProgram2计算新的速度,起始的速度方向变成与当前处理的line重直,
这就是上文提到的原因2,所有半平面分割线方向调整,都是为了保证新的速度尽量落在当前处理的line上
最后说明一下求交点的代码:
line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
这里
将(RVOMath.det(lines[j].direction, lines[i].point - lines[j].point)记做dis_ij
将(RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant)记录hypotenuse_len 它表示line[i].Point到交点的距离,即另一个直角三角形的斜边
这些变量的几何意义如下图:
这样,就可以利用lines[i].point + hypotenuse_len * lines[i].direction直线算出交点了,
致此RVO动态避障代码分析完毕
作者:林绍川