首先我们需要将AGV 坐标转三维空间坐标
这里我们使用AGVTest工具
AGVTest工具使用手册
说明：本工具主要功能是将AGV小车在虚拟场景中的坐标点数值与现实场景中的坐标点数值通过计算得出相关系数，为在Unity当中实现AGV小车位移动画功能做辅助参考。
引入几个主要参考代码
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace ParameterCalculations
{
class Matrix
{
int row, column; //矩阵的行列数
double [,] data; //矩阵的数据
//构造函数1
public Matrix(int rowNum,int columnNum)
{
row = rowNum;
column = columnNum;
data = new double[row, column];
}
public Matrix(double[,] members)
{
row = members.GetUpperBound(0) + 1;
column = members.GetUpperBound(1) + 1;
data = new double[row, column];
Array.Copy(members, data, row * column);
}
//矩阵乘法
public bool MatrixMultiply(double[,] a, double[,] b, ref double[,] c)
{
if (a.GetLength(1) != b.GetLength(0))
return false;
if (a.GetLength(0) != c.GetLength(0) || b.GetLength(1) != c.GetLength(1))
return false;
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < b.GetLength(1); j++)
{
c[i, j] = 0;
for (int k = 0; k < b.GetLength(0); k++)
{
c[i, j] += a[i, k] * b[k, j];
}
}
}
return true;
}
// 矩阵相加
public bool MatrixAdd(double[,] a, double[,] b, ref double[,] c)
{
if (a.GetLength(0) != b.GetLength(0) || a.GetLength(1) != b.GetLength(1)
|| a.GetLength(0) != c.GetLength(0) || a.GetLength(1) != c.GetLength(1))
return false;
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < a.GetLength(1); j++)
{
c[i, j] = a[i, j] + b[i, j];
}
}
return true;
}
//矩阵相减
public bool MatrixSubtration(double[,] a, double[,] b, ref double[,] c)
{
if (a.GetLength(0) != b.GetLength(0) || a.GetLength(1) != b.GetLength(1)
|| a.GetLength(0) != c.GetLength(0) || a.GetLength(1) != c.GetLength(1))
return false;
for (int i = 0; i < a.GetLength(0); i++)
{
for (int j = 0; j < a.GetLength(1); j++)
{
c[i, j] = a[i, j] - b[i, j];
}
}
return true;
}
矩阵的行列式的值
public double MatrixSurplus(double[,] a)
{
int i, j, k, p, r, m, n;
m = a.GetLength(0);
n = a.GetLength(1);
double X, temp = 1, temp1 = 1, s = 0, s1 = 0;
if (n == 2)
{
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
if ((i + j) % 2 > 0) temp1 *= a[i, j];
else temp *= a[i, j];
X = temp - temp1;
}
else
{
for (k = 0; k < n; k++)
{
for (i = 0, j = k; i < m && j < n; i++, j++)
temp *= a[i, j];
if (m - i > 0)
{
for (p = m - i, r = m - 1; p > 0; p--, r--)
temp *= a[r, p - 1];
}
s += temp;
temp = 1;
}
for (k = n - 1; k >= 0; k--)
{
for (i = 0, j = k; i < m && j >= 0; i++, j--)
temp1 *= a[i, j];
if (m - i > 0)
{
for (p = m - 1, r = i; r < m; p--, r++)
temp1 *= a[r, p];
}
s1 += temp1;
temp1 = 1;
}
X = s - s1;
}
return X;
}
// 矩阵转置
public bool MatrixInver(double[,] a, ref double[,] b)
{
if (a.GetLength(0) != b.GetLength(1) || a.GetLength(1) != b.GetLength(0))
return false;
for (int i = 0; i < a.GetLength(1); i++)
for (int j = 0; j < a.GetLength(0); j++)
b[i, j] = a[j, i];
return true;
}
//矩阵求逆
public bool MatrixOpp(double[,] a, ref double[,] b)
{
double X = MatrixSurplus(a);
if (X == 0) return false;
X = 1 / X;
double[,] B = new double[a.GetLength(0), a.GetLength(1)];
double[,] SP = new double[a.GetLength(0), a.GetLength(1)];
double[,] AB = new double[a.GetLength(0), a.GetLength(1)];
for (int i = 0; i < a.GetLength(0); i++)
for (int j = 0; j < a.GetLength(1); j++)
{
for (int m = 0; m < a.GetLength(0); m++)
for (int n = 0; n < a.GetLength(1); n++)
B[m, n] = a[m, n];
{
for (int x = 0; x < a.GetLength(1); x++)
B[i, x] = 0;
for (int y = 0; y < a.GetLength(0); y++)
B[y, j] = 0;
B[i, j] = 1;
SP[i, j] = MatrixSurplus(B);
AB[i, j] = X * SP[i, j];
}
}
MatrixInver(AB, ref b);
return true;
}
//矩阵求逆的重载，精度比较高
public double[,] MatrixOpp(double[,] Array)
{
int m = 0;
int n = 0;
m = Array.GetLength(0);
n = Array.GetLength(1);
double[,] array = new double[2 * m + 1, 2 * n + 1];
for (int k = 0; k < 2 * m + 1; k++) //初始化数组
{
for (int t = 0; t < 2 * n + 1; t++)
{
array[k, t] = 0.00000000;
}
}
for (int i = 0; i < m; i++)
{
for (int j = 0; j < n; j++)
{
array[i, j] = Array[i, j];
}
}
for (int k = 0; k < m; k++)
{
for (int t = n; t <= 2 * n; t++)
{
if ((t - k) == m)
{
array[k, t] = 1.0;
}
else
{
array[k, t] = 0;
}
}
}
//得到逆矩阵
for (int k = 0; k < m; k++)
{
if (array[k, k] != 1)
{
double bs = array[k, k];
array[k, k] = 1;
for (int p = k + 1; p < 2 * n; p++)
{
array[k, p] /= bs;
}
}
for (int q = 0; q < m; q++)
{
if (q != k)
{
double bs = array[q, k];
for (int p = 0; p < 2 * n; p++)
{
array[q, p] -= bs * array[k, p];
}
}
else
{
continue;
}
}
}
double[,] NI = new double[m, n];
for (int x = 0; x < m; x++)
{
for (int y = n; y < 2 * n; y++)
{
NI[x, y - n] = array[x, y];
}
}
return NI;
}
}
}转换参数工具类
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace ParameterCalculations
{
//求转换参数的类
public class PramSCals
{
public static Point2d GetPos(Point2d p1, double dx, double dy, double rota, double sacle)
{
double c = sacle * Math.Cos(rota);
double d = sacle * Math.Sin(rota);
Point2d t = new Point2d();
t.X = dx + (c * p1.X - d * p1.Y);
t.Y = dy + (c * p1.Y + d * p1.X);
return t;
}
//求坐标方位角
private static double FWJ(Point2d p1, Point2d p2)
{
Double dx, dy;
dx = p2.X - p1.X;
dy = p2.Y - p1.Y;
return Math.PI - Math.Sign(dy) - Math.Atan(dx / dy);
}
//两点之间的距离公式
private static double Dist(Point2d p1, Point2d p2)
{
double d;
d = Math.Sqrt(Math.Pow((p2.X - p1.X), 2) + Math.Pow((p2.Y - p1.Y), 2));
return d;
}
//
/// <summary>
/// 两点法求四参数
/// /// </summary>
/// <param name="p1旧坐标"></param>
/// <param name="p2新坐标"></param>
/// <param name="rota旋转参数"></param>
/// <param name="scale比例"></param>
/// <param name="dx x的平移"></param>
/// <param name="dy y的平移"></param>
public static void Canshu4(Point2d[] p1, Point2d[] p2, ref double rota, ref double scale, ref double dx, ref double dy)
{
rota = FWJ(p2[0], p2[1]) - FWJ(p1[0], p1[1]);
scale = Dist(p2[0], p2[1]) / Dist(p1[0], p1[1]);
dx = p2[0].X - scale * Math.Cos(rota) * p1[0].X + scale * Math.Sin(rota) * p1[0].Y;
dy = p2[0].Y - scale * Math.Sin(rota) * p1[0].X - scale * Math.Cos(rota) * p1[0].Y;
}
/// <summary>
///三个以上的点计算四参数
/// </summary>
/// <param name="p1旧坐标"></param>
/// <param name="p2新坐标"></param>
/// <param name="PointCount转换点的个数"></param>
/// <param name="rota旋转参数"></param>
/// <param name="scale比例"></param>
/// <param name="dx x的平移"></param>
/// <param name="dy y的平移"></param>
public static void Canshu4(Point2d[] p1, Point2d[] p2, int PointCount, ref double rota, ref double scale, ref double dx, ref double dy)
{
double u = 1.0, v = 0, Dx = 0.0, Dy = 0.0;
int intCount = PointCount;
//Matrix dx1 ;//误差方程改正数
Matrix B;//误差方程系数矩阵
// Matrix W ;//误差方程常数项
double[,] dx1 = new double[4, 1];
double[,] B1 = new double[2 * intCount, 4];
double[,] W1 = new double[2 * intCount, 1];
// Matrix BT, N, InvN, BTW;
double[,] BT = new double[4, 2 * intCount];
double[,] N = new double[4, 4];
double[,] InvN = new double[4, 4];
double[,] BTW = new double[4, 1];
for (int i = 0; i < intCount; i++)
{
//计算误差方程系数
B1[2 * i, 0] = 1;
B1[2 * i, 1] = 0;
B1[2 * i, 2] = p1[i].X;
B1[2 * i, 3] = -p1[i].Y;
B1[2 * i + 1, 0] = 0;
B1[2 * i + 1, 1] = 1;
B1[2 * i + 1, 2] = p1[i].Y;
B1[2 * i + 1, 3] = p1[i].X;
}
B = new Matrix(B1);
for (int i = 0; i < intCount; i++)
{
//计算误差方程系常数
W1[2 * i, 0] = p2[i].X - u * p1[i].X + v * p1[i].Y - Dx;
W1[2 * i + 1, 0] = p2[i].Y - u * p1[i].Y - v * p1[i].X - Dy;
}
//最小二乘求解
B.MatrixInver(B1, ref BT);//转置
B.MatrixMultiply(BT, B1, ref N);
InvN = B.MatrixOpp(N);
B.MatrixMultiply(BT, W1, ref BTW);
B.MatrixMultiply(InvN, BTW, ref dx1);
Dx = Dx + dx1[0, 0];
Dy = Dy + dx1[1, 0];
u = u + dx1[2, 0];
v = v + dx1[3, 0];
dx = Dx;
dy = Dy;
rota = Math.Atan(v / u);
scale = u / Math.Cos(rota);
dx = Math.Round(dx, 6);
dy = Math.Round(dy, 6);
rota = Math.Round(rota, 6);
scale = Math.Round(scale, 6);
}
/// <summary>
/// 求七参数
/// </summary>
/// <param name="p1 旧坐标"></param>
/// <param name="p2 新坐标"></param>
/// <param name="PointCount 坐标点数"></param>
/// <param name="rotax x的旋转量"></param>
/// <param name="rotay y的旋转量"></param>
/// <param name="rotaz z的旋转量"></param>
/// <param name="scale 比例"></param>
/// <param name="dx x的平移量"></param>
/// <param name="dy y的平移量"></param>
/// <param name="dz z的平移量"></param>
public static void Canshu7(Point3d[] p1, Point3d[] p2, int PointCount, ref double rotax, ref double rotay, ref double rotaz, ref double scale, ref double dx, ref double dy, ref double dz)
{
double[,] B1 = new double[PointCount * 3, 7];
Matrix B = new Matrix(B1);
double[,] dx1 = new double[7, 1];//V=B*X-L
double[,] L = new double[PointCount * 3, 1];
double[,] BT = new double[7, PointCount * 3];
double[,] N = new double[7, 7];
double[,] InvN = new double[7, 7];
double[,] BTL = new double[7, 1];
//初始化L矩阵
for (int i = 0; i < PointCount * 3; i++)
{
if (i % 3 == 0)
{
L[i, 0] = p2[i / 3].X;
}
else if (i % 3 == 1)
{
L[i, 0] = p2[i / 3].Y;
}
else if (i % 3 == 2)
{
L[i, 0] = p2[i / 3].Z;
}
}
//初始化B矩阵
for (int i = 0; i < PointCount * 3; i++)
{
if (i % 3 == 0)
{
B1[i, 0] = 1;
B1[i, 1] = 0;
B1[i, 2] = 0;
B1[i, 3] = p1[i / 3].X;
B1[i, 4] = 0;
B1[i, 5] = -p1[i / 3].Z;
B1[i, 6] = p1[i / 3].Y;
}
else if (i % 3 == 1)
{
B1[i, 0] = 0;
B1[i, 1] = 1;
B1[i, 2] = 0;
B1[i, 3] = p1[i / 3].Y;
B1[i, 4] = p1[i / 3].Z;
B1[i, 5] = 0;
B1[i, 6] = -p1[i / 3].X;
}
else if (i % 3 == 2)
{
B1[i, 0] = 0;
B1[i, 1] = 0;
B1[i, 2] = 1;
B1[i, 3] = p1[i / 3].Z;
B1[i, 4] = -p1[i / 3].Y;
B1[i, 5] = p1[i / 3].X;
B1[i, 6] = 0;
}
}
//转置
B.MatrixInver(B1, ref BT);
//法方程矩阵
//N=BT*B
B.MatrixMultiply(BT, B1, ref N);
//求逆
InvN = B.MatrixOpp(N);
//BTL=BT*L
B.MatrixMultiply(BT, L, ref BTL);
//dx1=invN*BTL;
B.MatrixMultiply(InvN, BTL, ref dx1);
//
dx = Math.Round(dx1[0, 0], 6);
dy = Math.Round(dx1[1, 0], 6);
dz = Math.Round(dx1[2, 0], 6);
scale = Math.Round(dx1[3, 0], 6);
rotax = Math.Round(dx1[4, 0] / dx1[3, 0], 6);
rotay = Math.Round(dx1[5, 0] / dx1[3, 0], 6);
rotaz = Math.Round(dx1[6, 0] / dx1[3, 0], 6);
}
}
}整不明白，就先用我写的测试工具用着吧。
https://download.csdn.net/download/qq_30065669/90355669?spm=1001.2014.3001.5503
- 坐标点数值收集
将AGV厂商提供的坐标点放置到Unity三维空间中，并将三维空间中点位的坐标值记录收集，同时记录收集厂商提供的坐标点数值，将两个坐标值一一对应收集好后备用。
注意：由于坐标系的不同，三维空间的坐标值取（x，z）的值，对照现实厂商提供的坐标点的（x，y）值。
- 启动并配置AGVTest.exe程序
打开AGVTest.exe应用程序，界面如图所示，分为三大区域：填写坐标值区、系数结果展示区、结果验证区。
- 填写坐标值
通过采样填写的方式，在填写坐标值区内将Excel文件中收集的坐标点值按照顺序编号输入进去。
表格中（X1，Y1）值，填写AGV厂商提供的真实坐标值（x,y），（X2，Y2）值，填写Unity三维空间坐标值（x,z）。
为了得到更加准确的结果，尽量多填写一些采样样本数据。理论上采样样本数据越多，计算误差越小，最终在计算公式时得到的效果更准确，趋近真实。如果转换参数误差偏差太大，建议重新采样样本数据，重新测试，直到获取最优解算参数。
- 求转换参数值
在填写坐标值区填写好坐标值后，点击下方求转换参数值按钮，在右侧系数结果展示区会显示根据当前样本值计算得出的转换参数值。
dx、dy值是两个坐标值进行转换时，套用的转换参数，rota、scale值是参考的缩放和角度参数，一般不进行计算。
- 参数结果测试
计算得出转换参数值后，在下方结果验证区左侧填写另外一个AGV真实坐标值，点击估算按钮，右侧会显示对应计算出来的Unity三维空间坐标值，与表格中进行对照，确认无误后，证明该转换参数为真实有效数据，可在Unity脚本中进行使用。
- 结果保存
验证通过后，点击保存参数按钮，将计算的得出的结果进行保存，存储文件格式为txt，以便后续操作进行使用。
保存成功后，会进行提示，点击确定按钮，结束操作。
在文件存储路径下打开存储的结果数据进行查看。
这里我将上面的算法封装成dll，方便在其他地方调用。
扫一扫
254
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
