合肥建设集团招聘信息网站,网站的demo怎么做,wordpress切换背景,电商数据二维ORCA原理参考#xff1a; https://zhuanlan.zhihu.com/p/669426124
ORCA原理图解
1. 找到避障速度增量 u
碰撞处理分为三种情况#xff1a; #xff08;1#xff09;没有发生碰撞#xff0c;且相对速度落在小圆里 #xff08;2#xff09;没有发生碰撞#xff0…二维ORCA原理参考 https://zhuanlan.zhihu.com/p/669426124
ORCA原理图解
1. 找到避障速度增量 u
碰撞处理分为三种情况 1没有发生碰撞且相对速度落在小圆里 2没有发生碰撞且相对速度落在圆锥里 3发生碰撞马上做出反应 timeStep 决定了仿真每一步的时间更新间隔是系统的时间推进基础。较小的 timeStep 可以提高仿真的精度但会增加计算量。
timeHorizon 决定了智能体在进行避障计算时预测的时间范围。较大的 timeHorizon 值使得智能体可以更早预测潜在碰撞但会减少它的速度选择自由度。
timeStep 是碰撞时需要计算的调整u所需的时间 timeHorizon 是未发生碰撞时需要计算的u所化的时间他是一种提前预测
2. 添加速度障碍平面
表示一个平面需要法向量和平面上的点
1和2对应代码如下 // 它使用ORCAOptimal 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求解方法目的是在有多个平面约束即避障条件的情况下找到最优的速度向量以确保多个智能体不会发生碰撞。
linearProgram1()寻找线与圆形区域的交点 linearProgram2()解决单个平面约束的最优速度 linearProgram3()求解所有平面的初步速度 linearProgram4()处理多个平面之间的约束冲突
