ORCA-3D避障代码和原理解析

二维ORCA原理参考：
https://zhuanlan.zhihu.com/p/669426124

ORCA原理图解+代码解释

1. 找到避障速度增量 u

碰撞处理分为三种情况：
（1）没有发生碰撞，且相对速度落在小圆里
（2）没有发生碰撞，且相对速度落在圆锥里
（3）发生碰撞，马上做出反应
请添加图片描述
timeStep 决定了仿真每一步的时间更新间隔，是系统的时间推进基础。较小的 timeStep 可以提高仿真的精度，但会增加计算量。

timeHorizon 决定了智能体在进行避障计算时预测的时间范围。较大的 timeHorizon 值使得智能体可以更早预测潜在碰撞，但会减少它的速度选择自由度。

timeStep 是碰撞时需要计算的调整u所需的时间
timeHorizon 是未发生碰撞时，需要计算的u所化的时间，他是一种提前预测

2. 添加速度障碍平面

表示一个平面需要法向量和平面上的点
请添加图片描述

1和2对应代码如下

	// 它使用ORCA（Optimal Reciprocal Collision Avoidance）方法来计算智能体之间的避碰行为void Agent::computeNewVelocity(){orcaPlanes_.clear();				  // 清空ORCA平面列表const float invTimeHorizon = 1.0f / timeHorizon_; // 计算时间视野的倒数/* 创建智能体的ORCA平面 */for (size_t i = 0; i < agentNeighbors_.size(); ++i){								       // 遍历每个邻居智能体const Agent *const other = agentNeighbors_[i].second;	       // 获取邻居智能体指针//这里的position_是在rvo->updateState添加的当前agent的位置// 改这块就好了===============================const Vector3 relativePosition = other->position_ - position_; // 计算相对位置const Vector3 relativeVelocity = velocity_ - other->velocity_; // 计算相对速度// const Vector3 relativePosition = relative_position_; // 计算相对位置// const Vector3 relativeVelocity = relative_velocity_; // 计算相对速度const float distSq = absSq(relativePosition);		       // 计算相对位置的平方距离const float combinedRadius = radius_ + other->radius_;	       // 计算合并半径const float combinedRadiusSq = sqr(combinedRadius);	       // 计算合并半径的平方Plane plane; // 定义一个平面对象Vector3 u;   // 定义速度调整向量if (distSq > combinedRadiusSq){										// 如果没有发生碰撞// w表示给定时间视野TimeHorizon内，两个智能题之间的相对速度偏移量const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition; // 计算从截断中心到相对速度的向量const float wLengthSq = absSq(w);					// 计算w向量的平方长度const float dotProduct = w * relativePosition; // 计算w向量和相对位置的点积// 1. 如果投影在截断圆上// dotProduct表示相差的速度和相差的位置的点乘，要是点乘小于0，表示在靠近if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq){						    const float wLength = std::sqrt(wLengthSq); // 计算w向量的长度const Vector3 unitW = w / wLength;	    // 计算w向量的单位向量plane.normal = unitW;					 // 设置平面的法向量u = (combinedRadius * invTimeHorizon - wLength) * unitW; // 计算速度调整向量}// 2. 如果投影在圆锥上else{																  const float a = distSq;													  // 设置系数aconst float b = relativePosition * relativeVelocity;									  // 设置系数bconst float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq); // 设置系数c// t表示圆锥中心线到斜线的距离 对于 半径的倍数const float t = (b + std::sqrt(sqr(b) - a * c)) / a;									  // 计算t值const Vector3 w = relativeVelocity - t * relativePosition;								  // 计算w向量const float wLength = abs(w);												  // 计算w向量的长度const Vector3 unitW = w / wLength;											  // 计算w向量的单位向量plane.normal = unitW;			    // 设置平面的法向量u = (combinedRadius * t - wLength) * unitW; // 计算速度调整向量}}// 3. 如果发生碰撞else{									     const float invTimeStep = 1.0f / sim_->timeStep_;		     // 计算时间步长的倒数const Vector3 w = relativeVelocity - invTimeStep * relativePosition; // 计算w向量const float wLength = abs(w);					     // 计算w向量的长度const Vector3 unitW = w / wLength;				     // 计算w向量的单位向量plane.normal = unitW;				      // 设置平面的法向量u = (combinedRadius * invTimeStep - wLength) * unitW; // 计算速度调整向量}// 有多少个neighbor，就有多少个orca平面plane.point = velocity_ + 0.5f * u; // 计算平面上的点orcaPlanes_.push_back(plane);	    // 将平面添加到ORCA平面列表中}const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度，如果失败返回失败的平面索引if (planeFail < orcaPlanes_.size()){									 // 如果存在失败的平面linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); // 调用备用算法处理失败的平面}}

3. 线性规划求解出最优速度

