[发明专利]一种多探头式车载雷达的距离求取方法

权利要求书

1.一种多探头式车载雷达的距离求取方法，多探头式车载雷达包括第一转轴(1)、第二转轴(2)、第一驱动电机(3)、第二驱动电机(4)和控制电路(9)；设载体的行进方向为X轴方向，沿X轴望去的右方为Y轴方向，垂直向上为Z轴方向，以确定立体坐标系XYZ；第一转轴、第二转轴的轴线分别与Z轴、X轴重合，第一驱动电机、第二驱动电机的输出轴分别与第一转轴、第二转轴相连接，第一驱动电机与第二驱动电机固定在一起；

第一转轴上固定有当第一转轴不发生转动时位于XZ平面内的1号超声波收发器(5)和2号超声波收发器(6)，1号超声波收发器、2号超声波收发器的探测方向分别朝前下方和前上方，且与X轴正向的夹角分别为δ₁、δ₂；第二转轴上固定有当第二转轴不发生转动时位于XZ平面内的3号超声波收发器(7)和4号超声波收发器(8)，3号超声波收发器、4号超声波收发器的探测方向分别朝向上方和下方，且与X轴正向的夹角分别为δ₃、δ₄；

其特征在于，设t时刻控制电路通过1号、2号、3号和4号超声波收发器测得与障碍物的距离分别为x₁、x₂、x₃、x₄，该时刻第一旋转轴、第二旋转轴的旋转角度分别为α、β，多探头式车载雷达的距离求取方法通过以下步骤来实现：

a).计算安装误差，设控制电路通过三轴加速计测得重力加速度在三个坐标轴上的分量分别为a_x、a_y、a_z，则车载雷达X轴和Y轴的安装误差分别通过公式(1)和公式(2)进行求取：

b).计算悬空物和台阶距离，通过公式(3)和公式(4)分别计算上方悬空物和下方台阶的距离：

L_v3＝x₃*sin(-θ_x+δ₃)*cos(θ_y+β)    (3)

L_v4＝x₄*sin(θ_x+δ₄)*cos(θ_y+β)    (4)

可得到载体车辆距离上下方障碍物的最近距离为L_v＝min(L_v3,L_v4)；

c).计算行进方向障碍物距离，通过公式(5)至公式(8)分别计算1号、2号、3号和4号超声波收发器测得的前方障碍物的距离：

L₁＝x₁*cos(θ_x+δ₁)*cosθ_y*cosα    (5)

L₁＝x₂*cos(-θ_x+δ₂)*cosθ_y*cosα    (6)

L₃＝x₃*cos(-θ_x+δ₃)*cos(θ_y+β)    (7)

L₄＝x₄*cos(θ_x+δ₄)*cos(θ_y+β)    (8)

d).计算障碍物距离，与前一时刻相比，当L_v无变化时，表明没有台阶或悬空类障碍物，此时载体车辆与前方障碍物的距离L为：

L＝min(L₁,L₂)

与前一时刻相比，当L_v存在变化时，表明有台阶或悬空类障碍物存在，此时载体车辆与前方障碍物的距离L为：

L＝min(L₁,L₂,L₃,L₄)。