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

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

概述

最初需求：想要无人机在执行完航线任务后，一键落到一个指定的位置，简化人工控制。
实现一套完整的无人机自主降落功能,通过虚拟摇杆控制使无人机飞向指定位置，再利用视觉识别引导无人机精确降落到具体位置。本文中采用自适应降落策略,根据高度动态调整精度要求和下降速度,以实现安全、精确的降落。
核心点:

• 虚拟摇杆导航替代FlyTo功能
  
• 双轴(X/Y)位置偏移实时调整
  
• 高度自适应降落策略
  
• 视觉识别引导定位
  
• 智能避障管理
  

系统架构

整体流程

graph TD    A[用户触发Return to Vehicle] --> B[获取无人机GPS位置]    B --> C[计算与目标点距离]    C --> D[启动虚拟摇杆导航]    D --> E[飞向目标位置 5m/s]    E --> F{距离小于10m?}    F -->|否| E    F -->|是| G[开始自适应降落]    G --> H[视觉识别系统]    H --> I[计算X/Y偏移量]    I --> J[更新偏移量到ViewModel]    J --> K[自适应降落循环]    K --> L{高度分段判断}    L -->|高于50m| M[高空模式]    L -->|20-50m| N[中空模式]    L -->|5-20m| O[低空模式]    L -->|低于5m| P[极低空模式]    M --> Q[计算调整速度和下降速度]    N --> Q    O --> Q    P --> Q    Q --> R{偏移大于阈值2倍?}    R -->|是| S[停止下降只调整]    R -->|否| T[边调整边下降]    S --> U{高度小于5m?}    T --> U    U -->|是| V[关闭下视避障]    U -->|否| K    V --> W{高度小于等于0.1m?}    W -->|否| K    W -->|是| X[着陆完成清理资源]技术实现思路

第一步:让无人机飞到目标位置?

问题分析

遥控器控制的无人机在执行完航线任务之后，飞到给定降落点（汽车或其他载具上）。最初的想法是使用DJI SDK提供的FlyTo功能,直接指定目标GPS坐标让无人机飞过去。但在实际测试中，发现部分机型（如M3E）并不支持FlyTo功能。
机型是否支持FlyTo功能参考文档：https://developer.dji.com/doc/mobile-sdk-tutorial/cn/tutorials/intelligent-flight.html
解决方案:虚拟摇杆导航

既然FlyTo功能不可用,那就用虚拟摇杆功能进行模拟。
思路:

• 计算当前位置到目标位置的方位角(bearing)
  
• 将方位角转换为速度分量(南北/东西)
  
• 持续发送虚拟摇杆指令,让无人机朝目标飞行
  
• 实时监测距离,接近目标时停止
  

方位角计算:

1. private fun calculateBearing(latA: Double, lonA: Double, latB: Double, lonB: Double): Double {
2.     val lat1 = Math.toRadians(latA)
3.     val lat2 = Math.toRadians(latB)
4.     val dLon = Math.toRadians(lonB - lonA)
5.    
6.     val y = Math.sin(dLon) * Math.cos(lat2)
7.     val x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1) * Math.cos(lat2) * Math.cos(dLon)
8.    
9.     var bearing = Math.toDegrees(Math.atan2(y, x))
10.     bearing = (bearing + 360) % 360  // 归一化到0-360度
11.    
12.     return bearing  // 0°=正北, 90°=正东, 180°=正南, 270°=正西
13. }

复制代码

速度分量计算:

1. val bearing = calculateBearing(currentLat, currentLon, targetLat, targetLon)
2. val bearingRad = Math.toRadians(bearing)
4. // 使用GROUND坐标系(地面坐标系)
5. val navParam = VirtualStickFlightControlParam().apply {
6.     rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND
7.     verticalControlMode = VerticalControlMode.POSITION
8.     yawControlMode = YawControlMode.ANGLE
9.     rollPitchControlMode = RollPitchControlMode.VELOCITY
10.    
11.     // 将速度分解为南北和东西分量
12.     pitch = NAVIGATION_SPEED * Math.cos(bearingRad)  // 南北分量(5m/s)
13.     roll = NAVIGATION_SPEED * Math.sin(bearingRad)   // 东西分量(5m/s)
14.     yaw = bearing  // 让机头指向目标
15.     verticalThrottle = targetAlt
16. }

复制代码

• GROUND坐标系是绝对方向,不受无人机朝向影响
  
• pitch控制南北,roll控制东西。
  

