多相机视觉流程优化及坐标转换增强

优化多相机视觉任务流程，增加独立相机流程ID校验与并发执行，完善PCA中心点计算。调整放料角度计算逻辑，支持机器人坐标获取。CalibrationService新增对独立XY平台相机的坐标转换处理，并引入IRobotService依赖。补充相关类型引用，提升系统健壮性与灵活性。

TeamAAS-VM/Core/Management.cs

@@ -1040,7 +1040,9 @@ namespace TeamAAS_VP.Core
         /// 移动向下相机拍照结果
         /// </summary>
         OpenCvSharp.Point3f[] UpCameraPutResults = new OpenCvSharp.Point3f[10];
-        
+
+
+
         /// <summary>
         /// 二次定位相机的工具坐标
         /// </summary>
@@ -1621,7 +1623,63 @@ namespace TeamAAS_VP.Core
                             SendTaskMessage($"触发固定相机锁附/组装拍照...", MessageLevel.Debug);
                             //相机的编号
                             //Int16 cameraIndex = plc.ReadNode<Int16>(addressConfig.In_FixedCameraPutNum.Address);
+                            if (currentProduct.IndependentCamer1ProductProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer2ProductProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer3ProductProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer4ProductProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
+                                return;
+                            }
 
+                            //获取四个独立相机的流程id
+                            var process1 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer1ProductProcedureId);
+                            var process2 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer2ProductProcedureId);
+                            var process3 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer3ProductProcedureId);
+                            var process4 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer4ProductProcedureId);
+
+                            // 执行视觉任务
+                            var _cts = new CancellationTokenSource();
+                            var task1 = _remoteCommandService.ExecutePhotoGetSinglePoint(process1, null, null, _cts.Token);
+                            var task2 = _remoteCommandService.ExecutePhotoGetSinglePoint(process2, null, null, _cts.Token);
+                            var task3 = _remoteCommandService.ExecutePhotoGetSinglePoint(process3, null, null, _cts.Token);
+                            var task4 = _remoteCommandService.ExecutePhotoGetSinglePoint(process4, null, null, _cts.Token);
+
+                            var visionResults = await Task.WhenAll(task1, task2, task3, task4);
+                            if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
+                            {
+                                Vector<double> p1=Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
+                                Vector<double> p2=Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p3=Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p4=Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+
+                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p4, p3 });
+                                UpCameraPutResults[0].X = (float)center.Center[0];
+                                UpCameraPutResults[0].Y = (float)center.Center[1];
+                                UpCameraPutResults[0].Z= (float)center.MajorAxisAngle;
+                            }
+                            else
+                            {
+                                if (isDryMode)
+                                {
+                                    await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)1);
+                                    return;
+                                }
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)2);
+                                return;
+                            }
 
                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameraPutStatus.Address, (Int16)1);
                         }
@@ -1643,6 +1701,63 @@ namespace TeamAAS_VP.Core
                             //相机的编号
                             //Int16 cameraIndex = plc.ReadNode<Int16>(addressConfig.In_FixedCameracheckNum.Address);
 
