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

发送导航指令

使用 GROUND 坐标系（绝对方向），不受机头朝向影响。pitch 控制南北，roll 控制东西。

val bearing = calculateBearing(currentLat, currentLon, targetLat, targetLon)
val bearingRad = Math.toRadians(bearing)

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
}
VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(navParam)

2. 到达判定与切换

每 100ms 监测一次当前位置与目标的距离。当距离小于阈值（如 10 米）时，停止导航循环，切换到视觉引导降落模式。

val navTask = object : Runnable {
    override fun run() {
        if (!isNavigating) return
        val currentLoc = getAircraftLocation()
        val remainingDistance = calculateDistance(
            currentLoc.latitude, currentLoc.longitude,
            targetLat, targetLon
        )

        if (remainingDistance < ARRIVAL_THRESHOLD) {
            isNavigating = false
            startDynamicAdjustment() // 切换至降落模式
        } else {
            sendNavigationCommand()
            virtualStickHandler?.postDelayed(this, 100)
        }
    }
}

3. 自适应降落策略

这是核心部分。GPS 精度通常在±3 米左右，无法满足精准着陆需求。我们需要依赖视觉系统提供的 X/Y/Z 偏移量，并根据高度动态调整下降速度和容错率。

数据结构

private var xOffset: Double = 0.0 // X 轴偏移 (米), 正=右，负=左
private var yOffset: Double = 0.0 // Y 轴偏移 (米), 正=前，负=后
private var zDistance: Double = 0.0 // Z 轴距离 (米)，距降落点高度

高度分段逻辑

随着高度降低，我们对精度的要求越来越高，允许的偏移阈值逐渐缩小。

// 根据高度动态计算允许的误差
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 米精度
    }
}

// 根据高度和偏移量动态计算下降速度
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 频率运行，根据当前状态构建虚拟摇杆参数。注意在极低空（<5m）时关闭下视避障，防止无人机误判地面为障碍物而悬停。

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 停桨。

var downwardObstacleDisabled = false

private fun setObstacleAvoidanceEnable(enabled: Boolean, direction: PerceptionDirection) {
    if (direction == null) 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", "设置失败：$error")
            }
        }
    )
}

private fun stopLanding() {
    virtualStickHandler?.removeCallbacksAndMessages(null)
    // 调用 KeyStartAutoLanding 进行停桨
    FlightControllerKey.KeyStartAutoLanding.create().action(
        { Log.i("stopLanding", "桨叶动力关闭成功") },
        { Log.e("stopLanding", "桨叶动力关闭失败") }
    )
    cleanupVirtualStick()
}

安全注意事项

1. 环境测试：必须在空旷、无遮挡的安全环境进行测试。
2. 模拟器验证：建议先在 DJI 模拟器中验证逻辑，避免炸机。
3. 视觉反馈：视觉识别系统需保持高频率更新（约 1Hz），否则会导致位置漂移。
4. 人工接管：始终保留手动接管权限，一旦异常立即切断自动逻辑。

提示：代码中的 NAVIGATION_SPEED、ADJUSTMENT_SPEED 等常量需根据实际机型性能调试确定。