虚拟摇杆参数含义：https://developer.dji.com/doc/mobile-sdk-tutorial/cn/basic-introduction/basic-concepts/flight-controller.html#虚拟摇杆
第二步:判断何时到达目标点上方附近

持续监测距离

每100ms检查一次当前位置与目标的距离，距离小于预期值ARRIVAL_THRESHOLD,就认为无人机已到达目标点上方附近，停止导航，开始降落:

1. val navTask = object : Runnable {
2.     override fun run() {
3.         val currentLoc = getAircraftLocation()
4.         val remainingDistance = calculateDistance(
5.             currentLoc.latitude, currentLoc.longitude,
6.             targetLat, targetLon
7.         )
8.         
9.         if (remainingDistance < ARRIVAL_THRESHOLD) {  // 10米内
10.             // 到达目标,停止导航,开始降落
11.             isNavigating = false
12.             startDynamicAdjustment()
13.         } else {
14.             // 继续飞行
15.             sendNavigationCommand()
16.             virtualStickHandler?.postDelayed(this, 100)
17.         }
18.     }
19. }

复制代码

第三步:精确降落到指定点

无人机虽然到了目标附近(10米内),但有以下问题：

• GPS精度有限(±3米),不够精确。
  
• 风力影响,有时候受风的影响，无人机会偏离。
  

解决方案:视觉识别+位置调整

工作原理:

• 无人机摄像头识别地面的特定图像(如二维码、标记点)
  
• 视觉算法计算偏移量(X轴左右,Y轴前后,Z轴距图像距离)
  
• 将偏移量传给无人机
  
• 无人机调整位置,边降落边对准
  

数据结构:

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

复制代码

外部接口:

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

复制代码

采用自适应策略，一边降落一遍调整

关键点:
在不同的高度，我们允许的偏移量阈值不同的，高度较高的时候，偏移量就算比较大也可以下降，随着高度降低，我们允许的偏移量阈值会不断缩小（要求越来越向中间对齐）
真实偏移超出偏移量阈值的2倍就停止下降，只进行对齐调整；
真实偏移超出偏移量的1倍，就以0.1m/s的慢速一边降落一边调整；
在偏移量范围内，且高度> 20m，以0.5m/s的速度快速下降；
在偏移量范围内，且高度在5m-20m之间，以0.2m/s的速度下降；
在偏移量范围内，且高度< 5m，以0.2m/s速度下降；
实现:

1. // 1. 根据高度动态计算允许的误差
2. private fun getOffsetThreshold(altitude: Double): Double {
3.     return when {
4.         altitude > 50.0 -> 1.0   // 高空:允许1米偏移误差
5.         altitude > 20.0 -> 0.5   // 中空:允许0.5米偏移误差
6.         altitude > 5.0  -> 0.3   // 低空:允许0.3米偏移误差
7.         else -> 0.2              // 极低空:要求0.2米精度
8.     }
9. }
11. // 2. 根据高度和偏移量动态计算下降速度
12. private fun getDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {
13.     val threshold = getOffsetThreshold(altitude)
14.     return when {
15.         xOffset > threshold * 2 || yOffset > threshold * 2 -> 0.0  // 偏移太大:停止下降
16.         xOffset > threshold || yOffset > threshold -> 0.1          // 偏移较大:慢降
17.         altitude > 20.0 -> 0.5                                     // 中高空:快降
18.         altitude > 5.0  -> 0.2                                     // 低空:慢降
19.         else -> 0.2                                                // 极低空:极慢降
20.     }
21. }

复制代码

控制逻辑:
graph TD    A[获取当前高度和偏移量] --> B{高度判断}    B -->|大于50m| C[偏离阈值1m]    B -->|20-50m| D[偏离阈值0.5m]    B -->|5-20m| E[偏离阈值0.3m]    B -->|小于5m| F[偏离阈值0.2m]    C --> G{偏移判断}    D --> G    E --> G    F --> G    G -->|偏移大于阈值的2倍| H[停止下降,只调整]    G -->|偏移大于阈值| I[慢降0.1m/s并且调整]    G -->|偏移小于阈值| J[快降并且微调]    H --> K[发送虚拟摇杆指令]    I --> K    J --> K    K --> L{高度小于等于0.1m?}    L -->|否| A    L -->|是| M[着陆完成]第四步:处理避障，降落后停桨。

问题:下视避障会阻止降落

