[发明专利]四旋翼无人机空中充电子母机精准对接控制方法及系统

权利要求书

1.一种四旋翼无人机空中充电子母机精准对接控制方法，其特征在于，包括：

子机/母机将GPS位置信息发送至地面控制站，地面控制站将子机/母机的动态位置转发给母机/子机；母机/子机以子机/母机的动态位置作为输入，采用动态位置控制器逼近子机/母机，飞行至子机/母机的下方/上方；

子机在母机上方进行悬停搜索母机上的地标后进行精准降落，包括：先锁定地标进行精准跟踪，当锁定地标次数大于设定阈值后再切换到降落模式；在降落过程中，通过视觉处理模块获取水平位置信息，激光测距仪获取高度信息，分别传入水平位置控制器与高度控制器，控制子机精准降落，使得搭载在子机下方的受电板与搭载在母机上方的充电板对接；

其中，激光测距仪获取高度信息后存入设置的缓存器中，每一次调用缓存器中的数据时都与上一时刻高度信息进行对比，当差值小于设定容错阈值时才允许传入高度控制器，否则子机自主切到定高模式；高度控制器中速度控制加入限幅：

其中，V为无人机下降速度，Vsp为无人机期望速度，d为子机测得的子母间距离，d_max为第二下降阶段安全距离，取值(0.8,1.5)，V_max为降落限幅速度，取值[10,20)，V_cu为子机当前速度，V_d-min为降落安全速度，取值(1,5)，速度单位为m/s，距离单位为m。

2.根据权利要求1所述的四旋翼无人机空中充电子母机精准对接控制方法，其特征在于，所述动态位置控制器以输入的动态位置作为期望位置采用串级PID控制算法，其中子机/母机期望速度首先由母机/子机实时位置与子机/母机实际位置做差乘以比例系数得到vel_sp，再结合子母机间水平距离d确定：

其中，vel_zi为子机/母机的速度，为飞机速度放大参数，取值范围(1,2]，为子母机安全距离阈值，取值范围[3,6]，单位为m，为飞机安全速度阈值，取值范围(0,5]，单位为m/s。

3.根据权利要求1所述的四旋翼无人机空中充电子母机精准对接控制方法，其特征在于，地标采用Apriltag，在子机位于母机上方后，通过子机搭载的视觉处理模块Openmv识别Apriltag，将Apriltag坐标位置实时传给飞控，引导子机跟踪地标，保持高度不变精准悬停于地标之上。

4.根据权利要求1所述的四旋翼无人机空中充电子母机精准对接控制方法，其特征在于，子机通过搭载的视觉处理模块Openmv识别地标，在Openmv传递位置信息时，水平位置控制器根据如下公式确定无人机相对于地标的位置偏移量：

其中，x、y为无人机相对于地标在横滚、俯仰轴方向上的位置偏移量，z为竖直距离，采用激光测距仪的测距数据，f为摄像头焦距，(u,v)为地标在像素坐标系中的坐标，表示图像中心点像素坐标与图像原点像素坐标在横向和纵向上的像素数差值，α、β为尺度因子，代表物理距离对应的像素点数；

将机体坐标系下的位置转换为北东地NED坐标系下的位置后判断，当x_ned、y_ned均为零时，表明子机正处于地标正上方，此时地标的像素坐标为图像中心点坐标；当x_ned或y_ned不为零时，表明无人机不位于地标正上方，通过调整飞行姿态来实现位置运动，让x_ned和y_ned的值逐渐趋向于零。