RVO源码阅读笔记

KDTREE

空间K维划分, 用来快速寻找距离最近的障碍(指定数量) 优化RVO的资源消耗.

路径规划

高级规划

DIJKSTRA 最短路径 等寻路方法

底层规划

VO RVO ORCA等Obstacle Avoidance避障算法

KDTREE

这个是一个简单的KDTREE实现 用来寻找距离最近的障碍(指定数量) 优化RVO的资源消耗.

VO Velocity Obstacles 速度障碍

核心思想: 只要在未来有可能会发生碰撞的速度 都排除在外
抖动现象: 两个位移单位存在可能会发生路径碰撞的情况下会同时采取保守的避让速度,导致新速度偏离过大又大幅度回归,从而产生震荡.

RVO Reciprocal Velocity Obstacles 互惠的速度障碍

核心思想: 优化VO思想, 假定对方也会采取避障行为, 缩小(average VO)速度.

ORCA Optimal Reciprocal Collision Avoidance 最优互惠碰撞避免

核心思想: 优化RVO, 额外考虑速度大小, 求解过程使用线性规划,更高效简洁.

• 对其他所有agents的ORCA求交（线性规划），再与自己可选速度求交集，得候选速度集ORCAτA
• 在候选集中求解跟自己偏好速度最近的一个速度vnewA

computeNewVelocity

/* Search for the best new velocity. */
void Agent::computeNewVelocity()
{
    orcaLines_.clear();

    const size_t numObstLines = orcaLines_.size();

    const float invTimeHorizon = 1.0f / timeHorizon_;

    /* Create agent ORCA lines. */
    for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
        const Agent *const other = agentNeighbors_[i].second;

        const Vector2 relativePosition = other->position_ - position_;
        const Vector2 relativeVelocity = velocity_ - other->velocity_;
        const float distSq = absSq(relativePosition);
        const float combinedRadius = radius_ + other->radius_;
        const float combinedRadiusSq = sqr(combinedRadius);

        Line line;
        Vector2 u;

        if (distSq > combinedRadiusSq) {
            /* No collision. */
            const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
            /* Vector from cutoff center to relative velocity. */
            const float wLengthSq = absSq(w);

            const float dotProduct1 = w * relativePosition;

            if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
                /* Project on cut-off circle. */
                const float wLength = std::sqrt(wLengthSq);
                const Vector2 unitW = w / wLength;

                line.direction = Vector2(unitW.y(), -unitW.x());
                u = (combinedRadius * invTimeHorizon - wLength) * unitW;
            }
            else {
                /* Project on legs. */
                const float leg = std::sqrt(distSq - combinedRadiusSq);

                if (det(relativePosition, w) > 0.0f) {
                    /* Project on left leg. */
                    line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
                }
                else {
                    /* Project on right leg. */
                    line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
                }

                const float dotProduct2 = relativeVelocity * line.direction;

                u = dotProduct2 * line.direction - relativeVelocity;
            }
        }
        else {
            /* Collision. Project on cut-off circle of time timeStep. */
            const float invTimeStep = 1.0f / sim_->timeStep_;

            /* Vector from cutoff center to relative velocity. */
            const Vector2 w = relativeVelocity - invTimeStep * relativePosition;

            const float wLength = abs(w);
            const Vector2 unitW = w / wLength;

            line.direction = Vector2(unitW.y(), -unitW.x());
            u = (combinedRadius * invTimeStep - wLength) * unitW;
        }

        line.point = velocity_ + 0.5f * u;
        orcaLines_.push_back(line);
    }

    size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);

    if (lineFail < orcaLines_.size()) {
        //linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
    }
}

linearProgram2

size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
    if (directionOpt) {
        /*
            * Optimize direction. Note that the optimization velocity is of unit
            * length in this case.
            */
        result = optVelocity * radius;
    }
    else if (absSq(optVelocity) > sqr(radius)) {
        /* Optimize closest point and outside circle. */
        result = normalize(optVelocity) * radius;
    }
    else {
        /* Optimize closest point and inside circle. */
        result = optVelocity;
    }

    for (size_t i = 0; i < lines.size(); ++i) {
        if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
            /* Result does not satisfy constraint i. Compute new optimal result. */
            const Vector2 tempResult = result;

            if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
                result = tempResult;
                return i;
            }
        }
    }

    return lines.size();
}

linearProgram1

bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
    const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
    const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);

    if (discriminant < 0.0f) {
        /* Max speed circle fully invalidates line lineNo. */
        return false;
    }

    const float sqrtDiscriminant = std::sqrt(discriminant);
    float tLeft = -dotProduct - sqrtDiscriminant;
    float tRight = -dotProduct + sqrtDiscriminant;

    for (size_t i = 0; i < lineNo; ++i) {
        const float denominator = det(lines[lineNo].direction, lines[i].direction);
        const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);

        if (std::fabs(denominator) <= RVO_EPSILON) {
            /* Lines lineNo and i are (almost) parallel. */
            if (numerator < 0.0f) {
                return false;
            }
            else {
                continue;
            }
        }

        const float t = numerator / denominator;

        if (denominator >= 0.0f) {
            /* Line i bounds line lineNo on the right. */
            tRight = std::min(tRight, t);
        }
        else {
            /* Line i bounds line lineNo on the left. */
            tLeft = std::max(tLeft, t);
        }

        if (tLeft > tRight) {
            return false;
        }
    }

    if (directionOpt) {
        /* Optimize direction. */
        if (optVelocity * lines[lineNo].direction > 0.0f) {
            /* Take right extreme. */
            result = lines[lineNo].point + tRight * lines[lineNo].direction;
        }
        else {
            /* Take left extreme. */
            result = lines[lineNo].point + tLeft * lines[lineNo].direction;
        }
    }
    else {
        /* Optimize closest point. */
        const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);

        if (t < tLeft) {
            result = lines[lineNo].point + tLeft * lines[lineNo].direction;
        }
        else if (t > tRight) {
            result = lines[lineNo].point + tRight * lines[lineNo].direction;
        }
        else {
            result = lines[lineNo].point + t * lines[lineNo].direction;
        }
    }

    return true;
}