linearProgram几个函数实现了一套线性规划（Linear Programming, LP）求解方法，目的是在有多个平面约束（即避障条件）的情况下找到最优的速度向量，以确保多个智能体不会发生碰撞。

初始调用

// 调用 linearProgram3 来计算满足所有约束（平面）的新的速度向量。如果失败，则返回失败的平面索引。const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_); // 计算新的速度，如果失败返回失败的平面索引if (planeFail < orcaPlanes_.size()) // 如果存在失败的平面{									 // 调用备用算法处理失败的平面linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_); }

3.1 `linearProgram3()`：求解所有平面的初步速度

	size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result){if(directionOpt) /* 如果使用方向优化，即只考虑速度方向，而不考虑速度大小。*/{result = optVelocity * radius; // 如果优化方向，将最优速度扩展到给定的半径}else if (absSq(optVelocity) > sqr(radius)){/* 优化最近点并且在圆外。 ？？ 是不是为了安全性啊，本来optVelocity不就是单位向量了吗*/result = normalize(optVelocity) * radius; // 如果最优速度的平方长度大于半径的平方，将其归一化并扩展到给定的半径}else{  /* 优化最近点并且在圆内。 */result = optVelocity; // 如果最优速度的平方长度小于或等于半径的平方，直接使用最优速度}for (size_t i = 0; i < planes.size(); ++i){// result 位于平面的外侧，不满足orca约束if (planes[i].normal * (planes[i].point - result) > 0.0f){const Vector3 tempResult = result; // 保存当前结果if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)){result = tempResult; // 如果 linearProgram2 返回 false，恢复之前的结果return i;	     // 返回不满足的平面的索引}}}return planes.size(); // 如果所有约束都满足，返回 planes.size()}

3.2 `linearProgram2()`：解决单个平面约束的最优速度

如图求出初步速度之后，这是满足了planeNo的约束，但可能破坏之前平面的约束，因此需要遍历planeNo之前的平面做检查
在这里插入图片描述

// 用于计算满足给定约束的速度向量。这个函数的主要目的是在智能体的避碰算法中，在一个半径为radius的球体内找到一个速度向量，使其尽可能接近给定的最优速度optVelocity，同时满足所有给定的平面约束。bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result){const float planeDist = planes[planeNo].point * planes[planeNo].normal; // 计算平面与原点的距离const float planeDistSq = sqr(planeDist);				// 计算距离的平方const float radiusSq = sqr(radius);					// 计算半径的平方// 用超过最大速度的速度才能达到orca避障平面if (planeDistSq > radiusSq){/* 最大速度球完全使平面 planeNo 无效。 */return false; // 如果平面距离的平方大于半径的平方，则返回false，表示无解}// 勾股定理计算最大速度radiusSq与平面中线的另外一边的平方const float planeRadiusSq = radiusSq - planeDistSq;		// 计算原点到平面中心的线const Vector3 planeCenter = planeDist * planes[planeNo].normal; if (directionOpt){/* 投影方向 optVelocity 到平面 planeNo 上。 */// optVelocity * planes[planeNo].normal 表示 optVelocity 在 planes[planeNo].normal 方向上的投影长度，这是一个标量，再乘以法向量，使之变为矢量const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal; // 计算平面上的最优速度const float planeOptVelocityLengthSq = absSq(planeOptVelocity);							// 计算平面上最优速度的平方长度if (planeOptVelocityLengthSq <= RVO_EPSILON){result = planeCenter; // 如果最优速度的平方长度小于一个很小的值，则结果为平面中心}else{// 否则，计算结果为平面中心加上最优速度在平面上的投影result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity; }}else{// 结果是optVelocity + optVelocity顶点离平面的最小距离向量result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal; // 计算点在平面上的投影// 就是结果超过最大速度if (absSq(result) > radiusSq){const Vector3 planeResult = result - planeCenter;				     // 计算结果相对于平面中心的向量const float planeResultLengthSq = absSq(planeResult);				     // 计算该向量的平方长度// 结果就是最大圆与平面的交点，并且里原始方向近的那个交点形成的向量result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult; }}// 新的result被求出，满足了planeNo的约束，但可能破坏之前的约束，因此需要检查for (size_t i = 0; i < planeNo; ++i){if (planes[i].normal * (planes[i].point - result) > 0.0f){	// 计算两个平面的法向量的叉积Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal); // 平面 planeNo 和 i（几乎）平行，因为平面 i 失败了，所以之前求出的平面 planeNo 也失败了if (absSq(crossProduct) <= RVO_EPSILON){return false; // 返回false}// 算出交线，result指到交线，那就可以同时满足这两个平面约束了呀Line line;line.direction = normalize(crossProduct); //平面交线方向		// 平面planeNo上并垂直于交线的线									      const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);	// ((planes[i].point - planes[planeNo].point) * planes[i].normal)两平面点连线在平面i法向量上的投影								      line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal; if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)){return false; // 如果 linearProgram1 返回 false，表示无解，返回 false}}}return true; // 返回 true，表示找到了解}

3.3 `linearProgram1()`：寻找线与圆形区域的交点

在这里插入图片描述

// 用于在一个圆形区域内（以给定的半径为界限）找到一条线的交点。bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result){const float dotProduct = line.point * line.direction;			      // 计算点和方向的点积const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point); // 计算判别式，用于判断交点是否在圆形区域内if (discriminant < 0.0f){// 如果判别式小于0，表示没有交点，返回falsereturn false; }const float sqrtDiscriminant = std::sqrt(discriminant); // 计算判别式的平方根float tLeft = -dotProduct - sqrtDiscriminant;		// 计算t的左边界float tRight = -dotProduct + sqrtDiscriminant;		// 计算t的右边界for (size_t i = 0; i < planeNo; ++i){const float numerator = (planes[i].point - line.point) * planes[i].normal; // 计算分子const float denominator = line.direction * planes[i].normal;		   // 计算分母if (sqr(denominator) <= RVO_EPSILON){/* 线几乎与平面i平行。 */if (numerator > 0.0f){return false; // 如果分子大于0，返回false，表示无解}else{continue; // 否则继续下一个平面}}const float t = numerator / denominator; // 计算t值if (denominator >= 0.0f){/* 平面i限制线的左边界。 */tLeft = std::max(tLeft, t); // 更新t的左边界}else{/* 平面i限制线的右边界。 */tRight = std::min(tRight, t); // 更新t的右边界}if (tLeft > tRight){return false; // 如果左边界超过右边界，返回false，表示无解}}// 优化方向if (directionOpt){if (optVelocity * line.direction > 0.0f){// 如果方向优化，则选择tRight作为结果result = line.point + tRight * line.direction; }else{// 否则选择tLeft作为结果result = line.point + tLeft * line.direction; }}else{/* 优化最近点。 */const float t = line.direction * (optVelocity - line.point); // 计算最优t值if (t < tLeft){result = line.point + tLeft * line.direction; // 如果t小于左边界，选择tLeft作为结果}else if (t > tRight){result = line.point + tRight * line.direction; // 如果t大于右边界，选择tRight作为结果}else{result = line.point + t * line.direction; // 否则选择t作为结果}}return true; // 返回true，表示找到了解}

3.4`linearProgram4()`：处理多个平面之间的约束冲突

其实这个代码是在构造一个新的平面集合projPlanes，然后重新调用linearProgram3()求解

projPlanes是对从有冲突的平面开始拿出一个平面i，然后找到这个平面i之前的平面j，用这两个平面i和平面j构造出一个中间平面重新调用linearProgram3()来解决问题

所谓的中间平面就是：

当两平面平行，就是中间的平行平面
当两平面相交，就是夹角那个方向的平面

其实我不是很理解这个中间平面的构造，但可以大致想一下就是因为两个平面ij限制太多了才找不到解，反正解也都是在平面边缘处找到的，不如找一个折中的平面，尝试一下这个位置是不是能找到。。。（待定）

	// 当 linearProgram3 从beginPlane无法满足约束时，linearProgram4 进一步处理这些情况。void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result){float distance = 0.0f; // 初始化距离为0for (size_t i = beginPlane; i < planes.size(); ++i){if (planes[i].normal * (planes[i].point - result) > distance){std::vector<Plane> projPlanes; for (size_t j = 0; j < i; ++j){Plane plane;// 计算两个平面的法向量的叉积，可以表示两个平面的交线的方向const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal); // 利用叉乘判断平面是否平行，绝对值小于0.1，则平行if (absSq(crossProduct) <= RVO_EPSILON) //RVO_EPSILON=0.1{	// 利用点乘判断平行平面方向，平面 i 和平面 j 指向相同方向if (planes[i].normal * planes[j].normal > 0.0f){continue;}else // 平面 i 和平面 j 指向相反方向。{// 平面点是两个平面点的中点plane.point = 0.5f * (planes[i].point + planes[j].point); }}else{// 在平面 i 内部，且垂直于交线方向的向量const Vector3 lineNormal = cross(crossProduct, planes[i].normal);	// (planes[j].point - planes[i].point) * planes[j].normal 是两点连线在平面 j 法向量方向上的投影			// (lineNormal * planes[j].normal) 表示 lineNormal 在平面 j 法向量方向上的投影。plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal; // 计算交点}plane.normal = normalize(planes[j].normal - planes[i].normal); // 计算投影平面的法向量并归一化projPlanes.push_back(plane);				       // 将投影平面添加到列表中}const Vector3 tempResult = result; // 保存当前结果if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()){/* 原则上不应该发生。这是因为结果已经在这个线性规划的可行区域内。如果失败，是由于小的浮点误差，并保持当前结果。 */result = tempResult;}distance = planes[i].normal * (planes[i].point - result); // 更新距离}}}}