CN108873935A - 物流配送无人机降落的控制方法、装置、设备及存储介质 - Google Patents

Abstract

物流配送无人机降落的控制方法、装置、设备及存储介质，所述方法包括步骤：获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；若e小于设定阈值，则所述无人机由PID算法进行控制。所述装置、设备及存储介质用于实现所述方法。它能够使得无人机能快速、稳定的跟踪车载平台，并准确降落其上。

Description

物流配送无人机降落的控制方法、装置、设备及存储介质

技术领域

本发明涉及无人机算法计算领域，尤其涉及一种用于实现物流配送无人机自主降落的控制方法、装置、设备及存储介质。

背景技术

随着电商平台的兴起，快递行业发展如火如荼，人工配送快递已愈加难以满足行业发展的需求。因此，无人快递车、物流配送无人机应运而生。关于如何把无人快递车与物流配送无人机结合起来组成车载无人机物流配送系统，以进一步提升配送效率、降低配送成本开始成为当前的重要研究方向，而车载无人机配送系统需要解决的核心问题之一便是无人机如何实现在运动车载平台上的自主降落。

无人机若要实现在运动状态下降落在运动的车载平台上，对算法性能要求极高，包括：目标识别算法快速准确、轨迹预估算法实时稳定、跟踪控制器响应速度快且跟踪误差小。

无人机对车载平台的跟踪属于复杂运动背景下的快速跟踪，而且两者都处于非线性、非高斯类型的机动状态，此时对目标进行位置跟踪属于非线性系统状态估计问题，因此对目标识别与跟踪算法要求很高。

无人机在载货状态下飞行时惯性较大，所以要求飞行控制算法能达到响应速度快、超调量小、控制精度高、鲁棒性强的控制效果。单一的PID(Proportion IntegrationDifferentiation)控制虽然控制精度高，但存在响应速度慢、超调量大的问题。单一的模糊控制具有响应速度快、超调量小的优点，但存在因对信息的模糊化处理而导致控制精度低的问题。

发明内容

为了解决上述技术问题，本发明提出一种用于实现物流配送无人机自主降落的控制方法、装置、设备及存储介质，它能够使得无人机能快速、稳定的跟踪车载平台，并准确降落其上。

为了实现上述目的，本发明采用的方案是：

一种用于实现物流配送无人机自主降落的控制方法，其特征在于，包括步骤：

获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；

若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；

若e小于设定阈值，则所述无人机由PID算法进行控制。

还包括步骤：

重复上述步骤，直至所述运动目标位于所述无人机下方。

还包括步骤：

当所述运动目标位于所述无人机下方时，所述无人机下降；

当所述无人机降落至所述运动目标上时，关闭所述无人机的动力装置。

所述e的获取过程包括步骤：

获取全局坐标系O_e下无人机与运动目标的距离误差：

e_p＝(x_p，y_p，z_p)；

根据所述e_p计算在所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差：

其中，为全局坐标系O_e变换到机体坐标系O_b的变换矩阵，

θ、φ分别为无人机的横滚角、俯仰角、偏航角。

所述e_p的获取过程包括步骤：

利用粒子滤波算法对所述运动目标进行目标识别；

根据所述目标识别的识别结果对所述运动目标进行运动轨迹拟合；

所述无人机根据行动轨迹拟合结果调整自身的运行轨迹，并根据全局坐标系O_e下的机体坐标为p_n＝(x_n，y_n，z_n)与运动目标在全局坐标系O_e中的坐标为p_m＝(x_m，y_m，z_m)得到所述e_p。

所述利用粒子滤波算法对所述运动目标进行目标识别在执行过程中，包括步骤：

步骤A：确定需要跟踪的目标；

步骤B：提取目标颜色特征，将跟踪的目标区域转化到HSV空间；

步骤C：粒子初始化，由先验概率构成粒子群；

步骤D：释放粒子，计算偏移后的粒子位置；

步骤E：度量粒子的相似度系数，并按相似度系数提取目标位置坐标；

步骤F：利用得到的目标位置坐标进行轨迹拟合并输出到飞行控制器，作为后续无人机飞行的期望路径；