无人机的下视避障系统会将地面识别为障碍物,在接近地面时自动停止下降，我们在高度为5m的时候关闭下视避障，落到地面后调用KeyStartAutoLanding进行停桨。
参考文档： https://sdk-forum.dji.net/hc/zh-cn/articles/14578693771033-如何使用虚拟摇杆降落
低空时关闭下视避障

1. var downwardObstacleDisabled = false  //确保关闭下视避障操作只成功执行一次
3. // 高度<5m时关闭下视避障
4. if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
5.     downwardObstacleDisabled = true
6.     setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
7. }
11. //关闭下视避障调用方法
12. private fun setObstacleAvoidanceEnable(enabled: Boolean,direction: PerceptionDirection){
13.         if (direction == null) {
14.             Log.e("Perception", "方向参数为空，无法设置避障")
15.             return
16.         }
17.         PerceptionManager.getInstance().setObstacleAvoidanceEnabled(    //调用大疆MSDK方法关闭下视避障
18.             enabled,
19.             direction,
20.             object : CommonCallbacks.CompletionCallback {
21.                 override fun onSuccess() {
22.                     toastResult?.postValue(DJIToastResult.success(
23.                         "成功设置【${direction.name}】方向的避障为：${if (enabled) "开启" else "关闭"}")
24.                     )
25.                     Log.i(
26.                         "Perception",
27.                         "成功设置【${direction.name}】方向的避障为：${if (enabled) "开启" else "关闭"}"
28.                     )
29.                 }
30.                 override fun onFailure(error: IDJIError) {
31.                     downwardObstacleDisabled = false
32.                     toastResult?.postValue(DJIToastResult.failed(
33.                         "设置【${direction.name}】方向的避障失败：$error"
34.                     ))
35.                     Log.e(
36.                         "Perception",
37.                         "设置【${direction.name}】方向的避障失败：$error"
38.                     )
39.                 }
40.             }
41.         )
42.     }

复制代码

以上，就实现了一整套视觉引导的自适应降落方案
安全注意事项

WARNING

• 必须在空旷、安全环境测试
  
• 建议先用DJI模拟器测试
  
• 视觉识别必须持续更新(~1Hz)
  
• 准备好随时手动接管
  

代码

1. private fun startDynamicAdjustment() {
2.     isAdjusting = true
3.     virtualStickHandler = Handler(Looper.getMainLooper())
4.    
5.     val adjustTask = object : Runnable {
6.         override fun run() {
7.             if (!isAdjusting) return
8.             
9.             // 1. 获取当前状态
10.             val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)
11.             val currentXOffsetAbs = Math.abs(xOffset)
12.             val currentYOffsetAbs = Math.abs(yOffset)
13.             
14.             // 2. 检查是否着陆
15.             if (currentAltitude <= 0.1) {
16.                 stopLanding()
17.                 return
18.             }
19.             
20.             // 3. 低空时关闭下视避障
21.             if (currentAltitude <= 5.0 && !downwardObstacleDisabled) {
22.                 downwardObstacleDisabled = true
23.                 setObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)
24.             }
25.             
26.             // 4. 计算自适应参数
27.             val offsetThreshold = getOffsetThreshold(currentAltitude)
28.             val descentSpeed = getDescentSpeed(currentAltitude, currentXOffsetAbs, currentYOffsetAbs)
29.             
30.             // 5. 构建虚拟摇杆指令
31.             val adjustParam = VirtualStickFlightControlParam().apply {
32.                 rollPitchCoordinateSystem = FlightCoordinateSystem.BODY
33.                 verticalControlMode = VerticalControlMode.VELOCITY
34.                 rollPitchControlMode = RollPitchControlMode.VELOCITY
35.                
36.                 // 水平调整
37.                 roll = if (currentXOffsetAbs > offsetThreshold) {
38.                     if (xOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
39.                 } else 0.0
40.                
41.                 pitch = if (currentYOffsetAbs > offsetThreshold) {
42.                     if (yOffset > 0) ADJUSTMENT_SPEED else -ADJUSTMENT_SPEED
43.                 } else 0.0
44.                
45.                 // 垂直下降
46.                 verticalThrottle = -descentSpeed
47.             }
48.             
49.             // 6. 发送指令
50.             VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)
51.             
52.             // 7. 100ms后再次执行(10Hz)
53.             virtualStickHandler?.postDelayed(this, 100)
54.         }
55.     }
56.    
57.     virtualStickHandler?.post(adjustTask)
58. }

复制代码

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！