Loading... 截骨的过程中，为保护周围的软组织和避免损害关节周围的韧带、肌腱、神经等重要人体结构，机器人必须在严格的安全边界内切骨。 ### 截骨边界判定原理 因为三维空间中检测碰撞比较复杂，又由于截骨边界线与锯片共享一个平面，将其投射到XY平面后不改变其碰撞结果，所以可以将三维降维成二维来简化问题 ### 截骨边界判定前提 通过手术规划得到截骨面上的边界线（事先已知，由三维点集组成），锯片的TCP的位姿（根据机械臂状态实时获得） 判定使用java语言实现，原截骨点通过创建一个ArrayList来承接 `List<Coordinate> originPoints = new ArrayList<>();` 使用jts库来更加直观的描述点信息 通过add增加点，因为ArrayList可以动态存储坐标值，用起来非常方便 `originPoints.add(new Coordinate(x, y, z))` 可以通过索引来获得每个点的信息，`i`的值和python相同 `Coorinate point = originalPoints.get(i)` `point.x`、`point.y`、`point.z` ### 截骨边界判定过程 1. 首先传入一系列点 2. 定义刀片尺寸和坐标系建立，并根据工具TCP的位置进行旋转操作，根据旋转矩阵进行刀片四个顶点位置的计算 3. 投影 4. 判定是否触碰边界 ### Appendix 完整代码如下： ```java import Jama.Matrix; import com.vividsolutions.jts.geom.Coordinate; import com.vividsolutions.jts.geom.GeometryFactory; import com.vividsolutions.jts.geom.LineString; import com.vividsolutions.jts.geom.Polygon; import java.util.ArrayList; import java.util.List; public class jtsBorder3D { public static void main(String[] args) { // 原始点集(−2,4,−2) (−1.5,2.25,-0.75) (−1,1,0) // (−0.5,0.25,0.25) (0,0,0) (0.5,0.25,−0.75) // (1,1,-2) (1.5,2.25,3.75) (2,4,−6) // TODO 这里的原始边界点集每次切骨的时候一次性给出 List<Coordinate> originPoints = new ArrayList<>(); originPoints.add(new Coordinate(-2,4,-2)); originPoints.add(new Coordinate(-1.5,2.25,-0.75)); originPoints.add(new Coordinate(-1,1,0)); originPoints.add(new Coordinate(-0.5,0.25,0.25)); originPoints.add(new Coordinate(0,0,0)); originPoints.add(new Coordinate(0.5,0.25,-0.75)); originPoints.add(new Coordinate(1,1,-2)); originPoints.add(new Coordinate(1.5,2.25,3.75)); originPoints.add(new Coordinate(2,4,-6)); // 同一平面矩形 真实的数值是width = 22; depth = 90 深度其实作用不大 主要还是锯片左上角和右上角的位置 double width = 2; double depth = 2; Coordinate tcp_point = new Coordinate(0,0,0); // TODO 这里要实时变动 Coordinate leftUp_local = new Coordinate(0,-width/2,0); Coordinate rightUp_local = new Coordinate(0,width/2,0); Coordinate leftDown_local = new Coordinate(0,width/2,-depth); Coordinate rightDown_local = new Coordinate(0,-width/2,-depth); List<Coordinate> localPoints = new ArrayList<>(); localPoints.add(leftUp_local); localPoints.add(rightUp_local); localPoints.add(rightDown_local); localPoints.add(leftDown_local); // 旋转角度，以弧度为单位 TODO 这里也要实时变动 double rx = Math.toRadians(45); double ry = Math.toRadians(0); double rz = Math.toRadians(0); // 创建围绕 x 轴的旋转矩阵 Matrix Rx = new Matrix(new double[][]{ {1, 0, 0}, {0, Math.cos(rx), -Math.sin(rx)}, {0, Math.sin(rx), Math.cos(rx)} }); // 创建围绕 y 轴的旋转矩阵 Matrix Ry = new Matrix(new double[][]{ {Math.cos(ry), 0, Math.sin(ry)}, {0, 1, 0}, {-Math.sin(ry), 0, Math.cos(ry)} }); // 创建围绕 z 轴的旋转矩阵 Matrix Rz = new Matrix(new double[][]{ {Math.cos(rz), -Math.sin(rz), 0}, {Math.sin(rz), Math.cos(rz), 0}, {0, 0, 1} }); // 计算复合旋转矩阵 R = Rz * Ry * Rx Matrix R = Rz.times(Ry).times(Rx); List<Coordinate> rotatedPoints = new ArrayList<>(); for (Coordinate point : localPoints) { // 将每个点转换为 3x1 矩阵 Matrix pointMatrix = new Matrix(new double[][]{ {point.getOrdinate(0)}, {point.getOrdinate(1)}, {point.getOrdinate(2)} }); // 应用旋转矩阵 Matrix rotatedPointMatrix = R.times(pointMatrix); // 将旋转后的矩阵转换回 Coordinate Coordinate rotatedPoint = new Coordinate( rotatedPointMatrix.get(0, 0) + tcp_point.getOrdinate(0), rotatedPointMatrix.get(1, 0) + tcp_point.getOrdinate(1), rotatedPointMatrix.get(2, 0) + tcp_point.getOrdinate(2) ); rotatedPoints.add(rotatedPoint); } // 这样originalPoints和rotatedPoints都是List点集了 接下来可以使用了 Coordinate[] coordsPolygon = new Coordinate[]{ new Coordinate(rotatedPoints.get(0).x, rotatedPoints.get(0).y), new Coordinate(rotatedPoints.get(1).x, rotatedPoints.get(1).y), new Coordinate(rotatedPoints.get(2).x, rotatedPoints.get(2).y), new Coordinate(rotatedPoints.get(3).x, rotatedPoints.get(3).y), new Coordinate(rotatedPoints.get(0).x, rotatedPoints.get(0).y) }; Coordinate[] coordsBorder = new Coordinate[originPoints.size()]; for (int i = 0; i < originPoints.size(); i++) { Coordinate originalPoint = originPoints.get(i); coordsBorder[i] = new Coordinate(originalPoint.x, originalPoint.y, originalPoint.z); } GeometryFactory geometryFactory = new GeometryFactory(); Polygon rectangle = geometryFactory.createPolygon(coordsPolygon); LineString border = geometryFactory.createLineString(coordsBorder); boolean intersects = border.intersects(rectangle); System.out.println(intersects); } } ``` 最后修改：2024 年 04 月 09 日 © 允许规范转载 赞 0 如果觉得我的文章对你有用，请随意赞赏
