高斯正交近似 Java 中的贝塞尔曲线长度

Question

我试图弄清楚如何近似计算我的 n 次贝塞尔曲线的长度，我发现我可以使用高斯正交积分来近似它。在多次尝试 P1(220, 40)、P2(220, 260) P3(35, 200) 和 P4(120, 160) 的控制点后，我的程序应该给出 272.87 的曲线长度（如图所示） here); however, I have only been getting results of around 229.18. I have checked my constants from the GaussLegendre class 他们是正确的。
有人可以告诉我我做错了什么。

这是class的全文

import java.util.Arrays;
import java.util.List;
import org.waltonrobotics.controller.PathData;
import org.waltonrobotics.controller.Pose;

/**
 * Everything about Bezier Curves https://pomax.github.io/bezierinfo/
 */
public class DynamicBezierCurve extends Path {

    private final double startVelocity;
    private final double endVelocity;
    private final int degree;
    private double[] coefficients;

    /**
     * This constructor is used with the splines, but feel free to use it when creating your own motions
     *
     * @param vCruise - the cruise velocity of the robot
     * @param aMax - the maximum acceleration of the robot
     * @param startVelocity - the start velocity
     * @param endVelocity - the end velocity
     * @param isBackwards - whether or not to move the robot backwards
     * @param controlPoints - the control points that define the curve
     */
    public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
        boolean isBackwards, List<Pose> controlPoints) {
        super(vCruise, aMax, isBackwards, controlPoints);
        this.startVelocity = startVelocity;
        this.endVelocity = endVelocity;
        // The starting average encoder distance should always be 0

        degree = getKeyPoints().size() - 1;
        coefficients = calculateCoefficients(degree);
    }

    public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
        boolean isBackwards,
        Pose... controlPoints) {
        this(vCruise, aMax, startVelocity, endVelocity, isBackwards, Arrays.asList(controlPoints));
    }

    /**
     * Uses the formula to find the value of nCr
     *
     * @return nCr
     */
    private static double findNumberOfCombination(int n, int r) {
        int nFactorial = factorial(n);
        int rFactorial = factorial(r);
        int nMinusRFactorial = factorial(n - r);

        return nFactorial / (rFactorial * nMinusRFactorial);
    }

    /**
     * Finds the factorial of any integer or double, d
     *
     * @return the factorial of d
     */
    private static int factorial(int d) {
        int result = 1;

        for (int i = 1; i <= d; i++) {
            result = result * i;
        }

        return result;
    }


    public double computeArcLength() {

        int n = 10;

        GaussLegendre gl = new GaussLegendre(n, -1, 1);

        double[] t = gl.getNodes();
        double[] C = gl.getWeights();
        double z = 1;

        double sum = 0;

        double zDivision = z / 2;

        for (int i = 0; i < t.length; i++) {
            double ti = t[i];
            ti = zDivision * ti + zDivision;

            Pose point = getDerivative(ti);

            double Ci = C[i];

            sum += Ci * Math.hypot(point.getX(), point.getY());
        }

        sum = zDivision * sum;

        return sum;
    }

    @Override
    public PathData createPathData(double startAverageEncoderLength, PathData previousPathData, double percentage) {
        Pose centerPoint = getPoint(percentage);

        PathData pathData;
//      pathData= calculateData(startAverageEncoderLength, previousPathData, centerPoint);
        pathData = new PathData(centerPoint);
        return pathData;
    }


    private Pose getDerivative(double percentage) {
        double dx = 0;
        double dy = 0;

        if (percentage == 1.0) {

            int last = getKeyPoints().size() - 1;

            dx = getKeyPoints().get(last).getX()
                - getKeyPoints().get(last - 1).getX();
            dy = getKeyPoints().get(last).getY()
                - getKeyPoints().get(last - 1).getY();
        } else {

            for (int i = 0; i < degree; i++) {

                Pose pointI = getKeyPoints().get(i);

                double multiplier =
                    coefficients[i] * StrictMath.pow(1 - percentage, (degree - i)) * StrictMath
                        .pow(percentage, (double) i);

                Pose nextPointI = getKeyPoints().get(i + 1);

                dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
                dy += multiplier * (nextPointI.getY() - pointI.getY());
            }
        }

        double angle = StrictMath.atan2(dy, dx);

        if (isBackwards()) {
            angle += Math.PI;
        }
        angle %= (2 * Math.PI);

        return new Pose(dx, dy, angle);
    }

    private Pose getPoint(double percentage) {
        return getPoint(degree, percentage);
    }

    /**
     * @param percentage - t
     * @return the Pose that is at percentage t along the curve
     */
    private Pose getPoint(double degree, double percentage) {
        double xCoordinateAtPercentage = 0;
        double yCoordinateAtPercentage = 0;

        double dx = 0;
        double dy = 0;

        for (int i = 0; i <= degree; i++) {

            Pose pointI = getKeyPoints().get(i);

            double multiplier =
                coefficients[i] * StrictMath.pow(1 - percentage, (degree - i)) * StrictMath.pow(percentage, (double) i);

            xCoordinateAtPercentage += (multiplier * pointI.getX());
            yCoordinateAtPercentage += (multiplier * pointI.getY());

            if (percentage != 1 && i < degree) {
                Pose nextPointI = getKeyPoints().get(i + 1);

                dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
                dy += multiplier * (nextPointI.getY() - pointI.getY());
            }
        }

        if (percentage == 1.0) {
            int last = getKeyPoints().size() - 1;

            dx = getKeyPoints().get(last).getX()
                - getKeyPoints().get(last - 1).getX();
            dy = getKeyPoints().get(last).getY()
                - getKeyPoints().get(last - 1).getY();
        }

        double angle = StrictMath.atan2(dy, dx);

        if (isBackwards()) {
            angle += Math.PI;
        }
        angle %= (2 * Math.PI);

        return new Pose(xCoordinateAtPercentage, yCoordinateAtPercentage, angle);
//      return new Pose(dx, dy, angle);
    }


    /**
     * Updates the coefficients used for calculations
     */
    private double[] calculateCoefficients(int degree) {
        double[] coefficients = new double[degree + 1];
        for (int i = 0; i < coefficients.length; i++) {
            coefficients[i] = findNumberOfCombination(degree, i);
        }

        return coefficients;
    }
}