步骤G：检查是否出现粒子退化，出现退化则根据设定阈值抛弃权重小的粒子，并补充相同数目新粒子，返回步骤D，继续执行。

控制所述无人机时使用的动力学模型为：

式中，是对速度V求导，为无人机在全局坐标系O_e中的加速度；e为基向量，e＝[0，0，1]^T；R_a为机体坐标系O_b相对于全局坐标系O_e的旋转矩阵；为角速度，是对姿态角求导；为角加速度，是对角速度ω求导；J为无人机的转动惯量；τ是无人机相对于自身质心的扭转力矩；相当于对无人机在全局坐标系O_e中的位置p求导，代表无人机在全局坐标系O_e中的速度。

一种用于实现物流配送无人机自主降落的控制装置，包括：

数据采集单元，配置用于获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；

控制单元：若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；若e小于设定阈值，则所述无人机由PID算法进行控制；

信息存储单元，配置用于依次驱动运行数据采集单元与控制单元，直至所述运动目标位于所述无人机下方。

一种设备，所述设备包括：

一个或多个处理器；

存储器，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行所述的方法。

一种存储有计算机程序的计算机可读存储介质，该程序被处理器执行时实现所述的方法。

本发明的有益效果有：无人机的控制器由模糊逻辑控制器和PID控制器组成，通过误差e与设定阈值的比较来决定是由模糊控制器对无人机实施控制，还是由PID控制器实施控制，且此分段控制方式不影响PID控制器与模糊逻辑控制器的独立性。无人机与运动目标相距较远时，误差e大于设定阈值，此阶段由模糊逻辑算法对无人机实施控制；利用其响应速度快的优点，使无人机快速接近运动目标。当距离误差e小于设定阈值时，由PID算法对无人机实施控制；利用其控制精度高的优点使无人机在下降过程中实现对运动平台的精确锁定。以便无人机实施降落。

同时本发明所述的一种用于实现物流配送无人机自主降落控制方法，基于粒子滤波的目标识别算法配合轨迹拟合算法可以在无人机及运动目标均处于非高斯、非线性类型的运动状态中时对目标进行有效的识别跟踪。由模糊逻辑控制器和PID控制器组合而成的分段控制器在无人机远离目标时由模糊控制器单独发挥控制作用，利用其响应速度快的优点快速缩小与目标的误差，无人机接近目标时由PID控制器单独发挥作用，利用其控制精度高的优点，实现对运动目标的精准锁定，以便实施在运动平台上的降落。这种分段控制算法响应速度快、超调量小、控制精度高、鲁棒性强，可满足无人机在运动平台上进行降落的控制要求。

附图说明

图1示出了本申请实施例的无人机主要模块构成示意图；

图2示出了本申请实施例的目标识别与轨迹拟合算法流程图；

图3示出了本申请实施例的模糊逻辑PID分段控制系统框图；

图4示出了本申请实施例的跟踪降落流程图；

图5示出了本申请实施例的模糊逻辑控制器、PID控制器、模糊逻辑PID分段控制器控制下的阶跃响应对比图；

图6示出了本申请实施例的模糊逻辑控制器、PID控制器、模糊逻辑PID分段控制器控制下对正弦信号跟踪性测试对比图；

图7示出了本申请实施例的现场跟踪降落实验图。

具体实施方式

为了更好的了解本发明的技术方案，下面结合附图1-7对本发明作进一步说明。

图1示出了本申请实施例的无人机主要模块构成示意图，如图1所示摄像头模块1用于采集现场视频，并交由图像处理模块2处理；图像处理器模块2分析运算出的目标位置坐标输出到无人机控制模块5；无人机控制模块5通过分析陀螺仪7、加速度计8、磁力计9、超声波模块10和GPS模块11的数据得出机体位置坐标；无人机控制模块5通过分析求解目标位置坐标与机体位置坐标进而输出控制信号到电调4，并由电调4驱动电机5进而控制无人机实现对目标的跟踪，以及最后的降落。

