Preparation & external solver workflow准备与外部求解流程#
SplineOptimizer<Spline, Parameterization> supplies a cost and gradient; it does not run an iterative solver. A prepared optimizer owns its reference input, layout and working storage. It is noncopyable, movable and exclusive to one synchronous evaluation at a time.SplineOptimizer<Spline, Parameterization> 提供代价与梯度,不执行迭代求解。准备后的优化器拥有参考输入、布局与工作存储;不可复制,可移动,同一时刻仅进行一次同步求值。
Deliver in this order: solver final x → successful update(x) → task constraint acceptance → copyPolynomial() → control-thread sampler. evaluate() changes the candidate; polynomial() is the last built trial, not an automatically accepted solution. Keep the saved snapshot alive and unchanged.交付顺序:求解器最终 x → update(x) 成功 → 规划约束验收 → copyPolynomial() → 控制线程采样。evaluate() 会改变候选;polynomial() 是最后构建的试探轨迹,不会自动成为接受解。保存的快照须存活且不变。
prepare()Validate and allocate验证并准备存储initialGuess()Copy initial coordinates复制初始坐标evaluate()External trial loop外部候选迭代update()Restore accepted x恢复接受解 x
prepare(input, options, objective)#
Copies inputs, encodes initial coordinates and prepares library buffers/capabilities. No objective reference is retained. Prepare user-owned cost resources separately.复制输入,编码初始坐标并准备库工作区及采样能力,不保留 objective 引用。用户代价资源另行提前准备。
template<class Objective>
Status prepare(const SplineInput<DIM>& input, const Objective& objective);
template<class Objective>
Status prepare(const SplineInput<DIM>& input, const OptimizerOptions& options,
const Objective& objective);
input- Initial physical trajectory data.初始物理轨迹数据。
options- Optional weights, optimization mask and integration settings.可选的权重、优化掩码与积分设置。
objective- User-defined callable costs, or NoObjective for built-in costs only.用户定义的可调用代价;仅用内置代价时传 NoObjective。
Returns返回Status
initialGuess()#
Get the initial decision vector after prepare(). Pass this vector to the numerical solver.prepare() 后取得初始决策向量,交给数值求解器。
Eigen::VectorXd initialGuess() const;
Returns返回Eigen::VectorXd
evaluate(x, gradient, objective, executor)#
x must have decisionDimension() entries. Owning g resizes/reuses. With matching output, fixed-topology evaluation and rejected-candidate retry do not allocate library storage.x 须匹配 decisionDimension();拥有型 g 按需调整尺寸或复用。输出匹配时,固定拓扑评价及失败重试不分配库内存。
template<class Objective, class Executor = SerialExecutor>
EvaluationResult evaluate(const Eigen::VectorXd& x, Eigen::VectorXd& gradient,
Objective& objective, const Executor& executor = {});
x- Candidate vector with decisionDimension() entries.包含 decisionDimension() 项的候选决策向量。
gradient- Overwritten decision gradient; owning storage resizes or reuses as needed.覆盖写入的决策梯度,拥有型存储按需调整尺寸或复用。
objective- The callable cost container.可调用代价容器。
executor- Optional integration executor; serial by default.可选积分执行器,默认串行。
Returns返回EvaluationResult {cost, status}
update(x)#
The accepted vector may differ from the last trial. Rebuild before consuming the result; no objective evaluation.接受向量可能不同于最后试探向量。消费结果前重建,不计算目标代价。
Status update(const Eigen::VectorXd& x);
x- The final decision vector returned by the solver.求解器返回的最终决策向量。
Returns返回Status
Complete optimization: custom costs → L-BFGS → final trajectory完整优化:自定义代价 → L-BFGS → 最终轨迹C++17
#include "SplineOptimizer.hpp"
#include <gcopter/lbfgs.hpp>
#include <iostream>
#include <numeric>
using Spline = SplineTrajectory::QuinticSplineND<3>;
using Optimizer = SplineTrajectory::SplineOptimizer<Spline>;
struct VelocityCost {
static constexpr int kDerivativeOrder = 1;
double weight = 0.02;
double operator()(const SplineTrajectory::IntegralPointInfo&,
const SplineTrajectory::IntegralState<3>& state,
SplineTrajectory::IntegralGradient<3>& gradient) const {
gradient.v += 2.0 * weight * state.v;
return weight * state.v.squaredNorm();
}
};
struct TargetDurationCost {
double target = 5.0;
double weight = 0.2;
double operator()(const SplineTrajectory::ParameterView<Spline>& state,
SplineTrajectory::ParameterGradientView<Spline>& gradient) const {
const double duration = std::accumulate(
state.durations.begin(), state.durations.end(), 0.0);
const double error = duration - target;
gradient.durations.array() += 2.0 * weight * error;
return weight * error * error;
}
};
struct Objective {
VelocityCost integral;
TargetDurationCost parameter;
};
int main() {
SplineTrajectory::SplineInput<3> input;
input.durations = {1.2, 1.5};
input.waypoints.resize(3, 3);
input.waypoints << 0, 0, 0, 1, 0.5, 0, 2, 0, 0;
SplineTrajectory::OptimizerOptions options;
options.energy_weight = 0.02;
options.duration_weight = 0.5;
options.integration_steps = 32;
Objective objective;
Optimizer optimizer;
if (const auto status = optimizer.prepare(input, options, objective); !status) {
std::cerr << status.message << '\n';
return 1;
}
auto x = optimizer.initialGuess();
struct Context { Optimizer& optimizer; Objective& objective; } context{optimizer, objective};
auto evaluate = [](void* instance, const Eigen::VectorXd& variables,
Eigen::VectorXd& gradient) {
auto& ctx = *static_cast<Context*>(instance);
return ctx.optimizer.evaluate(variables, gradient, ctx.objective).cost;
};
lbfgs::lbfgs_parameter_t parameters;
parameters.g_epsilon = 1e-6;
parameters.past = 5;
parameters.delta = 1e-9;
parameters.max_iterations = 500;
double cost = 0.0;
const int result = lbfgs::lbfgs_optimize(
x, cost, evaluate, nullptr, nullptr, &context, parameters);
if (result < 0) {
std::cerr << lbfgs::lbfgs_strerror(result) << '\n';
return 1;
}
if (const auto status = optimizer.update(x); !status) {
std::cerr << status.message << '\n';
return 1;
}
// Check application constraints here before accepting the trajectory.
auto trajectory = optimizer.copyPolynomial();
auto sampler = trajectory.makeSampler<2>();
const auto pva = sampler.evaluateDerivatives(
trajectory.startTime() + 0.5 * trajectory.duration());
std::cout << "Final cost: " << cost << "\nPosition: " << pva[0].transpose()
<< "\nVelocity: " << pva[1].transpose() << '\n';
}
The example uses the GCOPTER L-BFGS header bundled in benchmarks/planners/upstream/GCOPTER/gcopter/include. Save the program as main.cpp; with the repository at external/SplineTrajectory, use this application CMake file:示例使用仓库内 benchmarks/planners/upstream/GCOPTER/gcopter/include 下的 GCOPTER L-BFGS。将程序保存为 main.cpp,仓库放在 external/SplineTrajectory,应用的 CMake 文件如下:
cmake_minimum_required(VERSION 3.12)
project(spline_app LANGUAGES CXX)
set(BUILD_TESTING OFF CACHE BOOL "" FORCE)
set(SPLINE_BUILD_EXAMPLES OFF CACHE BOOL "" FORCE)
add_subdirectory(external/SplineTrajectory EXCLUDE_FROM_ALL)
add_executable(spline_app main.cpp)
target_link_libraries(spline_app PRIVATE SplineTrajectory::SplineTrajectory)
target_include_directories(spline_app SYSTEM PRIVATE
external/SplineTrajectory/benchmarks/planners/upstream/GCOPTER/gcopter/include)
Build with cmake -S . -B build and cmake --build build --target spline_app. Other solvers use the same evaluate(x, gradient, objective) callback. After solving, call update(x) with the solver’s final vector, check application constraints, then save copyPolynomial().执行 cmake -S . -B build 和 cmake --build build --target spline_app 即可构建。其他求解器同样调用 evaluate(x, gradient, objective)。求解后,用最终向量调用 update(x),验收规划约束,再通过 copyPolynomial() 保存。
Callable objective members可调用代价成员#
Define Objective in your application and give each cost a callable member such as integral or parameter. Each cost implements operator(), returns its cost and adds its partial derivatives. Return double for a numeric cost, or EvaluationResult when it can report failure. NoObjective selects only the enabled built-in costs.Objective 由用户定义,通过 integral、parameter 等可调用成员组合代价。每项代价实现 operator(),返回代价值并累加偏导。纯数值代价返回 double;需要报告失败时返回 EvaluationResult。NoObjective 仅使用已开启的内置代价。
struct VelocityCost {
static constexpr int kDerivativeOrder = 1;
double weight = 0.02;
double operator()(const SplineTrajectory::IntegralPointInfo&,
const SplineTrajectory::IntegralState<3>& state,
SplineTrajectory::IntegralGradient<3>& gradient) const {
gradient.v += 2.0 * weight * state.v;
return weight * state.v.squaredNorm();
}
};
struct TargetDurationCost {
double target = 5.0;
double weight = 0.2;
double operator()(const SplineTrajectory::ParameterView<Spline>& state,
SplineTrajectory::ParameterGradientView<Spline>& gradient) const {
const double duration = std::accumulate(
state.durations.begin(), state.durations.end(), 0.0);
const double error = duration - target;
gradient.durations.array() += 2.0 * weight * error;
return weight * error * error;
}
};
struct Objective {
VelocityCost integral;
TargetDurationCost parameter;
};
objective.integral#
Pointwise obstacles/dynamics. Unweighted integrand and p/v/a/j/s, trajectory_time partials. Optional kDerivativeOrder on this functor, default 4; allowed 0…4.逐点避障/动力学代价;未加权被积函数及 p/v/a/j/s、trajectory_time 偏导。此 Functor 可声明 kDerivativeOrder,默认 4,范围 0…4。
double operator()(const IntegralPointInfo& point,
const IntegralState<D>& state, IntegralGradient<D>& gradient) const;
point- Integration location and time.积分位置与时间信息。
state- Position and derivatives through the declared order.位置及所声明阶数以内的导数。
gradient- Unweighted integrand partials to add.要累加的未加积分权重的被积函数偏导。
Returns返回double cost, or EvaluationResult when reporting failure.double 代价值;需要报告失败时使用 EvaluationResult。
objective.sample#
Cross-sample position coupling. positions: M × D contiguous rows; trajectory_times: M. Return whole batch cost, no automatic quadrature weighting.跨点位置耦合;positions 为 M × D 连续行,trajectory_times 为 M 项。返回整批代价,不自动加积分权重。
double operator()(const Optimizer::SampleBuffer& samples,
SampleGradientView<Spline>& gradient) const;
samples- Segment-major quadrature records.按段排列的积分采样记录。
gradient- Position and explicit time partials, one row per record.位置与显式时间偏导,每行对应一条记录。
Returns返回double cost, or EvaluationResult when reporting failure.double 代价值;需要报告失败时使用 EvaluationResult。
objective.coefficient#
Hull/control costs. coefficients, independent durations and start_time partials before the spline adjoint.凸包/控制点代价;在样条伴随前累加 coefficients、独立 durations 和 start_time 偏导。
double operator()(const ParameterView<Spline>& state,
CoefficientGradientView<Spline>& gradient);
state- Current polynomial and physical parameters.当前多项式与物理参数。
gradient- Coefficient and independent-time partials to add.要累加的系数与独立时间偏导。
Returns返回double cost, or EvaluationResult when reporting failure.double 代价值;需要报告失败时使用 EvaluationResult。
objective.parameter#
Physical costs after adjoint: waypoints, durations, boundary.start/end.v/a/j, start_time. Same access paths as state; addWaypoint(i, partial) accepts rows or columns.伴随后物理代价:waypoints、durations、boundary.start/end.v/a/j、start_time,与输入访问路径对齐;addWaypoint 接受行或列。
double operator()(const ParameterView<Spline>& state,
ParameterGradientView<Spline>& gradient) const;
state- Current physical parameters.当前物理参数。
gradient- Waypoint, duration, boundary and start-time partials to add.要累加的途经点、时长、边界与起点时间偏导。
Returns返回double cost, or EvaluationResult when reporting failure.double 代价值;需要报告失败时使用 EvaluationResult。
objective.decision#
Direct regularization in decision coordinates; adds to already mapped gradients.决策坐标的直接正则项,累加至已映射梯度。
double operator()(const DecisionView& state,
Eigen::Ref<Eigen::VectorXd> gradient) const;
state- Decision coordinates and layout.决策坐标与布局。
gradient- Decision-space partials to add.要累加的决策坐标偏导。
Returns返回double cost, or EvaluationResult when reporting failure.double 代价值;需要报告失败时使用 EvaluationResult。
Prepare any cost-owned resources before solving. See the complete program and the optimizer guide.用户代价持有的资源在求解前准备。可直接使用上方的完整程序,更多协议说明见优化器指南。
Here Spline is QuinticSplineND<3>. The integral cost returns the velocity-squared integrand and adds its velocity partial. The parameter cost returns a total-duration penalty and adds the same partial to every segment duration. Built-in and custom costs are summed; set a built-in weight to zero to omit that term.这里 Spline 为 QuinticSplineND<3>。积分代价返回速度平方被积函数,并累加速度偏导;参数代价返回总时长罚项,并向每段时长累加同一个偏导。内置代价与自定义代价相加;某项内置权重设为零即可关闭该项。
Callback data and gradient fields回调数据与梯度字段#
Views borrow the optimizer’s data for the callback. Add partials to the supplied fields; the optimizer composes the remaining chain rule. ParameterView is read-only, and gradient views write to existing storage.视图在回调期间借用优化器数据。向对应字段累加偏导,余下的链式求导由优化器完成。ParameterView 只读,梯度视图写入现有存储。
IntegralPointInfo describes one quadrature point. With N segments and L = integration_steps, records are ordered by segment, then step; M = N·(L+1). Every segment includes both endpoints, so an inner waypoint appears twice.IntegralPointInfo 描述一个积分点。设 N 为段数、L 为 integration_steps,记录先按段、再按步排列,总数 M = N·(L+1)。每段都包含两端点,因此内部途经点出现两次。
To visit each waypoint only once in a discrete batch cost, skip point.isSegmentStart() && !point.isTrajectoryStart() and leave those gradient rows zero. Integral costs use both endpoint records with their trapezoidal weights.离散批量代价若需每个途经点只计一次,可跳过 point.isSegmentStart() && !point.isTrajectoryStart(),并将对应梯度行留零。积分代价使用两份端点记录及其梯形权重。
Time, spatial & auxiliary maps时间、空间与辅助映射#
Parameterization holds time, space and auxiliary. Defaults are QuadInvTimeMap, IdentitySpatialMap<D>, NoAuxiliaryMap. makeOptimizer<Spline>(maps) owns maps by value; std::ref/std::cref explicitly borrow longer-lived objects. Keep dimensions/inverse semantics fixed until reprepare.Parameterization 持有 time、space、auxiliary;默认 QuadInvTimeMap、IdentitySpatialMap<D>、NoAuxiliaryMap。makeOptimizer<Spline>(maps) 按值拥有映射;std::ref/std::cref 显式借用长生命周期对象。重新准备前不改变维度或逆映射语义。
time.forward(tau)#
Decode one time coordinate into a positive segment duration in seconds.将一个时间坐标解码为正的段时长,单位为秒。
double forward(double tau) const;
Returns返回double
time.inverse(duration)#
Encode an initial physical duration during prepare().prepare() 时将初始物理时长编码为决策坐标。
double inverse(double duration) const;
Returns返回double
time.backward(tau, duration, grad_duration)#
Pull a duration partial back to its time coordinate. The duration is the cached value before auxiliary transformations.将时长偏导传回时间坐标,duration 为辅助变换前缓存的时长。
double backward(double tau, double duration, double grad_duration) const;
Returns返回double
space.dimension(waypoint_index)#
Return the number of decision coordinates for waypoint 0…N, including endpoints.返回第 0…N 个途经点所需的决策坐标数,包含两端点。
int dimension(int waypoint_index) const;
Returns返回int
space.forward(x, waypoint_index)#
Decode one waypoint’s coordinate slice into a physical position. Vector is Spline::Vector.将一个途经点的决策切片解码为物理位置,Vector 为 Spline::Vector。
Vector forward(const Eigen::Ref<const Eigen::VectorXd>& x,
int waypoint_index) const;
Returns返回Vector
space.inverse(point, waypoint_index)#
Encode an initial physical waypoint into dimension(waypoint_index) coordinates.将初始物理途经点编码为 dimension(waypoint_index) 个坐标。
Eigen::VectorXd inverse(const Vector& point, int waypoint_index) const;
Returns返回Eigen::VectorXd
space.backward(x, gradient, waypoint_index, output)#
Pull the physical position partial back to this waypoint’s coordinates, overwriting output.将物理位置偏导传回该途经点的决策坐标,覆盖写入 output。
void backward(const Eigen::Ref<const Eigen::VectorXd>& x, const Vector& gradient,
int waypoint_index, Eigen::Ref<Eigen::VectorXd> output) const;
Returns返回void
auxiliary.dimension()#
Return the number of additional decision coordinates.返回额外决策坐标的维度。
int dimension() const;
Returns返回int
auxiliary.initial(reference)#
Build the initial auxiliary coordinates from the reference input during prepare().prepare() 时从参考输入生成初始辅助坐标。
StatusOr<Eigen::VectorXd> initial(const SplineInput<D>& reference) const;
Returns返回StatusOr<Eigen::VectorXd>
auxiliary.apply(z, state)#
Transform physical parameters using auxiliary coordinates z, preserving their topology.使用辅助坐标 z 变换物理参数,保持拓扑不变。
Status apply(const Eigen::Ref<const Eigen::VectorXd>& z,
MutableParameters<Spline>& state) const;
Returns返回Status
auxiliary.backward(z, state, gradient, grad_z)#
Add auxiliary-coordinate partials to grad_z and pull physical gradients back before the time and spatial maps. Put costs in parameter or decision.向 grad_z 累加辅助坐标偏导,并在时间、空间映射之前传回物理梯度。代价放在 parameter 或 decision。
Status backward(const Eigen::Ref<const Eigen::VectorXd>& z,
const ParameterView<Spline>& state, ParameterGradientView<Spline>& gradient,
Eigen::Ref<Eigen::VectorXd> grad_z) const;
Returns返回Status
Choose direct duration coordinates选择直接时长坐标C++17
#include "SplineOptimizer.hpp"
#include <iostream>
int main() {
using Spline = SplineTrajectory::QuinticSplineND<3>;
struct Maps {
SplineTrajectory::IdentityTimeMap time;
SplineTrajectory::IdentitySpatialMap<3> space;
SplineTrajectory::NoAuxiliaryMap auxiliary;
};
auto optimizer = SplineTrajectory::makeOptimizer<Spline>(Maps{});
SplineTrajectory::SplineInput<3> input;
input.durations = {1.2, 1.5};
input.waypoints.resize(3, 3);
input.waypoints << 0, 0, 0, 1, 0.5, 0, 2, 0, 0;
SplineTrajectory::OptimizerOptions options;
options.duration_weight = 0.5;
SplineTrajectory::NoObjective objective;
if (!optimizer.prepare(input, options, objective)) return 1;
const auto x = optimizer.initialGuess();
Eigen::VectorXd gradient;
const auto result = optimizer.evaluate(x, gradient, objective);
if (!result) return 1;
// With IdentityTimeMap, the first two coordinates are durations in seconds.
std::cout << "Durations: " << x.head(2).transpose()
<< "\nDuration gradient: " << gradient.head(2).transpose() << '\n';
return (x.head(2).isApprox(Eigen::Vector2d(1.2, 1.5)) &&
gradient.head(2).isApprox(Eigen::Vector2d(0.5, 0.5))) ? 0 : 1;
}
The physical cost code stays the same when maps change. Preparation uses inverse; evaluation decodes with forward, applies the auxiliary map, computes costs, then calls the maps’ backward functions. A start_time gradient reaches decision variables through an auxiliary map.更换映射时,物理代价代码保持一致。准备时调用 inverse;评价时通过 forward 解码、应用辅助映射并计算代价,再由各映射的 backward 传回决策梯度。start_time 偏导通过辅助映射传到决策变量。