Answer 1

为了减少累积误差，可以使用

sum += Ci * Math.hypot(point.getX(), point.getY());

而不是

sum += Ci * Math.sqrt(Math.pow(point.getX(), 2) + Math.pow(point.getY(), 2));

（但是，由于您发布的代码不完整，我无法用它来验证此更改会产生多大影响）

Answer 2

参数值 p1 和 p2 之间的参数化曲线 x(p)、y(p) 的弧长为

Integral{ p1<=p<=p2 | hypot( xdot(p), ydot(p)) dp

其中 xdot 是 x 和 ydot 的导数。

在我看来你好像在计算

积分{ p1<=p<=p2 | hypot( x(p), y(p)) dp

相反。

Answer 3

我已经设法解决了我的问题。我发现我做错了两件事 1) 我计算导数不正确 2) 根据上面的 @dmuir 回复，我没有使用导数来计算积分。

这是我的代码，returns 正确的贝塞尔曲线弧长：

import java.util.Arrays;
import java.util.List;
import org.waltonrobotics.controller.PathData;
import org.waltonrobotics.controller.Pose;

/**
 * Everything about Bezier Curves https://pomax.github.io/bezierinfo/
 */
public class DynamicBezierCurve extends Path {

    private final double startVelocity;
    private final double endVelocity;
    private final int degree;
    private double[] coefficients;

