基于大疆 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
}

到达检测 每 100ms 检查一次距离，一旦小于设定阈值（如 10 米），立即停止导航循环，切换至降落逻辑。

val navTask = object : Runnable {
    override fun run() {
        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)
        }
    }
}

2. 视觉引导与自适应降落

GPS 精度通常在±3 米左右，且受风力影响较大，无法满足厘米级降落需求。因此引入视觉识别系统，实时反馈 X/Y 轴偏移量和 Z 轴高度。

数据结构 视觉算法需定期更新以下变量（约 1Hz）：

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

高度自适应策略 这是本方案的核心。不同高度对精度的容忍度不同：

• 高空 (>50m)：允许 1 米误差，快速下降 (0.5m/s)。
• 中空 (20-50m)：允许 0.5 米误差，中速下降。
• 低空 (5-20m)：允许 0.3 米误差，慢速下降 (0.2m/s)。
• 极低空 (<5m)：要求 0.2 米精度，极慢速下降。

若实际偏移超过阈值的 2 倍，说明偏差过大，此时必须停止下降，仅进行水平校正，避免侧翻风险。

// 根据高度动态计算允许的误差
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                                             // 极低空：极慢降
    }
}

3. 避障处理与着陆收尾

无人机的下视避障系统在接近地面时容易将地面误判为障碍物而强制悬停。我们在高度降至 5 米时主动关闭下视避障，确保能顺利触地。落地后调用 KeyStartAutoLanding 停止电机。

var downwardObstacleDisabled = false

// 高度<5m 时关闭下视避障
if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
    downwardObstacleDisabled = true
    setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
}

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")
            }
        }
    )
}

4. 完整控制循环

整合上述逻辑，主循环负责高频（10Hz）读取传感器数据，计算控制参数，并下发虚拟摇杆指令。

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)
}

安全注意事项

1. 测试环境：必须在空旷、无遮挡的安全环境下进行测试，建议先在模拟器中验证逻辑。
2. 视觉依赖：视觉识别系统需保持稳定更新（约 1Hz），信号丢失时应具备应急悬停机制。
3. 人工接管：始终保留手动接管权限，遇到异常天气或突发状况立即切断自动逻辑。
4. 避障策略：关闭下视避障前务必确认下方无障碍物，防止误伤地面设施。

参考文档：DJI Mobile SDK 官方文档