ORCA避障源码笔记

参考资料

https://gamma.cs.unc.edu/ORCA/publications/ORCA.pdf

https://gamma.cs.unc.edu/RVO2/

数学知识

1.向量的点乘 dotProduct,计算方法:1. 2.

作用:点积如果为负,则a,b形成的角为钝角;如果为零,那么a,b垂直;如果为正,那么a,b形成的角为锐角

2.向量a,向量b,det(a,b)表示行列式的值,计算方法x1y2 - x2y1,同时也是叉乘的值

作用:1.以两个向量为邻边的平行四边形的有向面积,2.小于0表示b在a的顺时针方向,大于0表示b在a逆时针方向,等于0表示a,b平行

下图是碰撞需要修正,并且保证最短修正的两种情况,

  1. 将速度修正到切线上

  2. 将速度修正到圆

1,

复制代码
final double dotProduct1 = w.dotProduct(relativePosition);
复制代码
dotProduct1 < 0.0 && dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq

这个代码是为了判断当前是哪种情况,

向量w = 相对速度 - 相对位置

dotProduct1小于0,表示向量w与相对位置的夹角为钝角,即AE与AO的角为锐角(需要先判断这个,只有锐角范围内cos单调递减)

dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq,角OAE小于角OAD

推导:

cos在0-90单调递减

角OAE < 角OAD

=> cosOAE > cosOAD

=> OA*cosOAE > OA*cosOAD (同时乘以OA)

=> OA*cosOAE > AD (OA*cosOAD = AD)

=> OA * AE * cosOAE > AE * AD (同时乘以AE)

=> dotProduct1 * dotProduct1 > combinedRadiusSq * wLengthSq

2

复制代码
direction = new Vector2D(relativePosition.getX() * leg - relativePosition.getY() * combinedRadius, relativePosition.getX() * combinedRadius + relativePosition.getY() * leg)

这段代码为了计算第一情况,修正速度的落点,

leg为OD的长度,假设direction为line的方向,即向量OD的单位向量DIR(a, b)顺时针的单位向量DIE (b, -a),A的坐标(x, y)

向量OA 点乘 向量DIR = 向量OA的长度 * 1 * cosAOD

=> x*a + y*b = leg

向量OA 点乘 向量DIE = 向量OA的长度 * 1 * cosOAD

=>x*b - y*a = combinedRadius

结合这两个式子就可以解a,b,

复制代码
RVOMath.det(relativePosition, w) > 0.0

这个在判断修正方向是在相对位置的左边还是右边

3

速度的可选区域为line方向的左侧, linearProgram2 找到可行域的交集,即line的交集

3.1

RVOMath.det(lines.get(lineNo).direction, lines.get(lineNo).point.subtract(newVelocity)) > 0.0

大于0说明newVelocity在line的右侧,不在line的可行域内,需要重新计算速度

3.2

如图,绿色为line i, 黑色为line no

final double denominator = RVOMath.det(lines.get(lineNo).direction, lines.get(i).direction);

final double numerator = RVOMath.det(lines.get(i).direction, lines.get(lineNo).point.subtract(lines.get(i).point));

final double t = numerator / denominator;

denominator为BCED的面积 = 2个DBC的面积 = 2 * 1/2 * BP2 * 高

numerator为PGFP2的面积 = 2个GPP2 的面积 = 2个DBP2的面积 = 2 * 1/2 * BC * 高

t = numerator / denominator = BP2 / BC = BP2

t 为line i 和 line no 的交点 到 line point 的距离

4

linearProgram3

如果找不到line的交集,则执行linearProgram3,从上次失败的line开始往后遍历

将line i 之前的line做一个修正,修正的方法是:(1)将point修正到交点,(2)将方向修正到角平分线,靠近i的方向

复制代码
RVOMath.det(lines.get(i).direction, lines.get(i).point.subtract(newVelocity)) > distance

这段代码用来判断是否需要修正速度,如果修正后的速度与当前line的距离大于前一个line的距离大,那么需要修正。

因为修正后速度将在角平分线上,一定距离当前line更近

相关推荐
Yvemil721 分钟前
《开启微服务之旅:Spring Boot 从入门到实践》(三)
java
Anna。。23 分钟前
Java入门2-idea 第五章:IO流(java.io包中)
java·开发语言·intellij-idea
.生产的驴44 分钟前
SpringBoot 对接第三方登录 手机号登录 手机号验证 微信小程序登录 结合Redis SaToken
java·spring boot·redis·后端·缓存·微信小程序·maven
爱上语文1 小时前
宠物管理系统:Dao层
java·开发语言·宠物
王ASC1 小时前
SpringMVC的URL组成,以及URI中对/斜杠的处理,解决IllegalStateException: Ambiguous mapping
java·mvc·springboot·web
是小崔啊1 小时前
开源轮子 - Apache Common
java·开源·apache
因我你好久不见1 小时前
springboot java ffmpeg 视频压缩、提取视频帧图片、获取视频分辨率
java·spring boot·ffmpeg
程序员shen1616111 小时前
抖音短视频saas矩阵源码系统开发所需掌握的技术
java·前端·数据库·python·算法
Ling_suu2 小时前
SpringBoot3——Web开发
java·服务器·前端
天使day2 小时前
SpringMVC
java·spring·java-ee