    /**
     * This constructor is used with the splines, but feel free to use it when creating your own motions
     *
     * @param vCruise - the cruise velocity of the robot
     * @param aMax - the maximum acceleration of the robot
     * @param startVelocity - the start velocity
     * @param endVelocity - the end velocity
     * @param isBackwards - whether or not to move the robot backwards
     * @param controlPoints - the control points that define the curve
     */
    public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
        boolean isBackwards, List<Pose> controlPoints) {
        super(vCruise, aMax, isBackwards, controlPoints);
        this.startVelocity = startVelocity;
        this.endVelocity = endVelocity;
        // The starting average encoder distance should always be 0

        degree = getKeyPoints().size() - 1;
        coefficients = calculateCoefficients(degree);
    }

    public DynamicBezierCurve(double vCruise, double aMax, double startVelocity, double endVelocity,
        boolean isBackwards,
        Pose... controlPoints) {
        this(vCruise, aMax, startVelocity, endVelocity, isBackwards, Arrays.asList(controlPoints));
    }

    /**
     * Uses the formula to find the value of nCr
     *
     * @return nCr
     */
    private static double findNumberOfCombination(int n, int r) {
        int nFactorial = factorial(n);
        int rFactorial = factorial(r);
        int nMinusRFactorial = factorial(n - r);

        return nFactorial / (rFactorial * nMinusRFactorial);
    }

    /**
     * Finds the factorial of any integer or double, d
     *
     * @return the factorial of d
     */
    private static int factorial(int d) {
        int result = 1;

        for (int i = 1; i <= d; i++) {
            result = result * i;
        }

        return result;
    }


    public double computeArcLength() {

        int n = 100;

        GaussLegendre gl = new GaussLegendre(n, -1, 1);

        double[] t = gl.getNodes();
        double[] C = gl.getWeights();
        double z = 1;

        double sum = 0;

        double zDivision = z / 2;

        for (int i = 0; i < t.length; i++) {
            double ti = t[i];
            ti = zDivision * ti + zDivision;

            Pose point = getDerivative(ti);

            double Ci = C[i];

            sum += Ci * Math.hypot(point.getX(), point.getY());
        }

        sum = zDivision * sum;

        return sum;
    }

    @Override
    public PathData createPathData(double startAverageEncoderLength, PathData previousPathData, double percentage) {
        Pose centerPoint = getPoint(percentage);

        PathData pathData;
//      pathData= calculateData(startAverageEncoderLength, previousPathData, centerPoint);
        pathData = new PathData(centerPoint);
        return pathData;
    }


    private Pose getDerivative(double percentage) {
        double dx = 0;
        double dy = 0;

        if (percentage == 1.0) {

            int last = getKeyPoints().size() - 1;

            dx = getKeyPoints().get(last).getX()
                - getKeyPoints().get(last - 1).getX();
            dy = getKeyPoints().get(last).getY()
                - getKeyPoints().get(last - 1).getY();
        } else {

            double[] coefficients = calculateCoefficients(degree - 1);

            for (int i = 0; i < degree; i++) {

                Pose pointI = getKeyPoints().get(i);

                double multiplier =
                    coefficients[i] * StrictMath.pow(1 - percentage, ((degree - 1) - i)) * StrictMath
                        .pow(percentage, (double) i);

                Pose nextPointI = getKeyPoints().get(i + 1);

                dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
                dy += multiplier * (nextPointI.getY() - pointI.getY());
            }
        }

        double angle = StrictMath.atan2(dy, dx);

        if (isBackwards()) {
            angle += Math.PI;
        }
        angle %= (2 * Math.PI);

        return new Pose(dx, dy, angle);
    }

    private Pose getPoint(double percentage) {
        return getPoint(degree, percentage);
    }

    /**
     * @param percentage - t
     * @return the Pose that is at percentage t along the curve
     */
    private Pose getPoint(double degree, double percentage) {
        double xCoordinateAtPercentage = 0;
        double yCoordinateAtPercentage = 0;

        double dx = 0;
        double dy = 0;

        for (int i = 0; i <= degree; i++) {

            Pose pointI = getKeyPoints().get(i);

            double multiplier =
                coefficients[i] * StrictMath.pow(1 - percentage, (degree - i)) * StrictMath.pow(percentage, (double) i);

            xCoordinateAtPercentage += (multiplier * pointI.getX());
            yCoordinateAtPercentage += (multiplier * pointI.getY());

            if (percentage != 1 && i < degree) {
                Pose nextPointI = getKeyPoints().get(i + 1);

                dx += (multiplier = multiplier * (degree)) * (nextPointI.getX() - pointI.getX());
                dy += multiplier * (nextPointI.getY() - pointI.getY());
            }
        }

        if (percentage == 1.0) {
            int last = getKeyPoints().size() - 1;

            dx = getKeyPoints().get(last).getX()
                - getKeyPoints().get(last - 1).getX();
            dy = getKeyPoints().get(last).getY()
                - getKeyPoints().get(last - 1).getY();
        }

        double angle = StrictMath.atan2(dy, dx);

        if (isBackwards()) {
            angle += Math.PI;
        }
        angle %= (2 * Math.PI);

        return new Pose(xCoordinateAtPercentage, yCoordinateAtPercentage, angle);
//      return new Pose(dx, dy, angle);
    }


    /**
     * Updates the coefficients used for calculations
     */
    private double[] calculateCoefficients(int degree) {
        double[] coefficients = new double[degree + 1];
        for (int i = 0; i < coefficients.length; i++) {
            coefficients[i] = findNumberOfCombination(degree, i);
        }

        return coefficients;
    }
}

高斯正交近似 Java 中的贝塞尔曲线长度

Gauss Quadrature to approximate Bezier Curve length in Java

java

math

bezier