[发明专利]一种基于四面体构型的距离传感器法矢测量方法

权利要求书

1.一种基于四面体构型的距离传感器法矢测量方法，其特征是它包括以下步骤：

首先，在机床或机器人的末端执行器上安装呈等边三角形布置的三个激光测距传感器，并且固定三个激光测距传感器的测量方向，使得其激光束汇聚于同一点，由三个激光测距传感器和所述的汇聚点共同构成一个空间四面体；将坐标原点建立在刀具末端点，三个激光测距传感器呈正三角形分布在刀具四周；

其次，利用空间四面体顶点与待测曲面的三种位置关系：顶点在曲面内、顶点在曲面上、顶点在曲面外，结合激光测距传感器测量的数值，求得待测制孔曲面上待制孔中心点的法矢n；

最后，根据上述测量计算所得到的待制孔中心点的法矢信息，通过反解算法，求得机床或机器人各轴的驱动量，用于控制系统实现调姿运动，使刀具轴线与求得的法矢一致。

2.根据权利要求1所述的方法，其特征是：为了使得待测制孔曲面法矢n的测量结果更加精确，作如下条件约束：l_min≤l_i≤l_max(i＝1,2,3)

式中l_i为三个激光测距传感器激光发射点到待测制孔曲面的距离，l_min和l_max分别为满足制孔法矢精度要求时l_i的最小距离和最大距离，其具体数值可由激光测距传感器激光发射点到激光束汇聚点的距离和待制孔法矢的精度等级求出；当l_i满足上述约束后，待测制孔曲面的法矢n应根据四面体顶点在待测曲面内、四面体顶点在待测曲面外和四面体顶点在待测曲面上三种情况具体求解。

3.根据权利要求1所述的方法，其特征是四面体顶点在待测曲面内时，设定x_t方向为过坐标原点O_t点平行于AB且由A指向B方向，y_t方向为O_tC方向，z_t方向根据右手定理获得；首先，根据设计和安装，确定三个激光测距传感器的安装位置，并使得三个激光测距传感器三条激光汇聚于一点D，即在激光测距传感器对曲面法矢进行测量之前，通过机械结构及装配保证四面体D-ABC构型已知；则A、B、C在O_tx_ty_tz_t坐标系下的坐标分别为AC(0,CD·cos∠DCO_t,0)，

当待检测位置所在曲面与四面体相交，根据几何关系可以求得：

A₁(-A₁D·cos∠DAO_t·cos∠O_tAB,-A₁D·cos∠DAO_t·sin∠O_tAB,AA₁·sin∠DAO_t)

B₁(B₁D·cos∠DBO_t·cos∠O_tBA,1B₁D·cos∠DBO_t·sin∠O_tBA,BB₁·sin∠DBO_t)

C₁(0,DC₁·cos∠DCO_t,CC₁·sinDCO_t)

令A₁(x_a1,y_a1,z_a1)，B₁(x_b1,y_b1,z_b1)，C₁(x_c1,y_c1,z_c1)，则A₁B₁(x_b1-x_a1,y_b1-y_a1,z_b1-z_a1)，A₁C₁(x_c1-x_a1,y_c1-y_a1,z_c1-z_a1)，B₁C₁(x_b1-x_c1,y_b1-y_c1,z_b1-z_c1)，设平面A₁B₁C₁的法向坐标为(l₁,m₁,n₁)，则，根据法向量的定义

(xb1-xa1)·l1+(yb1-ya1)·m1+(zb1-za1)·n1=0(xc1-xa1)·l1+(yc1-ya1)·m1+(zc1-za1)·n1=0(xb1-xc1)·l1+(yb1-yc1)·m1+(zb1-zc1)·n1=0---(1)]]>

通过上式，可以求解(l₁,m₁,n₁)，其中n₁即为待制孔位置法矢量。

4.根据权利要求1所述的方法，其特征是四面体顶点在曲面外，待检测位置所在曲面相交于空间四面体顶点的延长线上时，根据几何关系可以求得：

A₂(DA₂·cos∠DAO_t·cos∠O_tAB,DA₂·cos∠DAO_t·sin∠O_tAB,AA₂·sin∠DAO_t)

B₂(-DB₂·cos∠DBO_t·cos∠O_tBA,DB₂·cos∠DBO_t·sin∠O_tBA,BB₂·sin∠DBO_t)

C₂(0,-DC₂·cos∠DCO_t,CC₂·sinDCO_t)

令A₂(x_a2,y_a2,z_a2)，B₂(x_b2,y_b2,z_b2)C₂(x_c2,y_c2,z_c2)，则A₂B₂(x_b2-x_a2,y_b2-y_a2,z_b2-z_a2)，A₂C₂(x_c2-x_a2,y_c2-y_a2,z_c2-z_a2)，B₂C₂(x_b2-x_c2,y_b2-y_c2,z_b2-z_c2)，设平面A₂B₂C₂的法向坐标为(l₂,m₂,n₂)，则，根据法向量的定义

(xb2-xa2)·l2+(yb2-ya2)·m2+(zb2-za2)·n2=0(xc2-xa2)·l2+(yc2-ya2)·m2+(zc2-za2)·n2=0(xb2-xc2)·l2+(yb2-yc2)·m2+(zb2-zc2)·n2=0---(3)]]>

通过上式(3)，可以求解(l₂,m₂,n₂)，其中n₂即为待制孔位置法矢量。