一种基于边界的烟田无人机自主导航算法
【技术领域】
[0001] 本发明涉及无人机导航算法领域，更具体地，涉及一种基于边界的烟田无人机自 主导航算法。
【背景技术】
[0002] 目前烟田无人机导航一般采用的方法是同步定位与地图构建，该方法是假定运动 轨迹预先已知的，这种被动形式的方法与环境的未知性相矛盾，现有的主动的同步定位与 地图构建方法主要是最优控制方法、局部子图法，而最优控制方法建立的目标函数是选择 航迹规划点的基础，但是容易陷入局部最优，局部子图的方法更适用于多无人机协同的同 步定位与地图构建航迹规划，对于单个无人机而言，除了可以减小一定计算量外，并没有明 显的优势。
【发明内容】
[0003] 本发明提供一种基于边界的烟田无人机自主导航算法，该算法目标点的选择范围 较宽，可在一定程度上避免陷入局部最优。
[0004] 为了达到上述技术效果，本发明的技术方案如下：
[0005] -种基于边界的烟田无人机自主导航算法，包括以下步骤：
[0006] Sl :在烟田中设置若干地标，并取候选边界点：
[0007] S2 :对候选的边界点进行评价确定边界点；
[0008] S3:使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。
[0009] 进一步地，所述步骤Sl的过程如下：
[0010] 将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同 的组，取每组中的边界点的重心的位置作为候选边界点。
[0011] 进一步地，所述步骤S2的过程如下：
[0012] 构建无人机观测地标的目标函数：
[0013] J= Q1A^co2AS
[0014]
Pjj (k Ik)表示k时亥丨J第j个地标的方差，N为地标总数，
是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，cop?2是根 据经验选取的权值；k时刻J值最大的候选边界点确定为一个边界点。
[0015] 进一步地，所述步骤S3的过程如下：
[0016]1)令无人机的运动状态模型为：
[0017] Xv=[Xy边]T
[0018] 式中x、y是无人机在二维平面中的位置，it是方位角；
[0019] 令控制量U = [Ad A it]T，Ad是一个时间步长对应的无人机运动距离，A也为 姿态变化量，其中：
[0021] 则无人的运动状态模型方程为：
[0022] =f (Xu^Uk)+ Wi
[0023] 无人机与第i个地标的测量方程为：
[0024] Zlk =HiXu^mtk)+Uk
[0025] 式中Iiiiik表示第i个地标的状态，Ini= [mxmy]T，地标是静止的则Hiiik=Iiiiik ^^和 u15运动噪声向量；
[0026] 2)无人机的运动状态预测过程为：
动噪声向量u(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差；
[0033] 3)无人机的运动状态更新过程为：
[0039] 式中R(k)是运动噪声向量u(k)的协方差，V一表示h( ?)相对于状态变变量 X(k)的雅克比矩阵。
[0040] 与现有技术相比，本发明技术方案的有益效果是：
[0041] 本发明算法在无人机的探测区域内产生候选边界点，通过建立合理的目标函数， 从候选边界点中选择目标点，建立的无人机运动状态模型，控制无人机朝该目标点方向运 动，再预测和更新更新无人机的运动状态，该算法从确定的边界点中选择下一步运动目标 点，目标点的选择范围可更宽，并在一定程度上避免陷入局部最优。
【附图说明】
[0042] 图1为本发明算法的流程图；
[0043]图2为本发明算法的仿真实验图；
[0044] 图3为最优控制法的仿真实验图。
【具体实施方式】
[0045] 附图仅用于示例性说明，不能理解为对本专利的限制；
[0046] 为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品 的尺寸；
[0047] 对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解 的。
[0048] 下面结合附图和实施例对本发明的技术方案做进一步的说明。
[0049] 实施例1
[0050] -种基于边界的烟田无人机自主导航算法，包括以下步骤：
[0051]Sl:在烟田中设置若干地标，并取候选边界点：
[0052]S2 :对候选的边界点进行评价确定边界点；
[0053] S3:使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。
[0054] 进一步地，所述步骤Sl的过程如下：
[0055] 将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同 的组，取每组中的边界点的重心的位置作为候选边界点。
[0056] 步骤S2的过程如下：
[0057] 构建无人机观测地标的目标函数：
[0058] J=Q1A^co2AS
[0059]
Pjj(kIk)表示k时亥Ij第j个地标的方差，N为地标总数，
是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，cop?2是根 据经验选取的权值；k时刻J值最大的候选边界点确定为一个边界点。
[0060] 步骤S3的过程如下：
[0061] 1)令无人机的运动状态模型为：
[0062] Xv=[Xy边]T
[0063] 式中x、y是无人机在二维平面中的位置，it是方位角；
[0064] 令控制量U=[AdAit]T，Ad是一个时间步长对应的无人机运动距离，A也为 姿态变化量，其中：
[0066] 则无人的运动状态模型方程为：
[0067] =I(Kl^ul) +Wk
[0068] 无人机与第i个地标的测量方程为：
[0069]
[0070] 式中Iiiiik表示第i个地标的状态，Ini= [mxmy]T，地标是静止的则miik=miik ^^和 u15运动噪声向量；
[0071] 2)无人机的运动状态预测过程为：
动噪声向量U(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差；
[0078] 3)无人机的运动状态更新过程为：
[0084] 式中R(k)是运动噪声向量u(k)的协方差，VxA表示h( ?)相对于状态变变量 X(k)的雅克比矩阵。
[0085] 利用本算法与最优控制法进行仿真对比，如图2和图3所示最优控制法的探测环 境有限，并且无人机的运动状态呈现混乱无规律的特点，经常会陷入某个区域而不能离开， 而采用本发明算法航迹规划，则能够较好地克服这种情况，无人机航迹平滑有序，显然，采 用本发明算法时，无人机可以探测到更大范围的环境，并得到了较高的地标估计值。
[0086] 相同或相似的标号对应相同或相似的部件；
[0087] 附图中描述位置关系的用于仅用于示例性说明，不能理解为对本专利的限制；
[0088] 显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对 本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可 以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本 发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求 的保护范围之内。
【主权项】
1. 一种基于边界的烟田无人机自主导航算法，其特征在于，包括以下步骤： 51 :在烟田中设置若干地标，并取候选边界点： 52 :对候选的边界点进行评价确定边界点； 53 :使无人机朝确定的边界点运动并对无人机运动状态进行预测和更新。2. 根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步 骤Sl的过程如下： 将无人机作业区域分为若干大小相等的小单元，每一个单元内的边界点分为相同的 组，取每组中的边界点的重心的位置作为候选边界点。3. 根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步 骤S2的过程如下： 构建无人机观测地标的目标函数： J = Q1A^co2ASPjj(kIk)表示k时亥Ij第j个地标的方差，N为地标总数，是无人机在边界点位置观测到的新的地标数L占总地标数的百分比，copCo2是根据经验选 取的权值；k时刻J值最大的候选边界点确定为一个边界点。4. 根据权利要求1所述的基于边界的烟田无人机自主导航算法，其特征在于，所述步 骤S3的过程如下： 1) 令无人机的运动状态模型为： Xv=[X y边]T 式中x、y是无人机在二维平面中的位置，也是方位角； 令控制量U=[AdAit]T，Ad是一个时间步长对应的无人机运动距离，Ait为姿态 变化量，其中：则无人的运动状态模型方程为：式中Iiiiik表示第i个地标的状态，IHi= [mxmy]T，地标是静止的则Iiiiik=Iiiiikuk 运动噪声向量； 2) 无人机的运动状态预测过程为：分别是f( ?)相对无人机状态变量X(k)和运动噪 声向量U(k)的雅克比矩阵，Q(k)是运动噪声向量W(k)的协方差； 3)无人机的运动状态更新过程为：式中R(k)是运动噪声向量u(k)的协方差，Va./?表示h( ?)相对于状态变变量X(k) 的雅克比矩阵。
【专利摘要】本发明提供一种基于边界的烟田无人机自主导航算法，该算法在无人机的探测区域内产生候选边界点，通过建立合理的目标函数，从候选边界点中选择目标点，建立的无人机运动状态模型，控制无人机朝该目标点方向运动，再预测和更新无人机的运动状态，该算法从确定的边界点中选择下一步运动目标点，目标点的选择范围可更宽，并在一定程度上避免陷入局部最优。
【IPC分类】G01C21/00
【公开号】CN105222770
【申请号】CN201510551590
【发明人】陈泽鹏
【申请人】中国烟草总公司广东省公司
【公开日】2016年1月6日
【申请日】2015年8月31日