所述无人机控制模块5中的分段控制器用于实现物流配送无人机自主降落的控制方法包括以下步骤：

S1：建立四旋翼无人机的动力学模型。

假设无人机机体坐标系为O_b，无人机所在全局坐标系为O_e，无人机在全局坐标系O_e中的位置用p＝[p_x，p_y，p_z]^T表达，无人机在全局坐标系O_e中的速度为V，则V＝[V_x，V_y，V_z]^T，重力加速度为g，无人机质量为m；理想状况下所有电机产生的总升力为f_A，，电机产生的扭矩为τ，且τ＝[τ_x，τ_y，τ_z]^T；无人机的转动惯量为J；基向量e＝[0，0，1]^T；R为O_b相对于O_e的旋转矩阵；为无人机相对于全局坐标系O_e的角速度为ω＝[ω_x，ω_y，ω_z]^T，变量Ω用矩阵表达为：

则无人机的动力学方程可表示为：

式中，是对速度V求导，为无人机在全局坐标系O_e中的加速度；e为基向量，c＝[0，0，1]^T；R_a为机体坐标系O_b相对于全局坐标系O_e的旋转矩阵；为无人机机体坐标系下的角速度，是对姿态角求导，所述姿态角为无人机机体坐标系下的；相当于对姿态角求导，代表角速度；为角加速度，是对角速度ω求导；J为无人机的转动惯量；τ是无人机相对于自身质心的扭转力矩；相当于对无人机在全局坐标系O_e中的位置p求导，代表无人机在全局坐标系O_e中的速度。

S2：目标识别与轨迹拟合算法建立。

S21：基于粒子滤波的目标识别算法。

如图2所示，基于粒子滤波的目标识别算法的实现有具体以下步骤：

步骤A：跟踪目标提取，确定出所要跟踪的目标。

步骤B：提取目标颜色特征，并将跟踪目标区域转化到HSV空间。

步骤C：粒子初始化，由先验概率构成粒子群。

步骤D：释放粒子，计算偏移后的粒子位置。

步骤E：度量粒子的相似度系数，并按相似度系数提取目标位置坐标。

步骤F：利用所得到的目标位置坐标进行轨迹拟合并输出到无人机控制模块5，作为后续无人机飞行的期望路径。

步骤G：检查是否出现粒子退化，出现粒子退化则根据设定阈值抛弃权重小的粒子，并补充相同数目新粒子。返回步骤D，继续执行。

S22：轨迹拟合算法。

轨迹拟合算法选取可表征大部分目标运动轨迹的二次函数作为轨迹拟合函数，轨迹拟合函数表达式为：

f(x)＝ax²+bx+c (3)

选取若干组实验数据，优选的为15组实验数据，并通过最小二乘法即可求解出系数a，b，c，最小二乘法计算步骤不再赘述。

S3：目标空间坐标系与无人机空间坐标系的变换，获取误差e。

假设(x_c，y_c，z_c，1)、(x_e，y_e，z_e，1)分别为空间内任一点P在摄像机坐标系和全局坐标系下的齐次坐标，则两坐标系间的变换关系可用下式表示：

s为任意非零参数，三维坐标系旋转参数存储在3×3正交单位矩阵N中，M为摄像机外参数矩阵，可将全局坐标系下的坐标映射到摄像机坐标系中，T为平移向量，表示摄像机坐标系原点与全局坐标系原点的偏移量，0＝(0，0，0)为零矢量。这种变换的意义，目标在摄像机坐标系下的坐标，如何变换到全局坐标系下，因为同一目标，在不同坐标系下的坐标是不同的。之所以要进行变换，是因为只有目标和飞机都变换到同一坐标系中才能进行误差运算和后续跟踪。

若全局坐标系O_e下得到机体坐标为p_n＝(x_n，y_n，z_n)，若摄像机获取并转换得到的运动目标在全局坐标系O_e中的坐标为p_m＝(x_m，y_m，z_m)，可知在全局坐标系O_e下无人机与运动目标的距离误差为e_p＝p_n-p_m，因此有：

e_p＝(x_n-x_m，y_n-y_m，z_n-z_m)＝(x_p，y_p，z_p)。

