大家好呀～

概况介绍

遇到一个特别有意思的功能，雷达相机配合，监测出现在空间的异物，雷达负责定位，相机负责拍摄。我们要做的是将雷达测得的数据处理后给到相机，保证相机能拍摄到物体，场景如下：

雷达监测到这个空间出现的物体，获得三个参数，雷达距离物体的距离distance、雷达的俯仰角hDeg、雷达与正北方偏离的角度nDeg，这三个参数唯一确定了物体的位置。相机要正确的拍摄到物体也需要三个参数，距离，俯仰角和偏离正北方的角度。所以我们要解决的问题是通过雷达获取到的数据，计算得出相机需要的参数。

雷达和相机的高度（radarHeight,cameraHeight）、雷达与相机的距离(radarToCamera)、雷达相机所在直线偏离正北方的角度(roadDeg)为已知条件。

        //初始化数据（注意角度和弧度的转换）
        
        //雷达相机连线与北边的夹角
        const roadDeg = ref((Math.PI / 180) * 45);
        //雷达与相机的距离
        const radarToCamera = ref(500);
        //雷达高度
        const radarHeight = ref(400);
        //相机高度
        const cameraHeight = ref(400);

（我第一版代码用的三角函数，通过三角形三边与角的关系来计算出距离和角度，这个可以算出来，但是有很多隐藏问题，比如需要用到的角度会着着雷达和相机的位置不同而不同，有兴趣的小伙伴可以画图试试～）

所以有了第二版代码，我觉得空间坐标系是一个很好的解决方案。

计算坐标

1.雷达坐标

如上图，我以雷达底部为原点，正东方为x轴正方向，正南方为y轴正方向建立坐标，那么雷达的坐标可以表示为（0，0，radarHeight）

            //雷达坐标
            const radar = ref({ x: 0, y: 0, z: radarHeight.value });

2.相机坐标

由于已知雷达与相机的距离(radarToCamera)，雷达相机所在直线偏离正北方的角度(roadDeg)，通过正余弦定理可以得出相机x和相机y。 相机x = radarToCamera * sin(roadDeg) 相机y = radarToCamera * cos(roadDeg) 相机z = cameraHeight

这里要注意相机x、y的正负值与相机在第几象限有关，也就是跟雷达相机所在直线偏离正北方的角度有关。0-180度，x为正，180-360度，x为负；0-90度或者270-360度，y为负，90-270度，y为正;

          //相机坐标
           const camera = ref({ x: 0, y: 0, z: 0 });
        //计算相机坐标
        const getCameraCoordinate = () => {
          const x = radarToCamera.value * Math.sin(roadDeg.value).toFixed(4);
          const y = radarToCamera.value * Math.cos(roadDeg.value).toFixed(4);
          if (roadDeg.value <= Math.PI) {
            camera.value.x = Math.abs(x);
          } else {
            camera.value.x = -Math.abs(x);
          }
          if (roadDeg.value >= Math.PI / 2 && roadDeg.value <= (Math.PI * 3) / 2) {
            camera.value.y = Math.abs(y);
          } else {
            camera.value.y = -Math.abs(y);
          }
        };

3.异物坐标

接下来计算出异物的坐标，分两种情况，第一种是异物在雷达高度以下（含雷达高度），雷达俯仰角为正；第二种是异物处于比雷达高的地方，雷达俯仰角为负。

第一种情况，先通过雷达测得的俯仰角和距离计算出异物离雷达顶部的垂直高度，为了计算更方便，此时把水平面提升至异物所在面，计算中的体现是雷达和相机的坐标z都需要减去异物到地面的高度，异物的z为0（相当于重置水平面为异物所在面）。

第二种情况，异物比雷达高，通过三角函数可求得异物坐标。

同样，这里异物的x,y也需要根据雷达与正北方角度考虑正负～

如图：

//障碍物坐标
 const hinder = ref({ x: 0, y: 0, z: 0 });
