[发明专利]一种用于三维场域的转角定位方法

权利要求书

1.一种用于三维场域的转角定位方法，其特征在于，包括以下步骤：

S1：得到物体运动轨迹点的三维坐标数据；

S2：通过三维坐标数据，推算出物体运动的转角数据；

S3：对三维坐标数据进行轨迹平滑处理；

S4：对转角数据进行平滑处理；

S5：将轨迹平滑处理后的三维坐标数据和平滑处理后的转角数据输出展示。

2.根据权利要求1所述的一种用于三维场域的转角定位方法，其特征在于，在步骤S1中，在三维直角坐标系中，用X、Y和Z分别代表横向、高度、纵向3个方位，X轴朝右为正，Y轴朝上为正，Z轴朝前为正，反之为负，将物体运动轨迹附在三维直角坐标系中，设定位置从P₁(X₁，Y₁，Z₁)运动到P₂(X₂，Y₂，Z₂)。

3.根据权利要求2所述的一种用于三维场域的转角定位方法，其特征在于，在步骤S2中，依据从P₁(X₁，Y₁，Z₁)运动到P₂(X₂，Y₂，Z₂)的行动轨迹，转角数据∠rx由以下算法得出tan∠rx＝(Z₂-Z₁)/(X₂-X₁)，∠rx＝arctan((Z₂-Z₁)/(X₂-X₁))。

4.根据权利要求3所述的一种用于三维场域的转角定位方法，其特征在于，在步骤S3中，运用滑动平滑平均法和基于局域多项式最小二乘法拟合的滤波方法对三维坐标数据进行轨迹平滑处理。

5.根据权利要求4所述的一种用于三维场域的转角定位方法，其特征在于，所述滑动平滑平均法包括以下公式：

y_s(1)＝y(1)

N是常数，代表需要处理的单位时间内的轨迹点数；i为变量，代表每个轨迹点索引，取值为1至N，则每个轨迹点可表示为[X(i),Y(i),Z(i)]，使邻域内的数据点做平均代替邻域的中心点值。

6.根据权利要求5所述的一种用于三维场域的转角定位方法，其特征在于，所述基于局域多项式最小二乘法拟合的滤波方法包括以下步骤：

S31：通过拟合多项式求出轨迹点的拟合效果；

S32：调用拟合结果，求出最小二乘残差；

S33：根据最小二乘残差，通过卷积运算的方式求出原始轨迹点转换后的结果。

7.根据权利要求6所述的一种用于三维场域的转角定位方法，其特征在于，所述步骤S31中，拟合多项式为

N是常数，代表需要处理的单位时间内的轨迹点数；k为变量，代表每个轨迹点索引，取值为1至N，则每个轨迹点可表示为[X(k),Y(k),Z(k)]，n代表第k个点的值X(k),Y(k)或Z(k)，即不同维度的坐标值，a代表系数，a的下标代表第k个点的系数，p(n)为轨迹点的拟合结果。

8.根据权利要求7所述的一种用于三维场域的转角定位方法，其特征在于，所述步骤S32中，最小二乘残差公式为：

最小二乘残差公式调用了上述的拟合结果，其中，x[n]为第k个实际的点值，x[-M]为第1个点，x[0]为中间点，x[M]为最后1个点，ε代表残差，下标N代表所有轨迹点的残差。