基于大疆MSDK实现的无人机视觉引导自适应降落功能
基于大疆MSDK实现的无人机视觉引导自适应降落功能
概述
最初需求:想要无人机在执行完航线任务后,一键落到一个指定的位置,简化人工控制。
实现一套完整的无人机自主降落功能,通过虚拟摇杆控制使无人机飞向指定位置,再利用视觉识别引导无人机精确降落到具体位置。本文中采用自适应降落策略,根据高度动态调整精度要求和下降速度,以实现安全、精确的降落。
核心点:
- 虚拟摇杆导航替代FlyTo功能
- 双轴(X/Y)位置偏移实时调整
- 高度自适应降落策略
- 视觉识别引导定位
- 智能避障管理
系统架构
整体流程
否
是
高于50m
20-50m
5-20m
低于5m
是
否
是
否
否
是
用户触发Return to Vehicle
获取无人机GPS位置
计算与目标点距离
启动虚拟摇杆导航
飞向目标位置 5m/s
距离小于10m?
开始自适应降落
视觉识别系统
计算X/Y偏移量
更新偏移量到ViewModel
自适应降落循环
高度分段判断
高空模式
中空模式
低空模式
极低空模式
计算调整速度和下降速度
偏移大于阈值2倍?
停止下降只调整
边调整边下降
高度小于5m?
关闭下视避障
高度小于等于0.1m?
着陆完成清理资源
技术实现思路
第一步:让无人机飞到目标位置?
问题分析
遥控器控制的无人机在执行完航线任务之后,飞到给定降落点(汽车或其他载具上)。最初的想法是使用DJI SDK提供的FlyTo功能,直接指定目标GPS坐标让无人机飞过去。但在实际测试中,发现部分机型(如M3E)并不支持FlyTo功能。
机型是否支持FlyTo功能参考文档:https://developer.dji.com/doc/mobile-sdk-tutorial/cn/tutorials/intelligent-flight.html
解决方案:虚拟摇杆导航
既然FlyTo功能不可用,那就用虚拟摇杆功能进行模拟。
思路:
- 计算当前位置到目标位置的方位角(bearing)
- 将方位角转换为速度分量(南北/东西)
- 持续发送虚拟摇杆指令,让无人机朝目标飞行
- 实时监测距离,接近目标时停止
方位角计算:
privatefuncalculateBearing(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°=正西}速度分量计算:
val bearing =calculateBearing(currentLat, currentLon, targetLat, targetLon)val bearingRad = Math.toRadians(bearing)// 使用GROUND坐标系(地面坐标系)val navParam =VirtualStickFlightControlParam().apply{ rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND verticalControlMode = VerticalControlMode.POSITION yawControlMode = YawControlMode.ANGLE rollPitchControlMode = RollPitchControlMode.VELOCITY // 将速度分解为南北和东西分量 pitch = NAVIGATION_SPEED * Math.cos(bearingRad)// 南北分量(5m/s) roll = NAVIGATION_SPEED * Math.sin(bearingRad)// 东西分量(5m/s) yaw = bearing // 让机头指向目标 verticalThrottle = targetAlt }- GROUND坐标系是绝对方向,不受无人机朝向影响
- pitch控制南北,roll控制东西。
第二步:判断何时到达目标点上方附近
持续监测距离
每100ms检查一次当前位置与目标的距离,距离小于预期值ARRIVAL_THRESHOLD,就认为无人机已到达目标点上方附近,停止导航,开始降落:
val navTask =object: Runnable {overridefunrun(){val currentLoc =getAircraftLocation()val remainingDistance =calculateDistance( currentLoc.latitude, currentLoc.longitude, targetLat, targetLon )if(remainingDistance < ARRIVAL_THRESHOLD){// 10米内// 到达目标,停止导航,开始降落 isNavigating =falsestartDynamicAdjustment()}else{// 继续飞行sendNavigationCommand() virtualStickHandler?.postDelayed(this,100)}}}第三步:精确降落到指定点
无人机虽然到了目标附近(10米内),但有以下问题:
- GPS精度有限(±3米),不够精确。
- 风力影响,有时候受风的影响,无人机会偏离。
解决方案:视觉识别+位置调整
工作原理:
- 无人机摄像头识别地面的特定图像(如二维码、标记点)
- 视觉算法计算偏移量(X轴左右,Y轴前后,Z轴距图像距离)
- 将偏移量传给无人机
- 无人机调整位置,边降落边对准
数据结构:
privatevar xOffset: Double =0.0// X轴偏移(米),正=右,负=左privatevar yOffset: Double =0.0// Y轴偏移(米),正=前,负=后privatevar zDistance: Double =0.0// Z轴距离(米),距降落点高度外部接口:
// 视觉识别系统调用这些方法更新偏移量(~1Hz)funsetXOffset(offset: Double){ xOffset = offset }funsetYOffset(offset: Double){ yOffset = offset }funsetZDistance(distance: Double){ zDistance = distance }采用自适应策略,一边降落一遍调整
关键点:
在不同的高度,我们允许的偏移量阈值不同的,高度较高的时候,偏移量就算比较大也可以下降,随着高度降低,我们允许的偏移量阈值会不断缩小(要求越来越向中间对齐)
真实偏移超出偏移量阈值的2倍就停止下降,只进行对齐调整;
真实偏移超出偏移量的1倍,就以0.1m/s的慢速一边降落一边调整;
在偏移量范围内,且高度> 20m,以0.5m/s的速度快速下降;
在偏移量范围内,且高度在5m-20m之间,以0.2m/s的速度下降;
在偏移量范围内,且高度< 5m,以0.2m/s速度下降;
实现:
// 1. 根据高度动态计算允许的误差privatefungetOffsetThreshold(altitude: Double): Double {returnwhen{ 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. 根据高度和偏移量动态计算下降速度privatefungetDescentSpeed(altitude: Double, xOffset: Double, yOffset: Double): Double {val threshold =getOffsetThreshold(altitude)returnwhen{ 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// 极低空:极慢降}}控制逻辑:
大于50m
20-50m
5-20m
小于5m
偏移大于阈值的2倍
偏移大于阈值
偏移小于阈值
否
是
获取当前高度和偏移量
高度判断
偏离阈值1m
偏离阈值0.5m
偏离阈值0.3m
偏离阈值0.2m
偏移判断
停止下降,只调整
慢降0.1m/s并且调整
快降并且微调
发送虚拟摇杆指令
高度小于等于0.1m?
着陆完成
第四步:处理避障,降落后停桨。
问题:下视避障会阻止降落
无人机的下视避障系统会将地面识别为障碍物,在接近地面时自动停止下降,我们在高度为5m的时候关闭下视避障,落到地面后调用KeyStartAutoLanding进行停桨。
参考文档:https://sdk-forum.dji.net/hc/zh-cn/articles/14578693771033-如何使用虚拟摇杆降落
低空时关闭下视避障
var downwardObstacleDisabled =false//确保关闭下视避障操作只成功执行一次// 高度<5m时关闭下视避障if(currentAltitude <=5.0&&!downwardObstacleDisabled){ downwardObstacleDisabled =truesetObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)}//关闭下视避障调用方法privatefunsetObstacleAvoidanceEnable(enabled: Boolean,direction: PerceptionDirection){if(direction ==null){ Log.e("Perception","方向参数为空,无法设置避障")return} PerceptionManager.getInstance().setObstacleAvoidanceEnabled(//调用大疆MSDK方法关闭下视避障 enabled, direction,object: CommonCallbacks.CompletionCallback{overridefunonSuccess(){ toastResult?.postValue(DJIToastResult.success("成功设置【${direction.name}】方向的避障为:${if(enabled)"开启"else"关闭"}")) Log.i("Perception","成功设置【${direction.name}】方向的避障为:${if(enabled)"开启"else"关闭"}")}overridefunonFailure(error: IDJIError){ downwardObstacleDisabled =false toastResult?.postValue(DJIToastResult.failed("设置【${direction.name}】方向的避障失败:$error")) Log.e("Perception","设置【${direction.name}】方向的避障失败:$error")}})}第五步:降落循环完整逻辑
privatefunstartDynamicAdjustment(){ isAdjusting =true virtualStickHandler =Handler(Looper.getMainLooper())val adjustTask =object: Runnable {overridefunrun(){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 =truesetObstacleAvoidanceEnable(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 }else0.0 pitch =if(currentYOffsetAbs > offsetThreshold){if(yOffset >0) ADJUSTMENT_SPEED else-ADJUSTMENT_SPEED }else0.0// 垂直下降 verticalThrottle =-descentSpeed }// 6. 发送指令 VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam)// 7. 100ms后再次执行(10Hz) virtualStickHandler?.postDelayed(this,100)}} virtualStickHandler?.post(adjustTask)}以上,就实现了一整套视觉引导的自适应降落方案
安全注意事项
WARNING
- 必须在空旷、安全环境测试
- 建议先用DJI模拟器测试
- 视觉识别必须持续更新(~1Hz)
- 准备好随时手动接管
代码
/** * One-key return to vehicle function (using Virtual Stick instead of FlyTo) * 1. Get aircraft current location * 2. Calculate distance to vehicle using Haversine formula * 3. If distance > 500m, reject with error * 4. Use Virtual Stick to navigate to vehicle location * 5. Switch to precision adjustment when close enough */funreturnToVehicle(callback: CommonCallbacks.CompletionCallback){// Get aircraft current locationval aircraftLocation =getAircraftLocation()if(aircraftLocation ==null||!isLocationValid(aircraftLocation.latitude, aircraftLocation.longitude)){ callback.onFailure(DJICommonError.FACTORY.build("无法获取无人机位置信息"))return}// Vehicle coordinates (hardcoded for now, will be replaced with API later)// TODO: Replace with actual vehicle GPS coordinates from APIval vehicleLatitude =22.579// Example coordinatesval vehicleLongitude =113.941// Example coordinates// Calculate distance using Haversine formulaval distance =calculateDistance( aircraftLocation.latitude, aircraftLocation.longitude, vehicleLatitude, vehicleLongitude )// Distance validation: reject if > 500mif(distance >500){ callback.onFailure(DJICommonError.FACTORY.build("距离过远: ${String.format("%.2f", distance)}m, 超出 500m 限制"))return}// Start virtual stick navigation to vehicle location toastResult?.postValue(DJIToastResult.success("开始飞向车辆位置"))//TODO 这个targetAlt需要后期经过计算算出来。navigateToTarget(vehicleLatitude, vehicleLongitude,100.0, callback)}/** * Navigate to target location using Virtual Stick */privatefunnavigateToTarget( targetLat: Double, targetLon: Double, targetAlt: Double, callback: CommonCallbacks.CompletionCallback ){ VirtualStickManager.getInstance().enableVirtualStick(object: CommonCallbacks.CompletionCallback{overridefunonSuccess(){ VirtualStickManager.getInstance().setVirtualStickAdvancedModeEnabled(true) isNavigating =truestartNavigation(targetLat, targetLon, targetAlt, callback)}overridefunonFailure(error: IDJIError){ callback.onFailure(error)}})}/** * Start navigation loop using Virtual Stick */privatefunstartNavigation( targetLat: Double, targetLon: Double, targetAlt: Double, callback: CommonCallbacks.CompletionCallback ){ virtualStickHandler =Handler(Looper.getMainLooper())val navTask =object: Runnable {overridefunrun(){if(!isNavigating){return}val currentLoc =getAircraftLocation()if(currentLoc ==null){ virtualStickHandler?.postDelayed(this,100)return}// Calculate remaining distanceval remainingDistance =calculateDistance( currentLoc.latitude, currentLoc.longitude, targetLat, targetLon )println("targetLat:"+targetLat+" targetLon:"+targetLon+" currentLoc.latitude:"+currentLoc.latitude+" currentLoc.longitude:"+currentLoc.longitude+" remainingDistance:"+remainingDistance)// Check if arrivedif(remainingDistance < ARRIVAL_THRESHOLD){// Arrived at target, stop navigation isNavigating =false virtualStickHandler?.removeCallbacksAndMessages(null) callback.onSuccess() toastResult?.postValue(DJIToastResult.success("已到达车辆位置,开始精确定位"))//开始调节云台角度,俯仰角为-90°,旋转时间1sstartGimbalAngleRotation(GimbalAngleRotationMode.ABSOLUTE_ANGLE,-90.0,0.0,0.0,1.0)// Start precision adjustmentstartDynamicAdjustment()}else{// Continue navigationval bearing =calculateBearing( currentLoc.latitude, currentLoc.longitude, targetLat, targetLon )val navParam =VirtualStickFlightControlParam().apply{ rollPitchCoordinateSystem = FlightCoordinateSystem.GROUND // Use ground coordinate system verticalControlMode = VerticalControlMode.POSITION yawControlMode = YawControlMode.ANGLE rollPitchControlMode = RollPitchControlMode.VELOCITY // Calculate velocity components based on bearingval bearingRad = Math.toRadians(bearing) pitch = NAVIGATION_SPEED * Math.sin(bearingRad)// North-South component roll = NAVIGATION_SPEED * Math.cos(bearingRad)// East-West component yaw = bearing // Point towards target verticalThrottle = targetAlt // Target altitude} VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(navParam) virtualStickHandler?.postDelayed(this,100)}}} virtualStickHandler?.post(navTask)}funstartGimbalAngleRotation(mode: GimbalAngleRotationMode,pitch: Double,yaw: Double,roll: Double,duration: Double){val rotation =GimbalAngleRotation().apply{setMode(mode)setPitch(pitch)setYaw(yaw)setRoll(roll)setDuration(duration)} KeyManager.getInstance().performAction( KeyTools.createKey(GimbalKey.KeyRotateByAngle), rotation,object: CommonCallbacks.CompletionCallbackWithParam<EmptyMsg>{overridefunonSuccess(result: EmptyMsg?){ toastResult?.postValue(DJIToastResult.success("云台旋转成功")) Log.i("Gimbal","云台旋转成功:yaw:${rotation.yaw},pitch:${rotation.pitch},roll:${rotation.roll}")}overridefunonFailure(error: IDJIError){ toastResult?.postValue(DJIToastResult.failed("云台旋转失败,$error")) Log.e("Gimbal","云台旋转失败,$error")}})}/** * Start dynamic position adjustment loop with adaptive descent * Adjusts position while descending, with stricter requirements at lower altitudes */privatefunstartDynamicAdjustment(){ isAdjusting =true virtualStickHandler =Handler(Looper.getMainLooper())// Send adjustment commands at 10Hzval adjustTask =object: Runnable {overridefunrun(){if(!isAdjusting){return}// TODO 获取脚本检测出的z轴距离val currentAltitude = FlightControllerKey.KeyAltitude.create().get(0.0)val currentXOffsetAbs = Math.abs(xOffset)val currentYOffsetAbs = Math.abs(yOffset)//关闭降落保护,下视避障失效if(currentAltitude <=5&&!downwardObstacleDisabled){ downwardObstacleDisabled =truesetObstacleAvoidanceEnable(false, PerceptionDirection.DOWNWARD)}// 检查是否落地if(currentAltitude <=0.1){stopLanding()return}// Get adaptive thresholds based on altitudeval offsetThreshold =getOffsetThreshold(currentAltitude)val descentSpeed =getDescentSpeed(currentAltitude, currentXOffsetAbs,currentYOffsetAbs)// Log for debuggingprintln("自动调整 - 高度:%.2fm, x偏移:%.2fm,y偏移:%.2fm, 阈值:%.2fm, 下降速度:%.2fm/s".format( currentAltitude, currentXOffsetAbs,currentYOffsetAbs,offsetThreshold, descentSpeed ))// Calculate adjustment parametersval adjustParam =VirtualStickFlightControlParam().apply{ rollPitchCoordinateSystem = FlightCoordinateSystem.BODY verticalControlMode = VerticalControlMode.VELOCITY yawControlMode = YawControlMode.ANGULAR_VELOCITY rollPitchControlMode = RollPitchControlMode.ANGLE // Calculate roll value based on offset// Positive offset (need to move forward) -> positive roll// Negative offset (need to move backward) -> negative rollif(currentXOffsetAbs > offsetThreshold){// Need adjustment roll =if(xOffset >0) ADJUSTMENT_SPEED else-ADJUSTMENT_SPEED }else{// Within threshold, no adjustment needed roll =0.0}if(currentYOffsetAbs > offsetThreshold){ pitch =if(yOffset >0) ADJUSTMENT_SPEED else-ADJUSTMENT_SPEED }else{ pitch =0.0} yaw =0.0 verticalThrottle =-descentSpeed // Descend at adaptive speed} VirtualStickManager.getInstance().sendVirtualStickAdvancedParam(adjustParam) virtualStickHandler?.postDelayed(this,100)}} virtualStickHandler?.post(adjustTask) toastResult?.postValue(DJIToastResult.success("开始动态位置调整"))}/** * Stop landing and cleanup */privatefunstopLanding(){ virtualStickHandler?.removeCallbacksAndMessages(null)//调用KeyStartAutoLanding进行停桨 FlightControllerKey.KeyStartAutoLanding.create().action({ toastResult?.postValue(DJIToastResult.success("桨叶动力关闭")) Log.i("stopLanding","桨叶动力关闭成功")},{ toastResult?.postValue(DJIToastResult.failed("桨叶动力关闭失败")) Log.i("stopLanding","桨叶动力关闭失败!!")})cleanupVirtualStick() toastResult?.postValue(DJIToastResult.success("降落完成"))}/** * Get offset threshold based on current altitude * Higher altitude allows larger offset, lower altitude requires stricter precision */privatefungetOffsetThreshold(altitude: Double): Double {returnwhen{ altitude > HIGH_ALTITUDE ->1.0// High altitude: allow 1m offset altitude > MID_ALTITUDE ->0.5// Mid altitude: allow 0.5m offset altitude > LOW_ALTITUDE ->0.4// Low altitude: allow 0.3m offsetelse->0.3// Very low altitude: require 0.2m precision}}/** * Get descent speed based on current altitude and offset * Larger offset or lower altitude results in slower descent */privatefungetDescentSpeed(altitude: Double, xOffset: Double,yOffset: Double): Double {val threshold =getOffsetThreshold(altitude)returnwhen{ xOffset > threshold *2|| yOffset > threshold *2->0.0// Offset too large: stop descending xOffset > threshold || yOffset > threshold ->0.1// Offset large: slow descent altitude > MID_ALTITUDE ->0.5// Mid-high altitude: fast descent altitude > LOW_ALTITUDE ->0.2// Low altitude: slow descentelse->0.1// Very low altitude: very slow descent}}