[发明专利]一种无人机上TOF距离传感器的测距方法及系统

权利要求书

1.一种无人机上TOF距离传感器的测距方法，其特征在于，所述方法包括：

通过TOF距离传感器测量无人机与目标物体之间的距离，获取测量距离数据；

根据滑动模型对所述测量距离数据进行滤波，得到滤波距离数据；

获取所述无人机的姿态数据，并根据所述姿态数据对所述滤波距离数据进行校正，得到校正距离数据；

所述根据滑动模型对所述测量距离数据进行滤波，得到滤波距离数据之前，包括：

建立测量距离数据与滑动模型的对应关系：当测量距离数据的数值小于X₁时，对应第一滑动模型；当测量距离数据的数值大于或等于X₁且小于X₂时，对应第二滑动模型；当测量距离数据的数值大于X₂时，对应第三滑动模型，其中X₁＜X₂；

使用两个滑动平均滤波器，m点滑动平均滤波器和n点滑动平均滤波器，mn，将F₁(x)设为当前时刻的测量距离数据，将F₂(x)作为m点滑动平均滤波器的输出值，将F₃(x)作为n点滑动平均滤波器的输出值，将F₄(x)设为上一时刻的滤波距离数据，则算法公式为：

其中，y_t为滤波距离数据，F(x)为滤波器的输出值，t(x)为各滤波器输出值的权值，t₁(x)+t₂(x)+…+t_n(x)＝1；

1.将当前时刻的测量距离数据与上一时刻的滤波距离数据进行比较，若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值大于阈值f(x_t)时，直接输出当前时刻的测量距离数据，即令上式中t₁(x)等于1，t₂(x)等于0，t₃(x)等于0，t₄(x)等于0，其中阈值f(x_t)＝a₁x_t+b₁，t为当前时刻；

2.若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值小于或等于阈值f(x_t)时，进行m点和n点的滑动平均滤波，mn；

3.根据判定结果进行输出，输出结果有三种情况，分别是输出m点结果、输出n点结果和输出上一时刻的滤波距离数据，具体判定过程如下：

1)若max(An_t-z，An_t-z+1，…，An_t)-min(An_t-z，An_t-z+1，…，An_t)g(x_t)，则输出m点平均滤波器滤波处理后的数据，即令上式中t₁(x)等于0，t₂(x)等于1，t₃(x)等于0，t₄(x)等于0，其中阈值g(x_t)＝a₂x_t+b₂，An为n点滑动平均滤波器滤波处理后的数据，max(An_t-z，An_t-z+1，…，An_t)表示从An_t-z到An_t这几个不同时刻的输出量中的最大值，min(An_t-z，An_t-z+1，…，An_t)表示从An_t-z到An_t这几个不同时刻的输出量中的最小值；

2)若min(An_t-w，An_t-w+1，…，An_t)y_t-1或者max(An_t-w，An_t-w+1，…，An_t)y_t-1或者abs(An_t-y_t-1)h(x_t)，则输出n点平均滤波器滤波处理后的数据，即令上式中t₁(x)等于0，t₂(x)等于0，t₃(x)等于1，t₄(x)等于0，其中阈值h(x_t)＝a₃x_t+b₃，y_t-1为上一时刻的滤波距离数据，abs(An_t-y_t-1)表示取An_t-y_t-1的绝对值；

3)如果条件1)和条件2)都不满足，则维持上一时刻输出值，即输出上一时刻的滤波距离数据，即令上式中t₁(x)等于0，t₂(x)等于0，t₃(x)等于0，t₄(x)等于1。