From 8c40281ef9439c3fbe98e6121674d7c9d0d1e318 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 28 Aug 2025 11:18:17 +0800 Subject: [PATCH] update abstract_interpolation --- include/utils/base/abstract_interpolation.h | 79 +++++++--- src/utils/base/abstract_interpolation.cpp | 154 ++++++++++++-------- 2 files changed, 154 insertions(+), 79 deletions(-) diff --git a/include/utils/base/abstract_interpolation.h b/include/utils/base/abstract_interpolation.h index 97c84f3f..18d1341b 100644 --- a/include/utils/base/abstract_interpolation.h +++ b/include/utils/base/abstract_interpolation.h @@ -1,31 +1,72 @@ #ifndef INTERPOLATION_BASE_H #define INTERPOLATION_BASE_H +#include "utils/math/geometry.h" #include #include -// 插值基类 -class AbstractInterpolation { -protected: - std::vector x_points; // 已知点的x坐标 - std::vector y_points; // 已知点的y坐标 +namespace cmvr::math { - // 查找x所在的区间索引（二分查找） - int findInterval(double x) const; + /** + * 插值基类，用于机械臂位姿插值 + * 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值 + */ + class AbstractInterpolation { + public: + // 构造函数 + AbstractInterpolation(const std::vector& waypoints); + AbstractInterpolation(const std::vector& waypoints); -public: - // 构造函数 - AbstractInterpolation(const std::vector& x, const std::vector& y); - - // 析构函数 - virtual ~AbstractInterpolation() = default; + // 析构函数 + virtual ~AbstractInterpolation() = default; - // 纯虚函数：单个点插值 - virtual double interpolate(double x) const = 0; + /** + * 插值计算接口 + * @param t 插值参数，范围[0, 1]，0表示起点，1表示终点 + * @return 插值得到的位姿 + */ + virtual Pose3d interpolate_3d(double t) const = 0; + virtual Pose2d interpolate_2d(double t) const = 0; - // 批量插值（默认实现） - virtual std::vector interpolate(const std::vector& x_values) const; -}; + /** + * 生成轨迹点序列 + * @param num_points 生成的轨迹点数量，包括起点和终点 + * @return 轨迹点序列 + */ + std::vector generate_trajectory_3d(size_t num_points) const; + std::vector generate_trajectory_2d(size_t num_points) const; + + protected: + // 路径点存储 + std::vector waypoints_3d_; + std::vector waypoints_2d_; + + /** + * 辅助函数：线性插值单个数值 + * @param a 起始值 + * @param b 目标值 + * @param t 插值参数[0,1] + * @return 插值结果 + */ + static double linear_interpolate(double a, double b, double t); + + /** + * 辅助函数：四元数球面线性插值(slerp) + * @param q1 起始四元数 + * @param q2 目标四元数 + * @param t 插值参数[0,1] + * @return 插值结果 + */ + static Quat slerp(const Quat& q1, const Quat& q2, double t); + + /** + * 检查t的范围并进行约束 + * @param t 原始参数 + * @return 约束在[0,1]范围内的参数 + */ + static double clamp_t(double t); + }; + +} // namespace cmvr::math #endif // INTERPOLATION_BASE_H - \ No newline at end of file diff --git a/src/utils/base/abstract_interpolation.cpp b/src/utils/base/abstract_interpolation.cpp index 65d645f4..1ab6ed29 100644 --- a/src/utils/base/abstract_interpolation.cpp +++ b/src/utils/base/abstract_interpolation.cpp @@ -1,73 +1,107 @@ #include "utils/base/abstract_interpolation.h" #include -#include +#include + +namespace cmvr::math { // 构造函数实现 -AbstractInterpolation::AbstractInterpolation(const std::vector& x, const std::vector& y) { - // 验证输入 - if (x.size() != y.size()) { - throw std::invalid_argument("x和y的长度必须相同"); - } - if (x.size() < 2) { - throw std::invalid_argument("至少需要2个已知点才能进行插值"); - } - - // 复制并排序数据点 - std::vector> points; - points.reserve(x.size()); - - for (size_t i = 0; i < x.size(); ++i) { - points.emplace_back(x[i], y[i]); - } - - // 按x坐标排序 - std::sort(points.begin(), points.end()); - - // 分离排序后的x和y - x_points.reserve(points.size()); - y_points.reserve(points.size()); - - for (const auto& p : points) { - x_points.push_back(p.first); - y_points.push_back(p.second); +AbstractInterpolation::AbstractInterpolation(const std::vector& waypoints) + : waypoints_3d_(waypoints) { + if (waypoints.size() < 2) { + throw std::invalid_argument("至少需要2个路径点才能进行插值"); } } -// 查找区间索引实现 -int AbstractInterpolation::findInterval(double x) const { - // 处理边界情况 - if (x <= x_points[0]) { - return 0; +AbstractInterpolation::AbstractInterpolation(const std::vector& waypoints) + : waypoints_2d_(waypoints) { + if (waypoints.size() < 2) { + throw std::invalid_argument("至少需要2个路径点才能进行插值"); } - if (x >= x_points.back()) { - return static_cast(x_points.size()) - 2; - } - - // 二分查找 - int left = 0; - int right = static_cast(x_points.size()) - 1; - - while (right - left > 1) { - int mid = (left + right) / 2; - if (x < x_points[mid]) { - right = mid; - } else { - left = mid; - } - } - - return left; } -// 批量插值实现 -std::vector AbstractInterpolation::interpolate(const std::vector& x_values) const { - std::vector results; - results.reserve(x_values.size()); - - for (double x : x_values) { - results.push_back(interpolate(x)); +// 生成3D轨迹 +std::vector AbstractInterpolation::generate_trajectory_3d(size_t num_points) const { + if (num_points < 2) { + throw std::invalid_argument("轨迹点数量至少为2"); } - - return results; + + std::vector trajectory; + trajectory.reserve(num_points); + + for (size_t i = 0; i < num_points; ++i) { + double t = static_cast(i) / (num_points - 1); + trajectory.push_back(interpolate_3d(t)); + } + + return trajectory; } + +// 生成2D轨迹 +std::vector AbstractInterpolation::generate_trajectory_2d(size_t num_points) const { + if (num_points < 2) { + throw std::invalid_argument("轨迹点数量至少为2"); + } + + std::vector trajectory; + trajectory.reserve(num_points); + + for (size_t i = 0; i < num_points; ++i) { + double t = static_cast(i) / (num_points - 1); + trajectory.push_back(interpolate_2d(t)); + } + + return trajectory; +} + +// 线性插值辅助函数 +double AbstractInterpolation::linear_interpolate(double a, double b, double t) { + t = clamp_t(t); + return a + t * (b - a); +} + +// 四元数球面线性插值 +Quat AbstractInterpolation::slerp(const Quat& q1, const Quat& q2, double t) { + t = clamp_t(t); + + Quat result; + double dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z; + + // 确保我们选择最短路径 + if (dot < 0.0) { + Quat q2_neg = {-q2.w, -q2.x, -q2.y, -q2.z}; + return slerp(q1, q2_neg, t); + } + + // 防止数值问题，接近时使用线性插值 + if (dot > 0.9995) { + result.w = linear_interpolate(q1.w, q2.w, t); + result.x = linear_interpolate(q1.x, q2.x, t); + result.y = linear_interpolate(q1.y, q2.y, t); + result.z = linear_interpolate(q1.z, q2.z, t); + return result; + } + + // 计算角度和插值系数 + double theta_0 = std::acos(dot); + double theta = theta_0 * t; + double sin_theta = std::sin(theta); + double sin_theta_0 = std::sin(theta_0); + + double s1 = std::cos(theta) - dot * sin_theta / sin_theta_0; + double s2 = sin_theta / sin_theta_0; + + result.w = s1 * q1.w + s2 * q2.w; + result.x = s1 * q1.x + s2 * q2.x; + result.y = s1 * q1.y + s2 * q2.y; + result.z = s1 * q1.z + s2 * q2.z; + + return result; +} + +// 参数范围约束 +double AbstractInterpolation::clamp_t(double t) { + return std::max(0.0, std::min(1.0, t)); +} + +} // namespace cmvr::math \ No newline at end of file