+                            if (currentProduct.IndependentCamer1AuxProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer2AuxProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer3AuxProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)2);
+                                return;
+                            }
+                            if (currentProduct.IndependentCamer4AuxProcedureId == Guid.Empty)
+                            {
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)2);
+                                return;
+                            }
+
+                            //获取四个独立相机的流程id
+                            var process1 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer1AuxProcedureId);
+                            var process2 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer2AuxProcedureId);
+                            var process3 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer3AuxProcedureId);
+                            var process4 = _productService.GetCurrentProductProcedureModelById(currentProduct.IndependentCamer4AuxProcedureId);
+
+                            // 执行视觉任务
+                            var _cts = new CancellationTokenSource();
+                            var task1 = _remoteCommandService.ExecutePhotoGetSinglePoint(process1, null, null, _cts.Token);
+                            var task2 = _remoteCommandService.ExecutePhotoGetSinglePoint(process2, null, null, _cts.Token);
+                            var task3 = _remoteCommandService.ExecutePhotoGetSinglePoint(process3, null, null, _cts.Token);
+                            var task4 = _remoteCommandService.ExecutePhotoGetSinglePoint(process4, null, null, _cts.Token);
+
+                            var visionResults = await Task.WhenAll(task1, task2, task3, task4);
+                            if (visionResults[0].IsSucceed && visionResults[1].IsSucceed && visionResults[2].IsSucceed && visionResults[3].IsSucceed)
+                            {
+                                Vector<double> p1 = Vector<double>.Build.Dense(new double[] { visionResults[0].X, visionResults[0].Y });
+                                Vector<double> p2 = Vector<double>.Build.Dense(new double[] { visionResults[1].X, visionResults[1].Y });
+                                Vector<double> p3 = Vector<double>.Build.Dense(new double[] { visionResults[2].X, visionResults[2].Y });
+                                Vector<double> p4 = Vector<double>.Build.Dense(new double[] { visionResults[3].X, visionResults[3].Y });
+
+                                var center = RectangleCenterCalculator.CalculateByPCAWithKnownOrder(new List<Vector<double>> { p1, p2, p4, p3 });
+                                UpCameraPutResults[1].X = (float)center.Center[0];
+                                UpCameraPutResults[1].Y = (float)center.Center[1];
+                                UpCameraPutResults[1].Z = (float)center.MajorAxisAngle;
+                            }
+                            else
+                            {
+                                if (isDryMode)
+                                {
+                                    await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)1);
+                                    return;
+                                }
+                                await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)2);
+                                return;
+                            }
 
                             await plc.WriteNodeAsync(addressConfig.Out_FixedCameracheckStatus.Address, (Int16)1);
                         }
@@ -1741,6 +1856,7 @@ namespace TeamAAS_VP.Core
                         if (state == 1)
                         {
                             SendTaskMessage($"收到多相机坐标合并请求，开始合并...", MessageLevel.Debug);
+
                             if (isDryMode)
                             {
                                 var point1 = currentProduct.ScrewPoints[0].Clone();
@@ -1751,31 +1867,33 @@ namespace TeamAAS_VP.Core
                                 return;
                             }
 
-                            double upCameraX = 0, upCameraY = 0, upCameraU = 0;
-                            upCameraX = UpCameraPutResults[1].X;
-                            upCameraY = UpCameraPutResults[1].Y;
-                            upCameraU = UpCameraPutResults[1].Z;
-                            //如果下相机拍照的次数是1，则判断用时拍照的相机是几个
-                            int upCameraNum = _configService.GetSystemConfiguration().UpCameraPutNum;
-                            if (upCameraNum == 2)
+                            // 获取机器人当前坐标
+                            var robot = _robotService.GetAllRobots().FirstOrDefault();
+                            if (robot == null)
                             {
-                                upCameraX = (UpCameraPutResults[1].X + UpCameraPutResults[2].X) / 2;
-                                upCameraY = (UpCameraPutResults[1].Y + UpCameraPutResults[2].Y) / 2;
-                                upCameraU = Common.CalculateAngleBetweenPointsDegrees(new PointF((float)UpCameraPutResults[1].X, (float)UpCameraPutResults[1].Y), new PointF((float)UpCameraPutResults[2].X, (float)UpCameraPutResults[2].Y));
+                                SendTaskMessage($"未找到机器人，无法执行上相机锁附/组装拍照视觉任务", MessageLevel.Alarm);
+                                await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)2);
+                                return;
+
                             }
+                            var currentPosition = robot.GetRobotPos();
 
-                            var point = currentProduct.ScrewPoints[0].Clone();
-                            var pn = Vector<double>.Build.Dense(new double[] { upCameraX, upCameraY });
-                            //目标角度：
-                            double angle = point.U_Position + (DowmCameraResults[1].Z - currentProduct.FixedUpCameraTempletAngle) + (upCameraU - currentProduct.MoveDownCamerTempletAngle);
+                            //以LCD的中线创建工具坐标
+                            ToolCoord toolCoord = new ToolCoord();
+                            toolCoord.SetTool(UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
 
-                            var toolCoord = new ToolCoord();
-                            toolCoord.SetTool(DowmCameraResults[1].X, DowmCameraResults[1].Y);
-                            //将_PutPoint转换为Tool0下要走的点
+                            
+                            //计算放料角度
+                            double angle= currentPosition.U - DowmCameraResults[1].Z + DowmCameraResults[0].Z;
+
+                            //将产品的中心点坐标转换为Tool0下的坐标
+                            var point = currentProduct.ScrewPoints[0].Clone();
+                            var pn = Vector<double>.Build.Dense(new double[] { DowmCameraResults[0].X, DowmCameraResults[0].Y });
                             var p0 = toolCoord.GetTool0Coord(pn, angle);
                             point.X_Position = (float)p0[0];
                             point.Y_Position = (float)p0[1];
                             point.U_Position = (float)angle;
+
                             SendTaskMessage($"点：1，{point.X_Position:F3}，{point.Y_Position:F4}，{point.U_Position:F4}", MessageLevel.Info);
                             await point.WriteToPlcAddress(addressConfig.Out_Screw, plc);
                             await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)1);

