[发明专利]一种基于毫米波雷达的轨道障碍物识别方法

权利要求书

1.一种基于毫米波雷达的轨道障碍物识别方法，其特征在于，包括以下步骤：

S1：毫米波雷达进入无GPS信号的场景之前，设置毫米波雷达的GPS经纬度信息G₁ (lo,la)为轨道的起始位置，笛卡尔坐标为P (0, 0)，通过GPS计算得到毫米波雷达其它时刻的笛卡尔坐标P(x, y)；

毫米波雷达进入无GPS信号的场景后，通过惯导系统计算并记录毫米波雷达的笛卡尔坐标P_n(x, y)；

利用毫米波雷达采集的轨道点云坐标，将整个轨道线路分割成多个轨道分段，每个轨道分段在垂直方向上的距离为S米；

按一定间隔记录毫米波雷达测量的相对于每个轨道分段起始点的x、y坐标，并将这些坐标与相应的笛卡尔坐标P_n (x, y)关联；

计算并记录每个轨道分段起始位置到结束位置在水平方向上偏转的角度θ；

轨道信息全部记录完毕后，进入步骤S2；

S2：毫米波雷达再次进入同一轨道线，进入无GPS信号的场景后毫米波雷达根据惯导系统获得当前的位置，结合当前识别的轨道分段信息与存储的多个轨道分段信息来拼接出远超S米的轨道信息，判断检测到的障碍物是否在轨道线之内。

2.根据权利要求1所述的基于毫米波雷达的轨道障碍物识别方法，其特征在于，步骤S2包括以下步骤：

S21：毫米波雷达通过惯导系统获得自身的笛卡尔坐标P_a(x_a, y_a)，并获取已记录的、与当前位置距离最近的轨道分段起始位置笛卡尔坐标P_b ( x_b, y_b)，然后识别从P_a到P_b这一段的轨道线位置；

S22：从内存中提取出已记录的P_a到P_b这一段的轨道分段位置信息，计算与当前识别的轨道线在x轴上的方差之和var_sum，当var_sum factor * n时，其中factor为系数，n为坐标个数，执行下一步骤；

S23：计算雷达从P_a运行到P_b位置时相对当前位置偏转的角度θ_a→b =；

S24：对于拼接的第一段轨道分段，存在记录的描述轨道分段与轨道分段起始位置的k组坐标(x_(1…k)，y_(1…k))，计算这些坐标相对于雷达当前位置的x坐标为x_(1…k)/cosθ_a→b，y坐标为y_(1…k) +(y_b-y_a)；对于拼接的第二段轨道分段，计算这些坐标相对于雷达当前位置的x坐标为x_(1…k)/cos(θ_a→b+θ_b→c)，y坐标为S + y_(1…k) + (y_b- y_a)，θ_b→c为雷达从P_b运行到P_c位置时相对P_b偏转的角度, P_c点的笛卡尔纵坐标比P_b点大S米；对于拼接的第三段轨道分段，计算这些坐标相对于雷达当前位置的x坐标为x_(1…k) / cos(θ_a→b+θ_b→c+θ_c→d)，y坐标为2* S +y_(1…k) + (y_b- y_a)，θ_c→d为雷达从P_c运行到P_d位置时相对P_c偏转的角度, P_d点的笛卡尔纵坐标比P_c点大S米，依此类推，拼接出远超S米距离的轨道线；

S25：毫米波雷达根据拼接后的轨道线及检测出的障碍物，判别障碍物是否在轨道线中。