我这个MatLab的程序：
%% 八索并联机器人标定核心算法（精简版）
clear;
clc;
%% 1. 数据加载
TAB0 = readmatrix('laser_data_3.csv');
TAB = TAB0;
posedata = TAB(1:26, [7:11]);
newCol = zeros(size(posedata, 1), 1);
posedata = [posedata, newCol];
motordata_nom = TAB(1:26, 12:19);
fprintf('\n验证posedata前3行:\n');
for i = 1:min(3, size(posedata, 1))
fprintf('Row %d: ', i);
for j = 1:size(posedata, 2)
fprintf('%.6f\t', posedata(i, j));
end
fprintf('\n');
end
fprintf('\n验证motordata_nom前3行:\n');
for i = 1:min(3, size(motordata_nom, 1))
fprintf('Row %d: ', i);
for j = 1:size(motordata_nom, 2)
fprintf('%.6f\t', motordata_nom(i, j));
end
fprintf('\n');
end
%% 2. 定义标称几何参数
B_nominal = [ ...
4705.96,4697.57,-4686.97,-4672.74,-4714.28,-4714.42,4688.24,4719.30; ...
-2429.04,-1796.00,-1771.11,-2423.78,2377.74,1732.08,1744.13,2382.44; ...
4127.29,4134.17,4126.85,4140.13,4157.88,4186.84,4161.26,4159.97 ...
];
a_nominal = [ ...
-265.94,279.48,-296.17,264.53,259.34,-265.49,274.50,-290.56; ...
-270.67,302.23,282.10,-237.42,287.04,-258.18,-279.60,292.92; ...
747.22,68.81,723.23,59.55,726.12,38.32,716.88,45.71 ...
];
rp = 98 * ones(1, 8);
Bk = [ 0, 0, 1; 0, 0, 1; 0, 0, 1; 0, 0, 1; 0, 0, 1; 0, 0, 1; 0, 0, 1; 0, 0, 1; ]';
l0_nominal = ones(8, 1);
%% 3. 执行一体化辨识
dpara0 = [zeros(48, 1); ones(8,1)];
% 定义参数边界
pos_err_bound = 200;
l0_err_bound = 0.1;
lb = [repmat([-pos_err_bound], 24*2, 1); repmat([1.0-l0_err_bound], 8, 1)];
ub = [repmat([ pos_err_bound], 24*2, 1); repmat([1.0+l0_err_bound], 8, 1)];
typical_x = ones(56, 1);
options_lsq = optimoptions(@lsqnonlin, 'Algorithm', 'levenberg-marquardt', ...
'Display', 'iter-detailed', ...
'MaxIterations', 300, ...
'StepTolerance', 1e-8, ...
'FunctionTolerance', 1e-8, ...
'TypicalX', typical_x);
[dpara_final, ~, ~, ~, output] = lsqnonlin(...
@(x) fun_objective_cable_error(x, a_nominal, B_nominal, l0_nominal, Bk, rp, motordata_nom, posedata), ...
dpara0, lb, ub, options_lsq);
%% 4. 计算并输出最终参数
final_B_error = reshape(dpara_final(1:24), 3, 8);
final_a_error = reshape(dpara_final(25:48), 3, 8);
final_l0_error = dpara_final(49:56);
% 计算最终参数
a_final = a_nominal + final_a_error;
B_final = B_nominal + final_B_error;
l0_final = diag(final_l0_error) * l0_nominal;
% 格式化输出
fprintf('\n=== 最终标定结果 ===\n');
fprintf('\n平台锚点坐标 a_final (mm):\n'); disp(a_final);
fprintf('\n基座锚点坐标 B_final (mm):\n'); disp(B_final);
fprintf('\n缆绳长度比例系数 l0_final:\n'); disp(l0_final);
%% 核心函数定义
function cable_error_vec = fun_objective_cable_error(dpara, a_nominal, B_nominal, l0_nominal, Bk, rp, motordata_nom, posedata)
B_current = B_nominal + reshape(dpara(1:24), 3, 8);
a_current = a_nominal + reshape(dpara(25:48), 3, 8);
l0_current = diag(dpara(49:56)) * l0_nominal;
datanum = size(posedata, 1);
total_cable_errors = zeros(datanum, 8);
for k = 1:datanum
P = posedata(k, 1:3)';
rz = posedata(k, 4)*pi/180; ry = posedata(k, 5)*pi/180; rx = posedata(k, 6)*pi/180;
Rz = [cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1];
Ry = [cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)];
Rx = [1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)];
R = Rz * Ry * Rx;
A_current = R * a_current + P;
L_calculated = zeros(8, 1);
for i = 1:8
L_calculated(i) = IK_P(A_current(:,i), B_current(:,i), Bk(:,i), rp(i));
end
error_k = diag(l0_current) * L_calculated - motordata_nom(k, :)';
total_cable_errors(k, :) = error_k';
end
cable_error_vec = total_cable_errors(:);
end
function L = IK_P(A, B, Bk, rp)
k = Bk; B_mid = B;
BA_mid_A = A - B_mid;
B_mid_A_k = BA_mid_A' * k * k;
B_mid_M_dir = BA_mid_A - B_mid_A_k;
if norm(B_mid_M_dir) < 1e-9, L=NaN; return; end
B_mid_M = rp * B_mid_M_dir / norm(B_mid_M_dir);
M = B_mid + B_mid_M;
MA = A - M;
norm_MA_sq = dot(MA, MA);
if norm_MA_sq < rp^2, length_st = 0;
else, length_st = sqrt(norm_MA_sq - rp^2); end
norm_MA = sqrt(norm_MA_sq);
if norm_MA < 1e-9, L=NaN; return; end
asin_arg = min(max(rp / norm_MA, -1), 1);
alpha = pi/2 + asin(asin_arg);
norm_B_mid_M = norm(B_mid_M);
if norm_B_mid_M < 1e-9, L=NaN; return; end
beta_cos_val = min(max(dot(B_mid_M, MA) / (norm_B_mid_M * norm_MA), -1), 1);
beta_sign = -sign(dot(BA_mid_A, k));
if beta_sign == 0, beta_sign = 1; end
beta = beta_sign * acos(beta_cos_val);
theta = alpha + beta;
length_circle = theta * rp;
L = length_st + length_circle;
end
转化成C#版本：
using System;
using System.Collections.Generic;
using System.Globalization;
using System.Linq;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.Optimization;
using System.IO;
using CsvHelper;
using CsvHelper.Configuration;
namespace ParallelRobotCalibration
{
class Program
{
static void Main(string[] args)
{
string baseDir = AppDomain.CurrentDomain.BaseDirectory;
// 拼接出 data 文件夹下的 csv 文件完整路径
string filePath = Path.Combine(baseDir, "data", "laser_data_3.csv");
double[,] csvData = CsvDataReader.ReadCsv(filePath);
if (csvData.GetLength(0) > 0)
{
var (posedata, motordata_nom) = CsvDataReader.ExtractData(csvData);
Console.WriteLine($"成功提取posedata: {posedata.GetLength(0)}行, {posedata.GetLength(1)}列");
Console.WriteLine($"成功提取motordata_nom: {motordata_nom.GetLength(0)}行, {motordata_nom.GetLength(1)}列");
// 示例：打印前几行数据
CsvDataReader.PrintDataSample("posedata", posedata);
CsvDataReader.PrintDataSample("motordata_nom", motordata_nom);
var result = CableRobotCalibrator.Calibrate(posedata, motordata_nom);
Console.WriteLine("平台锚点坐标 a_final (mm):");
Console.WriteLine(result.A_final.ToMatrixString());
Console.WriteLine("\n基座锚点坐标 B_final (mm):");
Console.WriteLine(result.B_final.ToMatrixString());
Console.WriteLine("\n缆绳长度比例系数 l0_final:");
Console.WriteLine(result.l0_final.ToVectorString());
Console.ReadKey();
}
else
{
Console.WriteLine("CSV文件读取失败或为空");
}
}
public class CsvDataReader
{
// 数据结构定义
public class CsvRow
{
// 定义CSV中所有列的属性，这里使用动态索引访问
public List<double> Values { get; set; } = new List<double>();
}
public class CsvRowMap : ClassMap<CsvRow>
{
public CsvRowMap()
{
// 自动映射所有列
AutoMap(CultureInfo.InvariantCulture);
}
}
public static void PrintDataSample(string name, double[,] data)
{
Console.WriteLine($"\n{name}数据示例 (前3行):");
int rows = Math.Min(3, data.GetLength(0));
int cols = data.GetLength(1);
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols; j++)
{
Console.Write($"{data[i, j],8:F6}");
}
Console.WriteLine();
}
}
public static double[,] ReadCsv(string filePath, char delimiter = ',')
{
try
{
if (!File.Exists(filePath))
{
Console.WriteLine($"错误: 文件 '{filePath}' 不存在");
return new double[0, 0];
}
var lines = File.ReadAllLines(filePath);
if (lines.Length == 0)
{
Console.WriteLine("警告: CSV文件为空");
return new double[0, 0];
}
// 假设所有行的列数相同（第一行决定列数）
var firstLine = lines[0].Split(delimiter);
int cols = firstLine.Length;
int rows = lines.Length;
double[,] result = new double[rows, cols];
for (int i = 0; i < rows; i++)
{
var parts = lines[i].Split(delimiter);
// 检查列数是否一致
if (parts.Length != cols)
{
// Console.WriteLine($"警告: 第 {i + 1} 行的列数({parts.Length})与第一行({cols})不一致");
// 只处理有效列
int validCols = Math.Min(cols, parts.Length);
for (int j = 0; j < validCols; j++)
{
if (double.TryParse(parts[j], NumberStyles.Any, CultureInfo.InvariantCulture, out double value))
{
result[i, j] = value;
}
else
{
// Console.WriteLine($"警告: 第 {i + 1} 行, 第 {j + 1} 列的 '{parts[j]}' 无法转换为double，使用默认值0");
result[i, j] = 0;
}
}
// 填充缺失的列
for (int j = validCols; j < cols; j++)
{
result[i, j] = 0;
}
}
else
{
// 列数一致的情况
for (int j = 0; j < cols; j++)
{
if (double.TryParse(parts[j], NumberStyles.Any, CultureInfo.InvariantCulture, out double value))
{
result[i, j] = value;
}
else
{
// Console.WriteLine($"警告: 第 {i + 1} 行, 第 {j + 1} 列的 '{parts[j]}' 无法转换为double，使用默认值0");
result[i, j] = 0;
}
}
}
}
return result;
}
catch (Exception ex)
{
Console.WriteLine($"读取CSV文件时发生错误: {ex.Message}");
Console.WriteLine(ex.StackTrace);
return new double[0, 0];
}
}
// 提取posedata和motordata_nom
public static (double[,] posedata, double[,] motordata_nom) ExtractData(double[,] csvData)
{
int totalRows = csvData.GetLength(0);
if (totalRows == 0) return (new double[0, 0], new double[0, 0]);
// 关键修正：跳过表头行（假设第0行是表头，数据从第1行开始）
int headerRows = 1; // 表头行数，根据实际CSV调整
int maxRows = Math.Min(26, totalRows - headerRows);
// 提取posedata (数据行1-26，对应CSV的headerRows~headerRows+25行)
double[,] posedata = new double[maxRows, 6];
for (int i = 0; i < maxRows; i++)
{
int csvRow = i + headerRows; // CSV行索引：跳过表头
for (int j = 0; j < 5; j++)
{
// 列7-11（MATLAB 1-based）→ CSV列6-10（0-based）
posedata[i, j] = csvData[csvRow, j + 6];
}
posedata[i, 5] = 0; // 新增零列
}
// 提取motordata_nom (数据行1-26，对应CSV的headerRows~headerRows+25行)
double[,] motordata_nom = new double[maxRows, 8];
for (int i = 0; i < maxRows; i++)
{
int csvRow = i + headerRows; // 跳过表头
for (int j = 0; j < 8; j++)
{
// 列12-19（MATLAB 1-based）→ CSV列11-18（0-based）
motordata_nom[i, j] = csvData[csvRow, j + 11];
}
}
return (posedata, motordata_nom);
}
}
public static class CableRobotCalibrator
{
public class CalibrationResult
{
public Matrix<double> A_final { get; set; } // 3x8
public Matrix<double> B_final { get; set; } // 3x8
public Vector<double> l0_final { get; set; } // 8x1
}
public static CalibrationResult Calibrate(double[,] posedata, double[,] motordata_nom)
{
// 1. 定义标称参数（与之前一致）
var B_nominal = Matrix<double>.Build.DenseOfArray(new double[,] {
{ 4705.96, 4697.57, -4686.97, -4672.74, -4714.28, -4714.42, 4688.24, 4719.30 },
{ -2429.04, -1796.00, -1771.11, -2423.78, 2377.74, 1732.08, 1744.13, 2382.44 },
{ 4127.29, 4134.17, 4126.85, 4140.13, 4157.88, 4186.84, 4161.26, 4159.97 }
});
var a_nominal = Matrix<double>.Build.DenseOfArray(new double[,] {
{ -265.94, 279.48, -296.17, 264.53, 259.34, -265.49, 274.50, -290.56 },
{ -270.67, 302.23, 282.10, -237.42, 287.04, -258.18, -279.60, 292.92 },
{ 747.22, 68.81, 723.23, 59.55, 726.12, 38.32, 716.88, 45.71 }
});
var rp = Vector<double>.Build.Dense(8, 98.0);
var Bk = Matrix<double>.Build.DenseOfArray(new double[,] {
{0,0,1}, {0,0,1}, {0,0,1}, {0,0,1},
{0,0,1}, {0,0,1}, {0,0,1}, {0,0,1}
}).Transpose();
var l0_nominal = Vector<double>.Build.Dense(8, 1.0);
// 2. 初始化参数和边界（与之前一致）
var dpara0 = Vector<double>.Build.Dense(56);
for (int i = 48; i < 56; i++) dpara0[i] = 1.0;
double pos_err_bound = 200;
double l0_err_bound = 0.1;
var lb = Vector<double>.Build.Dense(56, -pos_err_bound);
var ub = Vector<double>.Build.Dense(56, pos_err_bound);
for (int i = 48; i < 56; i++)
{
lb[i] = 1.0 - l0_err_bound;
ub[i] = 1.0 + l0_err_bound;
}
// 3. 计算误差向量长度（26组数据×8根缆绳=208）
int datanum = posedata.GetLength(0);
int errorLength = datanum * 8;
// 4. 定义模型函数：参数→误差向量（核心适配）
// 签名：(参数, 输入x) → 误差向量（x为占位符，无实际意义）
Func<Vector<double>, Vector<double>, Vector<double>> modelFunc = (parameters, x) =>
CableError(parameters, a_nominal, B_nominal, l0_nominal, Bk, rp, motordata_nom, posedata);
// 5. 构造观测向量（x和y长度必须相同，y设为0向量，x为占位符）
var observedX = Vector<double>.Build.Dense(errorLength, 0.0); // 占位符，长度=误差向量长度
var observedY = Vector<double>.Build.Dense(errorLength, 0.0); // 目标：误差→0
// 6. 创建目标模型（关键修正：使用 NonlinearModel 构建 IObjectiveModel）
var objective = ObjectiveFunction.NonlinearModel(
modelFunc, // 误差计算函数
observedX, // 输入x（占位符）
observedY, // 目标y=0
null, // 权重（无）
2 // 数值微分精度
);
// 7. 配置优化器（Levenberg-Marquardt）
var optimizer = new LevenbergMarquardtMinimizer(
gradientTolerance: 1e-8,
stepTolerance: 1e-8,
functionTolerance: 1e-8,
maximumIterations: 300
);
// 8. 执行优化（现在 objective 是 IObjectiveModel，匹配方法参数）
var result = optimizer.FindMinimum(objective, dpara0, lb, ub);
// 9. 解析结果（与之前一致）
var dpara_final = result.MinimizingPoint;
var final_B_error = Matrix<double>.Build.Dense(3, 8, dpara_final.SubVector(0, 24).ToArray());
var final_a_error = Matrix<double>.Build.Dense(3, 8, dpara_final.SubVector(24, 24).ToArray());
var final_l0_error = dpara_final.SubVector(48, 8);
var a_final = a_nominal + final_a_error;
var B_final = B_nominal + final_B_error;
var l0_final = final_l0_error.PointwiseMultiply(l0_nominal);
return new CalibrationResult
{
A_final = a_final,
B_final = B_final,
l0_final = l0_final
};
}
// 误差向量计算（与MATLAB的fun_objective_cable_error一致）
private static Vector<double> CableError(
Vector<double> dpara,
Matrix<double> a_nominal,
Matrix<double> B_nominal,
Vector<double> l0_nominal,
Matrix<double> Bk,
Vector<double> rp,
double[,] motordata_nom,
double[,] posedata)
{
var B_current = B_nominal + Matrix<double>.Build.Dense(3, 8, dpara.SubVector(0, 24).ToArray());
var a_current = a_nominal + Matrix<double>.Build.Dense(3, 8, dpara.SubVector(24, 24).ToArray());
var l0_current = dpara.SubVector(48, 8).PointwiseMultiply(l0_nominal);
int datanum = posedata.GetLength(0);
var errors = new double[datanum * 8];
for (int k = 0; k < datanum; k++)
{
// 位置和姿态
var P = Vector<double>.Build.Dense(new[] { posedata[k, 0], posedata[k, 1], posedata[k, 2] });
double rz = posedata[k, 3] * Math.PI / 180;
double ry = posedata[k, 4] * Math.PI / 180;
double rx = posedata[k, 5] * Math.PI / 180;
// 旋转矩阵
var Rz = Matrix<double>.Build.DenseOfArray(new double[,] {
{ Math.Cos(rz), -Math.Sin(rz), 0 },
{ Math.Sin(rz), Math.Cos(rz), 0 },
{ 0, 0, 1 }
});
var Ry = Matrix<double>.Build.DenseOfArray(new double[,] {
{ Math.Cos(ry), 0, Math.Sin(ry) },
{ 0, 1, 0 },
{ -Math.Sin(ry), 0, Math.Cos(ry) }
});
var Rx = Matrix<double>.Build.DenseOfArray(new double[,] {
{ 1, 0, 0 },
{ 0, Math.Cos(rx), -Math.Sin(rx) },
{ 0, Math.Sin(rx), Math.Cos(rx) }
});
var R = Rz * Ry * Rx;
// 平台锚点坐标
var A_current = R * a_current;
for (int j = 0; j < 8; j++)
A_current.SetColumn(j, A_current.Column(j) + P);
// 计算每根缆绳的误差
for (int i = 0; i < 8; i++)
{
double L_calc = IK_P(A_current.Column(i), B_current.Column(i), Bk.Column(i), rp[i]);
double L_obs = motordata_nom[k, i];
errors[k * 8 + i] = L_calc * l0_current[i] - L_obs; // 误差=计算值-观测值
}
}
return Vector<double>.Build.Dense(errors);
}
// 修正IK_P函数（向量运算修复）
private static double IK_P(
Vector<double> A,
Vector<double> B,
Vector<double> Bk,
double rp)
{
var k = Bk;
var B_mid = B;
var BA_mid_A = A - B_mid;
// 修正点积计算：(BA_mid_A · k) * k
double dot = BA_mid_A.DotProduct(k);
var B_mid_A_k = dot * k;
var B_mid_M_dir = BA_mid_A - B_mid_A_k;
if (B_mid_M_dir.L2Norm() < 1e-9) return double.NaN;
var B_mid_M = rp * B_mid_M_dir / B_mid_M_dir.L2Norm();
var M = B_mid + B_mid_M;
var MA = A - M;
double norm_MA_sq = MA.DotProduct(MA);
double length_st = norm_MA_sq < rp * rp ? 0 : Math.Sqrt(norm_MA_sq - rp * rp);
double norm_MA = Math.Sqrt(norm_MA_sq);
if (norm_MA < 1e-9) return double.NaN;
double asin_arg = Math.Clamp(rp / norm_MA, -1, 1);
double alpha = Math.PI / 2 + Math.Asin(asin_arg);
double norm_B_mid_M = B_mid_M.L2Norm();
if (norm_B_mid_M < 1e-9) return double.NaN;
double beta_cos_val = Math.Clamp(B_mid_M.DotProduct(MA) / (norm_B_mid_M * norm_MA), -1, 1);
double beta_sign = -Math.Sign(BA_mid_A.DotProduct(k));
if (beta_sign == 0) beta_sign = 1;
double beta = beta_sign * Math.Acos(beta_cos_val);
double theta = alpha + beta;
double length_circle = theta * rp;
return length_st + length_circle;
}
}
}
}
但是计算出来的数据有问题，请问为什么？
最新发布
本文探讨了在使用Objective-C开发播放器时遇到的问题：播放时间在视频播放结束后显示为NaN。详细解释了NaN的概念及其在单精度浮点数表示中的具体形式，并提供了检查是否为NaN的方法。通过实例分析，帮助开发者理解并解决此类计算错误。
扫一扫
70
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
