非平坦地形下运动规划相关理论

1.SVD平面拟合方法

空间中的离散点得到拟合平面，其实就是一个最优化的过程。即求这些点到某个平面距离和最小的问题。我们知道一个先验消息，那就是该平面一定会过众散点的平均值。接着我们需要做的工作就是求这个平面的法向量。
根据协方差矩阵的SVD变换，最小奇异值对应的奇异向量就是平面的方向。
C++代码如下：

#include <iostream>
#include <highgui.h>
 
using namespace std;
using namespace cv;
 
//AX+BY+CZ+D=0
void cvFitPlane(const CvMat* points, float* plane){
	// Estimate geometric centroid.
	int nrows = points->rows;
	int ncols = points->cols;
	int type = points->type;
	CvMat* centroid = cvCreateMat(1, ncols, type);
	cvSet(centroid, cvScalar(0));
	for (int c = 0; c < ncols; c++){
		for (int r = 0; r < nrows; r++)
		{
			centroid->data.fl[c] += points->data.fl[ncols*r + c];
		}
		centroid->data.fl[c] /= nrows;
	}
	// Subtract geometric centroid from each point.  points2存放的是各个点减去几何重心的值
	CvMat* points2 = cvCreateMat(nrows, ncols, type);
	for (int r = 0; r < nrows; r++)
		for (int c = 0; c < ncols; c++)
			points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
	// Evaluate SVD of covariance matrix.
	CvMat* A = cvCreateMat(ncols, ncols, type);
	CvMat* W = cvCreateMat(ncols, ncols, type);
	CvMat* V = cvCreateMat(ncols, ncols, type);
	cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
	cvSVD(A, W, NULL, V, CV_SVD_V_T);
	Mat A1(A);
	cout << "A: " << A1<<endl;
	Mat V1(V);
	cout << "V: "<<V1<<endl;
	// Assign plane coefficients by singular vector corresponding to smallest singular value.
	plane[ncols] = 0;
	for (int c = 0; c < ncols; c++){
		plane[c] = V->data.fl[ncols*(ncols - 1) + c];
		plane[ncols] += plane[c] * centroid->data.fl[c];
	}
	plane[ncols] = -plane[ncols];
	// Release allocated resources.
	//cvReleaseMat(?roid);
	cvReleaseMat(&points2);
	cvReleaseMat(&A);
	cvReleaseMat(&W);
	cvReleaseMat(&V);
}
 
