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

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

概述

在航线任务结束后，让无人机自动降落到指定位置（如汽车顶部）是常见的自动化需求。虽然 DJI SDK 提供了 FlyTo 功能，但部分机型（如 M3E）并不支持直接通过 GPS 坐标飞行。此外，GPS 精度有限且受风力影响，单纯依靠坐标无法实现精准着陆。

本方案采用虚拟摇杆导航替代 FlyTo，结合视觉识别系统进行末端微调，并引入高度自适应策略动态调整下降速度与对齐精度，确保从高空巡航到低空定点的安全着陆。

核心思路

• 虚拟摇杆导航：模拟遥控器指令，解决不支持 FlyTo 的机型问题。
• 双轴偏移修正：实时计算 X/Y 轴偏差，控制无人机对准目标。
• 高度分段策略：不同高度区间设定不同的允许误差和下降速度。
• 避障管理：低空自动关闭下视避障，防止误判地面为障碍物。

---

系统架构与流程

整体逻辑分为三个阶段：远程导航、接近判断、精细降落。流程图如下：

graph TD
    A[用户触发返航] --> B[获取当前 GPS]
    B --> C{距离 > 10m?}
    C -- 是 --> D[虚拟摇杆导航飞向目标]
    D --> C
    C -- 否 --> E[启动自适应降落]
    E --> F{高度判断}
    F -->|>50m| G[高空模式：快降]
    F -->|20-50m| H[中空模式：中速]
    F -->|5-20m| I[低空模式：慢速]
    F -->|<5m| J[极低空模式：极慢/停]
    G --> K[视觉识别计算偏移]
    H --> K
    I --> K
    J --> K
    K --> L{偏移量检查}
    L -- 超出阈值*2 --> M[停止下降，仅水平调整]
    L -- 超出阈值 --> N[慢速下降 + 调整]
    L -- 正常范围内 --> O[按高度速度下降]
    M --> P{高度<=0.1m?}
    N --> P
    O --> P
    P -- 是 --> Q[着陆完成，停桨]
    P -- 否 --> F

---

技术实现细节

1. 虚拟摇杆导航替代 FlyTo

由于部分机型不支持 FlyTo，我们通过计算方位角，将目标方向分解为虚拟摇杆的速度分量（Pitch/Roll），持续发送指令使无人机朝目标飞行。

方位角计算

private fun calculateBearing(latA: Double, lonA: Double, latB: Double, lonB: Double): Double {
    val lat1 = Math.toRadians(latA)
    val lat2 = Math.toRadians(latB)
    val dLon = Math.toRadians(lonB - lonA)
    val y = Math.sin(dLon) * Math.cos(lat2)
    val x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon)
    var bearing = Math.toDegrees(Math.atan2(y, x))
    bearing = (bearing + 360) % 360 // 归一化到 0-360 度
    return bearing // 0°=正北，90°=正东，180°=正南，270°=正西
}

发送导航指令

使用 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 等常量需根据实际机型性能调试确定。