关于dh解算的说明

dh解算线程
输入：视觉计算的抬升角、回转角
输出：截割头在三维巷道中的x、y、z坐标

截割头的几何模型如下，炮头理想为梯形。

掘进机炮头在巷道中的坐标系如下：

最终输出的第0个点就是炮头中心位置的坐标。
实际测量时需要三个变量，即大直径，小直径，梯台高度。
例如：
三楼实验室的掘进机炮头大直径BR为190mm，小直径lr150mm，高度h为160mm。

/

参数的具体值需要和以下代码进行匹配：

	double p[30] = { 0,0,0,0,-parameter[9] / 2 ,0,0,parameter[9] / 2 ,0,parameter[9] / 2 ,0,0, -parameter[9] / 2, 0,0,0,0,-parameter[7],0,-R,-parameter[7] ,0,R,-parameter[7] ,R,0,-parameter[7] ,-R,0, -parameter[7] };

将实验室的炮头参数输入后，该数组的数据如下：

0    0    0
0    -75    0
0    75    0
75    0    0
-75    0    0
0    0    -160
0    -95    -160
0    95    -160
95    0    -160
-95    0   -160