Field D*
Filed D*算法是D_star Lite算法的一种改进版本，该算法针对基于栅格的路径规划算法通常以栅格端点或中心点作为路径的节点，限制了路径方向变化只能为π/4的倍数，会导致机器人不必要的运动转向，影响执行效率。而Dave Ferguson提出的Filed D*算法，通过对栅格进行线性插值使路径点不局限于端点，规划方向的变化不再受限于π/4的倍数，生成较为平滑的路径。
原文：
https://blog.csdn.net/lqzdreamer/article/details/85158542
Filed D*算法的关键是在给定相邻节点路径代价的前提下，提出一种计算每个网格节点路径代价的新方法。在基于网格的路径规划中，一个节点的代价为（从节点到目标的路径代价）：
其中，nbrs(s)是s的所有相邻节点的集合；c(s, s’)是遍历s和s’之间的边的代价，g(s’)是节点s’的路径代价。而在此之前都是假设从节点s到相邻节点的唯一可能的移动只能是直线轨迹（只能是从一格移动到另一格）。
而Filed D*考虑放宽这个假设，允许从节点s到其网格单元边界上的任何一点的直线轨迹（如下图所示）。如果知道每一个点 s b s_b sb在边界的值,那么仅仅可以通过最小化 c ( s , s b ) + g ( s b ) c(s, s_b)+ g(s_b) c(s,sb)+g(sb)计算节点s的最优值， c ( s , s b ) c(s, s_b) c(s,sb)通过s和sb之间的距离乘以到达s所在单元的代价。但是，这样的点有无数个，因此不可能为每个点计算 g ( s b ) g(s_b) g(sb)。
但是，用线性插值法对每个边界点提供g(sb)的近似是可能的。首先将上图中的节点视为连续代价的样本点，由节点s开始的一条优化路径必须通过连接两个连续相邻的边缘,例如
。因此，设置通过这些边的路径的最小代价为s的路径代价，而这些边被认为是一次一条。为了通过
计算节点s的路径，需要使用节点 
的路径代价以及图5（a）中所示中间网格的转移代价c和底部网格的转移代价b。
假设位于 s 1和 s 2之间边界上的任意点sy的路径代价是 g ( s 1 ) 和 g ( s 2 ) 的线性组合（如图6）:
其中，其中y是 s 1 到 s y的距离(假设单位单元格)。这个假设并不完美， s y 的路径代价可能不是 g ( s 1 ) 和 g ( s 2 )的线性组合，甚至也不是这些路径代价的函数。然而，这种线性近似在实际应用中效果良好，可以构造节点s路径代价的封闭形式解。根据这个近似，s在 s 1、 s 2 、网格代价c和b下的路径代价可以计算为
这个公式可以通过上图7中的（a）图理解。公式中的第1项为s→ s x 的代价，第2项为 s x → s y 的代价，第3、4项为y处的代价。其中， x ∈ [ 0 , 1 ] ， y ∈ [ 0 , 1 ] 。当x=y=1时，路径为底部的路径，但是代价却是中间网格的代价。
C++代码：
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <limits>
using namespace std;
const double INFINITY_COST = numeric_limits<double>::infinity();
struct Node {
int x, y;
double g, rhs;
};
struct CompareCost {
bool operator()(const Node& a, const Node& b) {
return (a.g + a.rhs) > (b.g + b.rhs);
}
};
vector<vector<double>> map; // 代表地图的二维数组
vector<vector<Node>> nodes;
priority_queue<Node, vector<Node>, CompareCost> openList;
void init(int rows, int cols) {
map.resize(rows, vector<double>(cols, 1));
nodes.resize(rows, vector<Node>(cols, {0, 0, INFINITY_COST, INFINITY_COST}));
}
double heuristic(int x1, int y1, int x2, int y2) {
return abs(x2 - x1) + abs(y2 - y1);
}
void updateVertex(Node& u, int goalX, int goalY) {
if (u.x != goalX || u.y != goalY) {
u.rhs = INFINITY_COST;
for (int i = -1; i <= 1; i++) {
for (int j = -1; j <= 1; j++) {
if (i == 0 && j == 0) continue;
if (u.x + i >= 0 && u.x + i < map.size() && u.y + j >= 0 && u.y + j < map[0].size()) {
Node v = nodes[u.x + i][u.y + j];
u.rhs = min(u.rhs, v.g + heuristic(u.x, u.y, v.x, v.y));
}
}
}
}
if (find(openList.begin(), openList.end(), u) != openList.end()) {
openList.pop();
}
u.g = u.rhs;
openList.push(u);
}
void fieldDStar(int startX, int startY, int goalX, int goalY) {
Node goal = {goalX, goalY, 0, 0};
nodes[goalX][goalY] = goal;
openList.push(goal);
while (!openList.empty()) {
Node u = openList.top();
openList.pop();
if (u.x == startX && u.y == startY) {
cout << "Start reached!" << endl;
break;
}
// 更新节点代价
updateVertex(u, startX, startY);
}
}
int main() {
init(5, 5); // 初始化地图和节点数组
fieldDStar(0, 0, 4, 4); // 从(0,0)到(4,4)的路径规划
return 0;
}Morphin局部避障规划
三次样条路径规划
三次样条路径规划是一种有效的路径规划方法，广泛应用于机器人轨迹规划、自动驾驶运动规划等领域。
三次样条路径规划基于三次样条插值方法，通过一系列的控制点生成一条连续平滑的轨迹。这种方法的核心在于使用三次多项式来逼近给定的数据点，从而得到一条光滑曲线。三次样条插值方法的关键在于解决一组方程，这些方程确保了曲线在所有数据点处的连续性，并且在这些点之间达到一定的光滑度。
在三次样条路径规划中，每个分段使用三次多项式来表示，这些多项式在连接点处满足一定的条件，如速度和加速度的连续性。通过这种方式，可以生成一条既通过所有给定数据点又保持平滑的曲线。这种方法特别适用于需要生成平滑轨迹的应用场景，如机器人运动规划、自动驾驶等。
三次样条插值的基本原理：
-
数据点集合：三次样条插值通常基于一组已知的数据点，例如起始点和目标点，以及可能的中间参考点。
-
插值函数：通过这些数据点，三次样条插值会生成一个连续的三次多项式函数，这个函数在每两个相邻数据点之间是三次可微的。
-
平滑性：三次样条插值保证生成的曲线在数据点处平滑连接，并且一阶导数和二阶导数连续。
-
边界条件：通常需要为插值函数的边界指定条件，例如固定端点的值或者固定端点的一阶导数值，以确保生成的曲线在边界处有合适的行为。
-
参数化曲线：通过三次样条插值生成的曲线可以进行参数化，使得机器人可以沿着这条曲线移动，并且可以在需要时对参数进行调整以改变路径的形状。
C++代码：这个示例代码使用了 Eigen 库来进行矩阵运算，Eigen 是一个用于线性代数运算的 C++ 模板库，提供了方便的矩阵和向量操作。
#include <iostream>
#include <Eigen/Dense>
#include <vector>
using namespace Eigen;
using namespace std;
// 三次样条插值计算函数
MatrixXd cubicSplineInterpolation(const vector<double>& x, const vector<double>& y) {
int n = x.size();
// 构建系数矩阵和右侧向量
MatrixXd A = MatrixXd::Zero(n, n);
VectorXd b(n);
VectorXd h = VectorXd::Zero(n - 1);
for (int i = 0; i < n - 1; i++) {
h(i) = x[i + 1] - x[i];
}
for (int i = 1; i < n - 1; i++) {
A(i, i - 1) = h(i - 1);
A(i, i) = 2 * (h(i - 1) + h(i));
A(i, i + 1) = h(i);
b(i) = 3 * ((y[i + 1] - y[i]) / h(i) - (y[i] - y[i - 1]) / h(i - 1));
}
// 设置边界条件
A(0, 0) = 1;
A(n - 1, n - 1) = 1;
// 解线性方程组
VectorXd c = A.colPivHouseholderQr().solve(b);
// 计算系数矩阵
MatrixXd coefMatrix(n - 1, 4);
for (int i = 0; i < n - 1; i++) {
double a = y[i];
double b = (y[i + 1] - y[i]) / h(i) - h(i) * (2 * c(i) + c(i + 1)) / 3;
double d = (c(i + 1) - c(i)) / (3 * h(i));
coefMatrix.row(i) << a, b, c(i), d;
}
return coefMatrix;
}
int main() {
vector<double> x = {0, 1, 2, 3, 4}; // 数据点 x 坐标
vector<double> y = {0, 1, 0.5, 1.5, 1}; // 数据点 y 坐标
MatrixXd coefMatrix = cubicSplineInterpolation(x, y);
cout << "Coefficients for each segment:" << endl;
cout << coefMatrix << endl;
return 0;
}请确保已经安装了 Eigen 库，并将其正确包含在项目中。这段代码演示了如何通过三次样条插值计算系数矩阵，以实现平滑的曲线插值。