全局坐标系O_e变换到机体坐标系O_b的变换矩阵为：

其中θ、φ分别为无人机的横滚角、俯仰角、偏航角，再通过表达式：

可得在机体坐标系O_b下用于输入模糊逻辑PID分段控制器的误差e＝(x_e，y_e，z_e)，即所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差。

S4：模糊逻辑PID(Proportion Integration Differentiation)分段控制。

如图3所示，本发明的采用分段控制即模糊逻辑PID分段控制器，模糊逻辑PID分段控制器由模糊逻辑控制器和PID控制器组成，这里不再分别赘述模糊逻辑控制器原理和PID控制器原理，只给出二者在本发明中发挥控制作用的方式。通过误差e与设定阈值的比较来决定是由模糊控制器对无人机实施控制，还是由PID控制器实施控制，且此分段控制方式不影响PID(Proportion Integration Differentiation)控制器与模糊逻辑控制器的独立性。如图4所示，图中x_e为机体坐标系O_b下x轴方向误差，y_e为机体坐标系O_b下y轴方向误差。无人机与运动目标相距较远时，误差e大于设定阈值，此阶段由模糊逻辑算法对无人机实施控制。利用其响应速度快的优点，使无人机快速接近运动目标。当距离误差e小于设定阈值时，由PID算法对无人机实施控制。利用其控制精度高的优点使无人机在下降过程中实现对运动平台的精确锁定，以便无人机实施降落。

S5、重复步骤S2-S4，直至所述运动目标位于所述无人机下方。

S6、当所述运动目标位于所述无人机下方时，所述无人机下降；

S7、重复步骤S5-S6，直至所述无人机降落至所述运动目标上；当所述无人机降落至所述运动目标上时，关闭所述无人机的动力装置。

为了验证本发明提出的控制方案效果可行性，利用搭建的无人机控制系统进行仿真验证。下面结合图5至6进一步说明本发明的效果。

图7示出了本申请实施例的现场跟踪降落实验图，设计了相应的常规PID控制器和模糊逻辑控制器，与本发明提出的模糊逻辑PID分段控制器控制下的无人机进行对比实验。

在输入阶跃信号下对比仿真结果，如图5所示，模糊逻辑PID分段控制算法比PID算法超调量低12％、响应时间短1.5s；相比于模糊逻辑算法稳态误差低0.12cm。

在输入正弦信号下对比仿真结果，如图6所示，应用皮尔逊相关系数衡量不同控制算法作用下无人机实际运动轨迹和期望的正弦轨迹的拟合程度，模糊逻辑算法的相关系数为0.970，PID算法的相关系数为0.981，模糊逻辑PID分段控制算法的相关系数最高为0.998。

通过仿真对比结果可知，模糊逻辑PID分段控制器的响应速度要高于PID控制器，并且在控制精度高于模糊控制器，超调量小于模糊控制器，综合性能比这两种控制器优越很多。

为进一步验证本发明的无人机控制算法的性能优越性，进行了现场跟踪降落试验，现场试验表明，在模糊逻辑PID分段控制器控制下无人机降落在运动平台上的成功率要远高于另外两种控制器单独控制下无人机的降落成功率。

本发明基于分段控制器的物流配送无人机自主降落算法，既拥有模糊逻辑算法响应速度快、超调量小、鲁棒性强的优点，又能兼顾PID算法控制精度高的优点，能稳定、准确控制无人机自主降落在运动的车载平台上。

一种用于实现物流配送无人机自主降落的控制装置，包括：

数据采集单元，配置用于获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；

控制单元：若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；若e小于设定阈值，则所述无人机由PID算法进行控制；

信息存储单元，配置用于依次驱动运行数据采集单元与控制单元，直至所述运动目标位于所述无人机下方，直至所述无人机降落至所述运动目标上。

所述控制单元，还配置用于当所述运动目标位于所述无人机下方时，控制所述无人机下降；还配置用于当所述无人机降落至所述运动目标上时，关闭所述无人机的动力装置。

本发明还提供了一种设备，所述设备包括：

