我试图弄清楚如何近似我的n阶贝塞尔曲线的长度，我发现我可以使用高斯正交积分来近似它。在我的控制点P1（220,40），P2（220,260）P3（35,200）和P4（120,160）多次尝试后，我的程序应该给我一个272.87的曲线长度（如图所示） here）;但是，我只获得了大约229.18的结果。我已经从GaussLegendre 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;
}
}
答案 0 :(得分：2)
要减少累积错误，您可以使用
sum += Ci * Math.hypot(point.getX(), point.getY());
而不是
sum += Ci * Math.sqrt(Math.pow(point.getX(), 2) + Math.pow(point.getY(), 2));
（但是，由于您发布的代码不完整，我无法用它来验证此更改会产生多大影响）
答案 1 :(得分：2)
参数值p1和p2之间的参数化曲线x（p），y（p）的弧长是
Integral{ p1<=p<=p2 | hypot( xdot(p), ydot(p)) dp
其中xdot是x的导数和y的ydot。
在我看来，好像你在计算
积分{p1＆lt; = p＆lt; = p2 | hypot（x（p），y（p））dp
代替。
答案 2 :(得分：0)
我设法解决了我的问题。我发现我做错了2件事1）我正在计算导数不正确2）根据@dmuir响应，我没有使用导数来计算积分。
这是我的代码，它返回正确的贝塞尔曲线弧长：
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;
}
}