基于大疆 MSDK 的无人机视觉引导自适应降落实现

在设置虚拟摇杆参数时，使用 GROUND 坐标系（绝对方向），不受无人机朝向影响：

val navParam = VirtualStickFlightControlParam().apply {
    rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND
    verticalControlMode = VerticalControlMode.POSITION
    yawControlMode = YawControlMode.ANGLE
    rollPitchControlMode = RollPitchControlMode.VELOCITY
    
    // 将速度分解为南北和东西分量
    pitch = NAVIGATION_SPEED * Math.cos(bearingRad) // 南北分量
    roll = NAVIGATION_SPEED * Math.sin(bearingRad) // 东西分量
    yaw = bearing // 让机头指向目标
    verticalThrottle = targetAlt
}

2. 到达判定与视觉接入

每 100ms 检查一次当前位置与目标的距离。当剩余距离小于阈值（例如 10 米）时，认为已到达目标点上方，停止导航，启动视觉调整循环。

视觉系统通过外部接口实时提供偏移数据：

// 视觉识别系统调用这些方法更新偏移量 (~1Hz)
fun setXOffset(offset: Double) { xOffset = offset }
fun setYOffset(offset: Double) { yOffset = offset }
fun setZDistance(distance: Double) { zDistance = distance }

3. 自适应降落核心逻辑

这是本方案的关键。在不同高度，我们对偏移量的容忍度和下降速度要求不同。高度越高，容错越大；高度越低，要求越严。

阈值与速度计算：

// 1. 根据高度动态计算允许的误差
private fun getOffsetThreshold(altitude: Double): Double {
    return when {
        altitude > 50.0 -> 1.0      // 高空：允许 1 米偏移误差
        altitude > 20.0 -> 0.5      // 中空：允许 0.5 米偏移误差
        altitude > 5.0 -> 0.3       // 低空：允许 0.3 米偏移误差
        else -> 0.2                 // 极低空：要求 0.2 米精度
    }
}

// 2. 根据高度和偏移量动态计算下降速度
private fun getDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {
    val threshold = getOffsetThreshold(altitude)
    return when {
        xOffset > threshold * 2 || yOffset > threshold * 2 -> 0.0 // 偏移太大：停止下降
        xOffset > threshold || yOffset > threshold -> 0.1         // 偏移较大：慢降
        altitude > 20.0 -> 0.5                                    // 中高空：快降
        altitude > 5.0 -> 0.2                                     // 低空：慢降
        else -> 0.2                                               // 极低空：极慢降
    }
}

控制循环：

主循环以 10Hz 频率运行，获取当前状态，计算调整参数，并发送虚拟摇杆指令。如果偏移量超过阈值的 2 倍，则只进行水平调整，暂停下降，直到对准为止。

private fun startDynamicAdjustment() {
    isAdjusting = true
    virtualStickHandler = Handler(Looper.getMainLooper())
    val adjustTask = object : Runnable {
        override fun run() {
            if (!isAdjusting) return
            
            // 1. 获取当前状态
            val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
            val currentXOffsetAbs = Math.abs(xOffset)
            val currentYOffsetAbs = Math.abs(yOffset)
            
            // 2. 检查是否着陆
            if (currentAltitude <= 0.1) {
                stopLanding()
                return
            }
            
            // 3. 低空时关闭下视避障
            if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
                downwardObstacleDisabled = true
                setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
            }
            
            // 4. 计算自适应参数
            val offsetThreshold = getOffsetThreshold(currentAltitude)
            val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
            
            // 5. 构建虚拟摇杆指令
            val adjustParam = VirtualStickFlightControlParam().apply {
                rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
                verticalControlMode = VerticalControlMode.VELOCITY
                rollPitchControlMode = RollPitchControlMode.VELOCITY
                
                // 水平调整
                roll = if (currentXOffsetAbs > offsetThreshold) {
                    if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else 0.0
                
                pitch = if (currentYOffsetAbs > offsetThreshold) {
                    if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
                } else 0.0
                
                // 垂直下降
                verticalThrottle = -descentSpeed
            }
            
            // 6. 发送指令
            VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
            
            // 7. 100ms 后再次执行
            virtualStickHandler?.postDelayed(this, 100)
        }
    }
    virtualStickHandler?.post(adjustTask)
}

4. 避障处理与停桨

无人机的下视避障系统在接近地面时会误判障碍物而停止下降。我们在高度降至 5 米时主动关闭下视避障，并在着陆完成后调用 KeyStartAutoLanding 停桨。

private fun setObstacleAvoidanceEnable(enabled: Boolean, direction: PerceptionDirection) {
    if (direction == null) {
        Log.e("Perception", "方向参数为空，无法设置避障")
        return
    }
    PerceptionManager.getInstance().setObstacleAvoidanceEnabled(
        enabled, direction,
        object : CommonCallbacks.CompletionCallback {
            override fun onSuccess() {
                Log.i("Perception", "成功设置【${direction.name}】方向的避障为：${if (enabled) "开启" else "关闭"}")
            }
            override fun onFailure(error: IDJIError) {
                Log.e("Perception", "设置【${direction.name}】方向的避障失败：$error")
            }
        }
    )
}

安全注意事项

1. 环境测试：必须在空旷、安全的环境下进行测试，建议先用 DJI 模拟器验证逻辑。
2. 视觉稳定性：视觉识别必须持续更新（约 1Hz），避免数据滞后导致失控。
3. 人工接管：准备好随时手动接管控制权，特别是在复杂光照或风切变环境下。
4. 代码健壮性：确保所有回调都有错误处理，防止因网络或传感器异常导致无人机悬停或坠毁。

这套方案在实际项目中有效解决了多机型兼容性和降落精度问题，实现了从远距离导航到厘米级精准落地的闭环。