一个或多个处理器；

存储器，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行所述的一种用于实现物流配送无人机自主降落的控制方法。

本发明还提供了一种存储有计算机程序的计算机可读存储介质，该程序被处理器执行时实现所述的一种用于实现物流配送无人机自主降落的控制方法。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.一种用于实现物流配送无人机自主降落的控制方法，其特征在于，包括步骤：

获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；

若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；

若e小于设定阈值，则所述无人机由PID算法进行控制。

2.根据权利要求1所述的一种用于实现物流配送无人机自主降落的控制方法，其特征在于，还包括步骤：

重复上述步骤，直至所述运动目标位于所述无人机下方。

3.根据权利要求2所述的一种用于实现物流配送无人机自主降落的控制方法，其特征在于，还包括步骤：

当所述运动目标位于所述无人机下方时，所述无人机下降；

当所述无人机降落至所述运动目标上时，关闭所述无人机的动力装置。

4.根据权利要求1所述的一种用于实现物流配送无人机自主降落的控制方法，其特征在于，所述e的获取过程包括步骤：

获取全局坐标系O_e下无人机与运动目标的距离误差：

e_p＝(x_p，y_p，z_p)；

根据所述e_p计算在所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差：

其中，为全局坐标系O_e变换到机体坐标系O_b的变换矩阵，

θ、φ分别为无人机的横滚角、俯仰角、偏航角。

5.根据权利要求4一种用于实现物流配送无人机自主降落的控制方法，其特征在于，所述e_p的获取过程包括步骤：

利用粒子滤波算法对所述运动目标进行目标识别；

根据所述目标识别的识别结果对所述运动目标进行运动轨迹拟合；

所述无人机根据行动轨迹拟合结果调整自身的运行轨迹，并根据全局坐标系O_e下的机体坐标为p_n＝(x_n，y_n，z_n)与运动目标在全局坐标系O_e中的坐标为p_m＝(x_m，y_m，z_m)得到所述e_p。

6.根据权利要求5所述的一种用于实现物流配送无人机自主降落的控制方法，其特征在于，所述利用粒子滤波算法对所述运动目标进行目标识别在执行过程中，包括步骤：

步骤A：确定需要跟踪的目标；

步骤B：提取目标颜色特征，将跟踪的目标区域转化到HSV空间；

步骤C：粒子初始化，由先验概率构成粒子群；

步骤D：释放粒子，计算偏移后的粒子位置；

步骤E：度量粒子的相似度系数，并按相似度系数提取目标位置坐标；

步骤F：利用得到的目标位置坐标进行轨迹拟合并输出到飞行控制器，作为后续无人机飞行的期望路径；

步骤G：检查是否出现粒子退化，出现退化则根据设定阈值抛弃权重小的粒子，并补充相同数目新粒子，返回步骤D，继续执行。

7.根据权利要求5所述的一种用于实现物流配送无人机自主降落的控制方法，其特征在于，控制所述无人机时使用的动力学模型为：

式中，是对速度V求导，为无人机在全局坐标系O_e中的加速度；e为基向量，e＝[0，0，1]^T；R_a为机体坐标系O_b相对于全局坐标系O_e的旋转矩阵；为角速度，是对姿态角求导；为角加速度，是对角速度ω求导；J为无人机的转动惯量；τ是无人机相对于自身质心的扭转力矩；相当于对无人机在全局坐标系O_e中的位置p求导，代表无人机在全局坐标系O_e中的速度。

8.一种用于实现权利要求1-7任一所述方法的装置，其特征在于，包括：

数据采集单元，配置用于获取所述无人机的机体坐标系O_b下，所述无人机与运动目标的距离误差e；

控制单元：若e大于设定阈值，则所述无人机由模糊逻辑算法进行控制；若e小于设定阈值，则所述无人机由PID算法进行控制。

9.一种设备，其特征在于，所述设备包括：

一个或多个处理器；

存储器，用于存储一个或多个程序，

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器执行如权利要求1-7中任一项所述的方法。

10.一种存储有计算机程序的计算机可读存储介质，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的方法。