[发明专利]一种新型无人机路径规划方法

权利要求书

1.一种新型无人机路径规划方法，其特征在于，包括如下步骤：

步骤1、确定无人机飞行数学模型

无人机飞行数学模型包括无人机机体运动模型和无人机动力学模型，具体如下：

无人机机体运动模型：

四旋翼飞行器通过控制四个旋翼的转速从而对俯仰角(θ)、偏航角(ψ)、横滚角进行调节进而完成机体的一系列飞行运动；

首先建立两个坐标系：机体坐标系、惯用坐标系；

惯用坐标系E(OXYZ)相对于地球表面不动；

机体坐标系A(oxyz)与机体质心重合，横轴ox指向一号电机，纵轴oy指向四号电机，oz垂直于oxy面；

四旋翼飞行器欧拉角：

(1)横滚角机体绕ox轴转动角度；

(2)俯仰角(θ)：机体绕oy轴转动角度；

(3)偏航角(ψ)：机体绕oz轴转动飞行器纵轴在水平面内投影与惯性坐标系OX轴之间的夹角；

无人机动力学模型：

根据四旋翼的姿态矢量其中为横滚角，ψ为偏航角，θ为俯仰角，以及位移矢量[x,y,z]，其动力学方程：

其中：

u₁表示四个旋翼总升力，u₂表示横滚力矩，u₃表示俯仰力矩，u₄表示偏航力矩，w₁、w₂、w₃、w₄分别表示四个旋翼的转速，I_x、I_y、I_z表示机体在xyz轴上的转动惯量；

步骤2、Maklink环境模型构建

采用Maklink图论法对二维路径进行建模：

(1)选择一个障碍物，选取障碍物上的一顶点，连接该点与其他障碍物的定点，作该点到环境空间边界的垂线段；

(2)将得到的所有顶点和定点的连接线段按照长度从短到长的顺序，加入到线段存储表；

(3)选择线段存储表中的表头线段；

(4)检查该表头线段是否穿越环境空间中任意障碍物边界，如果发生相交，那么这条线段就不是一条自由连接线；因此，放弃当前线段并选择线段存储表中的下一条线段；重复检查过程，直至找到一条线段与所有障碍物边界不相交；

(5)将线段储存表中的各连接线段的中点相连，检查链接线段是否穿越障碍物边界，若链接线段穿越障碍物，则删除该线段；

步骤3、采用粒子群算法对蚁群算法参数进行优化，通过优化后的蚁群算法进行二维路径规划；

通过粒子群算法对蚁群算法参数进行优化，通过优化后的蚁群算法在Maklink环境模型中找出最优路径；路径长度通过公式(11)计算得出：

其中x_i y_i为当前位置坐标，x_i+1 y_i+1为下一点位置坐标，m为起始点到目标点的节点个数；Dist为路径总长度。