int main()
{
	//float x0 = 1,L1 = 2,y0 = 1,	L2 = 2, x1,y1,z1;
	//vector<float> x, y, z;
	//for (int i = 0; i < 20;++i)
	//{
	//	x1 = x0 + rand()*L1;
	//	y1 = y0 + rand()*L2;
	//	z1 = 1 + 2 * x1 + 3 * y1;
	//	x.push_back(x1);
	//	y.push_back(y1);
	//	z.push_back(z1);
	//}
	Mat point3D = (Mat_<float>(20, 3) << 2.62944737278636, 2.31148139831317, 13.1933389405122, 2.81158387415124, 1.07142335714838, 9.83743781974761,
		1.25397363258701, 2.69825861173755, 11.6027231003867, 2.82675171227804, 2.86798649551510, 15.2574629111014,
		2.26471849245082, 2.35747030971555, 12.6018479140483, 1.19508080999882, 2.51548026115667, 10.9366024034676,
		1.55699643773410, 2.48626493624983, 11.5727876842177, 2.09376303840997, 1.78445403906834, 10.5408881940249,
		2.91501367086860, 2.31095578035511, 13.7628946828025, 2.92977707039855, 1.34237337562312, 10.8866742676665,
		1.31522616335510, 2.41209217603922, 10.8667288548278, 2.94118556352123, 1.06366569275484, 10.0733682053070,
		2.91433389648589, 1.55384596992178, 11.4902057027371, 1.97075129744568, 1.09234278126231, 8.21853093867829,
		2.60056093777760, 1.19426356247170, 9.78391256297029, 1.28377267725443, 2.64691565665459, 11.5082923244726,
		1.84352256525255, 2.38965724595163, 11.8560168683600, 2.83147105037813, 1.63419896012172, 11.5655389811214,
		2.58441465911911, 2.90044409767671, 14.8701616112683, 2.91898485278581, 1.06889216100582, 10.0446461885891);
	vector<Point3f> points;
	points = Mat_<Point3f>(point3D);
	CvMat* points_mat = cvCreateMat(points.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵 
	for (int i = 0; i < points.size(); ++i)
	{
		points_mat->data.fl[i * 3 + 0] = points[i].x;//矩阵的值进行初始化   X的坐标值
		points_mat->data.fl[i * 3 + 1] = points[i].y;//  Y的坐标值
		points_mat->data.fl[i * 3 + 2] = points[i].z;// < span style = "font-family: Arial, Helvetica, sans-serif;" >//  Z的坐标值</span>
 
	}
	float plane12[4] = { 0 };//定义用来储存平面参数的数组 
	cvFitPlane(points_mat, plane12);//调用方程 
}

Matlab代码如下：

% 随机生成一组（x,y,z),这些点的坐标离一个空间平面比较近
x0=1;
L1=2;
y0=1;
L2=2;
x=x0+rand(20,1)*L1;
y=y0+rand(20,1)*L2;
z=1+2*x+3*y;
scatter3(x,y,z,'filled')
hold on;
 
planeData=[x,y,z];
 
% 协方差矩阵的SVD变换中，最小奇异值对应的奇异向量就是平面的方向
xyz0=mean(planeData,1);
centeredPlane=bsxfun(@minus,planeData,xyz0);
A=centeredPlane'*planeData;
[U,S,V]=svd(A);
%[U,S,V]=svd(centeredPlane);
a=V(1,3);
b=V(2,3);
c=V(3,3);
d=-dot([a b c],xyz0);
% 图形绘制
xfit = min(x):0.1:max(x);
yfit = min(y):0.1:max(y);
[XFIT,YFIT]= meshgrid (xfit,yfit);
ZFIT = -(d + a * XFIT + b * YFIT)/c;
mesh(XFIT,YFIT,ZFIT);

2.最小二乘平面拟合法

对空间中的一系列散点，寻求一个近似平面，与线性最小二乘一样，只是变换了拟合方程：

使用平面的一般方程：

Ax + By + CZ + D = 0

可以令平面方程为：

由最小二乘法知：

分别取 a0,a1,a2的偏导数：

即是：

换算为矩阵形式有：

可以直接通过克拉默法则求出a0,a1,a2的行列式表达式，有：

C++代码如下：

bool gFittingPlane(double *x, double *y, double *z, int n, double &a, double &b, double &c)
{
    int i;
    double x1, x2, y1, y2, z1, xz, yz, xy, r;
 
    x1 = x2 = y1 = y2 = z1 = xz = yz = xy = 0;
    for(i=0; i<n; i++)
    {
        x1 += x[i];
        x2 += x[i]*x[i];
        xz += x[i]*z[i];
 
        y1 += y[i];
        y2 += y[i]*y[i];
        yz += y[i]*z[i];
 
        z1 += z[i];
        xy += x[i]*y[i];
    }
    // gDaterm3()为自定义的三阶行列式计算函数
    r = gDeterm3(x2, xy, x1, xy, y2, y1, x1, y1, n);
    if(IS_ZERO(r)) return false;
 
    a = gDeterm3(xz, xy, x1, yz, y2, y1, z1, y1, n) / r;
    b = gDeterm3(x2, xz, x1, xy, yz, y1, x1, z1, n) / r;
    c = gDeterm3(x2, xy, xz, xy, y2, yz, x1, y1, z1) / r;
 
    return true;
}