//计算障碍物的坐标(参数需传入雷达测得的距离、与北边的角度、俯仰角)
const getHinderCoordinate = (len, ndeg, hdeg) => {
  let x;
  let y;
  //把角度转化为弧度
  const nRadian = (ndeg % 360 * Math.PI) / 180;
  const hRadian = (hdeg * Math.PI) / 180;
  if (hRadian > 0) {
    //障碍物距离雷达顶部的高度
    const hinderZ = len * Math.sin(hRadian);
    //将水平面提高至障碍物高度，便于计算
    radar.value.z = hinderZ;
    camera.value.z = cameraHeight.value - (radarHeight.value - hinderZ);
    hinder.value.z = 0;
    //计算雷达底部到障碍物的距离
    const distance = Math.sqrt(Math.pow(len, 2) - Math.pow(hinderZ, 2));
    x = distance * Math.sin(nRadian).toFixed(4);
    y = distance * Math.cos(nRadian).toFixed(4);
  } else {
    radar.value.z = radarHeight.value;
    camera.value.z = cameraHeight.value;
    hinder.value.z = len * Math.sin(-hRadian) + radarHeight.value;
    x = len * Math.cos(-hRadian) * Math.sin(nRadian).toFixed(4);
    y = len * Math.cos(-hRadian) * Math.cos(nRadian).toFixed(4);
  }

  if (nRadian <= Math.PI) {
    hinder.value.x = Math.abs(x);
  } else {
    hinder.value.x = -Math.abs(x);
  }
  if (nRadian >= Math.PI / 2 && nRadian <= (Math.PI * 3) / 2) {
    hinder.value.y = Math.abs(y);
  } else {
    hinder.value.y = -Math.abs(y);
  }
};

计算获得相机需要的参数

目前雷达、相机、异物的坐标都计算出来了，有坐标了计算长度和角度还不容易，直接上代码！

1.距离

//计算相机到障碍物的距离
const getResultDistance = () => {
  //空间中两点之间的距离公式：d=√[（x1-x2）^2+（y1-y2)）2+（z1-z2）^2]
  let d = Math.sqrt(
    Math.pow(camera.value.x - hinder.value.x, 2) +
      Math.pow(camera.value.y - hinder.value.y, 2) +
      Math.pow(camera.value.z - hinder.value.z, 2)
  );
  return d;
};

2.俯仰角

需要通过雷达的俯仰角判断相机的俯仰角为正还是负

//计算相机的俯仰角，参数为雷达的俯仰角
const getHorizontalDeg = (deg) => {
  let sinDeg;
  let d = getResultDistance();
  if (deg <= 0) {
    sinDeg = -(hinder.value.z - camera.value.z) / d;
  } else {
    sinDeg = camera.value.z / d;
  }
  return (Math.asin(sinDeg) * 180) / Math.PI;
};

3.相机需要相对于正北方偏离角度

这里用到了平面向量求夹角，引入指向正北方的单位向量，求异物向量与单位向量的夹角。

由于向量之间的夹角范围是0-180，但是对于相机来说，相对于北方的角度范围是0-360，所以最后需要判断一步，异物向量的x小于0时，求得的角度c虽然在0-180之间，但实际相机需要旋转360-c

//计算相机与北边的角度
const getNDeg = () => {
  //去掉z，转为平面向量求夹角，向量夹角公式cosθ=a*b/(|a|*|b|)
  const vectorAB = {
    x: hinder.value.x - camera.value.x,
    y: hinder.value.y - camera.value.y,
  };
  const vectorN = { x: 0, y: -1 };
  const lenAB = Math.sqrt(Math.pow(vectorAB.x, 2) + Math.pow(vectorAB.y, 2));
  const lenN = Math.sqrt(Math.pow(vectorN.x, 2) + Math.pow(vectorN.y, 2));
  const cosDeg =
    (vectorAB.x * vectorN.x + vectorAB.y * vectorN.y) / (lenAB * lenN);
  if (hinder.value.x > 0) {
    return (Math.acos(cosDeg) * 180) / Math.PI;
  } else {
    return 360 - (Math.acos(cosDeg) * 180) / Math.PI;
  }
};

执行函数

通过下面的入口函数，传入参数，就完成啦

//参数为雷达测得的距离
const start = (len, ndeg, hdeg) => {
  getCameraCoordinate();
  getHinderCoordinate(len, ndeg, hdeg);
  let resultDistance = getResultDistance();
  let horizontalDeg = getHorizontalDeg(hdeg);
  let nDeg = getNDeg();
  
  //console.log('相机坐标',camera.value);
  //console.log('雷达坐标',radar.value);
  //console.log('异物坐标',hinder.value);
  
  console.log("相机距离异物的距离===>", resultDistance);
  console.log("相机的俯仰角===>", horizontalDeg);
  console.log("相机相对于正北方需要旋转的角度===>", nDeg);

};

测试

最后，就是测试环节，要怎么测试才最高效呢～

想到了一个特别简便的方式，当雷达和相机无限近的时候，雷达测得的数据直接给相机就可以使用，所以这种情况下我们输入和输出的数据应该无限接近。 所以，将radarToCamera设置为0.1

测试了几组数据，如下：

结束语

又整理了一遍思路，不知道还有没有隐藏问题，老觉得没有讲清楚，欢迎大家指正，也希望能帮助到你们～