[发明专利]一种基于混连式轨道制孔系统的加工点定位、法向检测和调整方法

权利要求书

1.一种基于混连式轨道制孔系统的加工点定位、法向检测和调整方法，其特征是：

所述的加工点定位方法是：

设：已知目标点在小车坐标系下的位置为Dc＝[xDt,yDt,zDt]^T和法向Nc＝[l,m,n]^T；压力脚距离目标点位置Lpd；

末端坐标系OeXYZ的坐标原点Oe在压力脚压力环中心贴合蒙皮的位置上，Xe平行于压力脚安装座平面，初始平行位置时和Xc平行并指向驱动机构方向，Ze轴沿底座平面法向向上，Ye形成向外的右手系；小车坐标系OcXYZ的坐标原点Oc在固定腿足部铰链中心，Xc平行于底座指向驱动机构方向，Zc轴沿底座平面法向向上，Yc形成向外的右手系，环形轨道系统的制孔小车固定腿Z1、单向补偿腿Z2、Z3、Z4、X1、Y、压力脚P共7个运动轴决定位姿；

则：从末端坐标系到小车坐标系的旋转矩阵Re^c:

其中SA、CA、SB、CB分别表示sinA,cosA,sinB,cosB；

可知：

其中：A、B分别为绕固定坐标系(小车坐标系)的x-y顺序的角度；l，m，n为法向Nc＝[l,m,n]^T的三个分量；

OcOe为目标位置下，小车坐标系原点Oc到末端坐标系原点Oe的距离，也就是Oe在小车坐标系下的位置，等于目标位置在目标点沿目标法向提高Lpd；

即OcOe＝Dc+Lpd*Nc，所述法向是蒙皮的外法向；

将公式(2)求得的A、B角值和式(3)求得的OcOe值代入从末端坐标系到小车坐标系的齐次变换矩阵Te^c:

式中d是从小车坐标系原点到末端坐标系原点的矢量，用公式(3)求解；

上部框架上F1、F2、F3、F4点，在末端坐标系的位置为Fie，在小车坐标系下位置为Fic(i＝1,2,3,4)

注意：

F_1e＝[-x₁ y p 1]^T (6)

式中：

x₁为小车x方向的运动位置，y为小车y方向的运动位置，p为小车压力脚的运动位置；

设F_1c＝[x_F1c y_F1c z_F1c 1]^T

由于腿的目标位置就是目标法向Nc，所以各腿的点法式方程均是：

球铰链位置Si就是该方程与所在平面方程Z＝Zi的交点；

如S1就是该方程与Z＝0的交点，联立可得：

注意S1c为原点Oc，所以：

S_1c＝[0 0 0 1]^T (9)

由(8)和(9)可知：

公式(11)中可求得x1、y、Zf1c

y＝z_F1c·m/(n·CA)+p·tanA-d_y/CA (13)

x₁＝-y·SA/tanB与z_F1c/SB-p·CA/tanB-d_z/SB (14)

代入公式(5)可求得

所述的法向检测方法是：

设四个传感器Si(即S1～S4)的位置和方向为：

传感器Si检测到的距离为DSi；

那么Si检测到蒙皮的实际位置为TSi：

TS_i＝Ps_i+DS_i·NS_i

注意方向是有正负号的，垂直安装时NSi为[0，0，-1]^T；

任何3点TSi、TSj、TSk确定的蒙皮外法向N_ijk：

N_ijk＝(TS_k-TS_j)×(TS_i-TS_j)

N_ijk再单位化即可获得单位化的法向向量；

四个传感器Si(i＝1，2，3，4)确定末端坐标系下的法向N_e；N₄₁₂、N₁₂₃、N₂₃₄、N₃₄₁单位化后的法向为n₄₁₂、n₁₂₃、n₂₃₄、n₃₄₁，则：

N_e＝(n₄₁₂+n₁₂₃+n₂₃₄+n₃₄₁)/4；

所述的法向调姿的方法为：

首先，将末端坐标系下的法向Ne转换为当前小车坐标系下目标法向Nc；

将Dc＝[xDt，yDt，zDt]^T和Nc’＝[1′，m′，n′]^T值重新代入公式(2)～(16)可求解调整到检测法向状态下各轴的运动位置；然后设置各轴运行到新的绝对位置，或者与原有的位置比较后得到增量位置值，各轴运行增量值；

在调姿过程重，制孔点始终没变，也就是TCP点就是制孔/K孔点Dc，在末端坐标系的[0；0；-Lpd]处。

2.根据权利要求1所述的基于混连式轨道制孔系统的加工点定位、法向检测和调整方法，其特征是法向检测时，如果某一传感器Si数据有问题，则含有i的三组法向与离线差别大于阈值20％以上时报警，说明传感器Si数据有问题，则此时应使用不含Si数据的那个方向为法向。