TeamAAS-VM/Services/CalibrationService.cs

@@ -4,10 +4,12 @@ using MathNet.Numerics.LinearAlgebra;
 using System;
 using System.Collections.Generic;
 using System.Collections.ObjectModel;
+using System.Drawing;
 using System.IO;
 using System.Linq;
 using System.Windows.Forms;
 using TeamAAS_VP.Core;
+using TeamAAS_VP.Core.Robots;
 using TeamAAS_VP.Enums;
 using TeamAAS_VP.Interfaces;
 using TeamAAS_VP.Models.Calibration;
@@ -22,6 +24,7 @@ namespace TeamAAS_VP.Services
     /// </summary>
     public class CalibrationService : ICalibrationService
     {
+        IRobotService _robotService;
         /// <summary>
         /// 内部路径常量定义（相对路径）
         /// </summary>
@@ -40,9 +43,9 @@ namespace TeamAAS_VP.Services
         /// <summary>
         /// 构造函数。注意：不在构造中自动加载校准，调用者应根据需要调用 <see cref="LoadAll"/>。
         /// </summary>
-        public CalibrationService()
+        public CalibrationService(IRobotService robotService)
         {
-            // 不在构造中自动加载，调用者可以选择 LoadAll
+            _robotService = robotService ?? throw new ArgumentNullException(nameof(robotService));
         }
 
         /// <summary>
@@ -364,6 +367,32 @@ namespace TeamAAS_VP.Services
                 var cc = rotationMatrix * p3 + phere;
                 rpos = Vector<double>.Build.Dense(new double[] { cc[0], cc[1] }); // Mark 点在 tool0 下的坐标
             }
+            else if (calib.CameraMount == Enums.CameraMount.MobileDown_IndependentXYPlatform)
+            {
+                //获取相机对应的移动模组的编号
+                int moduleIndex = calib.CameraMotionModuleIndex;
+                //获取机器人对象
+                XYZU_Robot robot = _robotService.GetRobot(calib.RobotId) as XYZU_Robot;
+                //获取当前移动模组的位置
+                var movePos = robot?.GetCameraPos(moduleIndex);
+
+                //将此时的坐标转换成独立模组坐标系下的坐标
+                //旋转矩阵
+                Matrix<double> R = Matrix<double>.Build.DenseOfArray(calib.IndependentDownCameraMotionModuleRotationMatrix);
+                //平移矩阵
+                Matrix<double> T = Matrix<double>.Build.DenseOfArray(calib.IndependentDownCameraMotionModuleTranslationMatrix);
+                //平移向量
+                Vector<double> PT = Vector<double>.Build.Dense(new double[] { T[0, 2], T[1, 2] });
+
+                Vector<double> p100 = Vector<double>.Build.Dense(new double[] { calib.IndependentDownCameraMotionModulePoint.X, calib.IndependentDownCameraMotionModulePoint.Y });
+                Vector<double> p101 = Vector<double>.Build.Dense(new double[] { movePos.X, movePos.Y });
+                //计算独立模组坐标系下的坐标
+                Vector<double> pl1 = R.Inverse() * (rpos - PT);
+                Vector<double> pl2 = pl1 + (p101 - p100);
+                //转换回机器人坐标系下的坐标
+                var finalrpos = R * pl2 + PT;
+                return (true, finalrpos[0], finalrpos[1], angle);
+            }
             else
             {
                 // 其他 CameraMount：如果提供了 robotCoord，则将仿射变换得到的点转换为工具坐标