[发明专利]一种面向城市空间的多旋翼无人机路径规划方法

权利要求书

1.一种面向城市空间的多旋翼无人机路径规划方法，其特征在于，包括以下步骤：

步骤1，确定多旋翼无人机动力学模型；

步骤2，确定多旋翼无人机轨迹优化约束条件及优化指标；

步骤3，建立多旋翼无人机动力学模型与障碍物约束线性化；

步骤4，建立多旋翼无人机动力学方程及约束条件离散化；

步骤5，求解完成面向城市空间的多旋翼无人机路径规划；

步骤1中所述多旋翼无人机动力学模型如下：

其中， x、 y和 z分别表示多旋翼无人机的三轴位置坐标， 、 和 分别表示多旋翼无人机沿三轴的速度分量， 、 和 分别表示三轴位置坐标关于时间的导数， 、 和分别表示三轴位置坐标关于时间的二阶导数； 、 和 分别表示多旋翼无人机的俯仰角、滚转角和偏航角， p、q和 r分别表示多旋翼无人机的俯仰角速度、滚转角速度和偏航角速度，、和 表示姿态角关于时间的一阶导数， 、和表示姿态角关于时间的二阶导数； 、和 表示多旋翼无人机对应三个轴的转动惯量； m表示多旋翼无人机的质量， 表示升力作用点，即多旋翼无人机螺旋桨中心到质心的距离； 、 、 和 分别表示多旋翼无人机的四个控制输入量， g表示重力加速度；

步骤2中多旋翼无人机的初始状态约束定义如下：

其中， 、和 分别表示多旋翼无人机在初始时刻的三轴位置坐标， 、 和 分别表示多旋翼无人机在初始时刻的沿x轴、y轴和z轴的速度分量， 、和 分别表示多旋翼无人机在初始时刻的姿态角， 、 和 分别表示多旋翼无人机在初始时刻姿态角速度；

步骤2中多旋翼无人机的终端状态约束定义如下：

其中， 表示飞行时间， 、 和 分别表示多旋翼无人机在时刻的三轴位置坐标，、 和分别表示多旋翼无人机在时刻的沿 x轴、 y轴和z轴的速度分量， 、 和分别表示多旋翼无人机在时刻的姿态角， 、和分别表示多旋翼无人机在 时刻姿态角速度；

步骤2中多旋翼无人机的过程约束定义如下：

其中， 、、 、、和分别表示多旋翼无人机飞行过程中各状态变量的最小值， 、、、、和 分别表示多旋翼无人机飞行过程中各状态变量的最大值；

步骤2中多旋翼无人机的控制输入约束定义如下：

其中， 、、和 分别表示四个控制输入量的最大值；

步骤2中多旋翼无人机的障碍物约束如下：

其中， 表示障碍物中心， a表示障碍物边缘沿x轴方向距离障碍物中心的距离， b表示障碍物边缘沿 y轴方向距离障碍物中心的距离， z表示障碍物边缘沿z轴方向距离障碍物中心的距离， 表示多旋翼无人机距离障碍物的安全阈值；

优化指标定义如下：

；

步骤3中所述多旋翼无人机动力学模型中，动力学方程线性化表达式如下：

其中， X^*为任意参考轨迹， X表示状态变量， U表示控制输入，B为系数矩阵，

障碍物约束线性化形式如下：

其中， 为的雅可比矩阵；

步骤4中所述多旋翼无人机动力学方程及约束条件离散化方法如下：

步骤4-1，将时间区间分成N等份，时间步长为h，其表达式如下：

；

步骤4-2，根据显式四阶龙格库塔公式，对动力学方程进行离散化如下：

其中， 和分别表示状态变量在第和第节点取值， 、、和 表达式如下

其中， 和分别表示控制输入在第和第节点取值；

步骤4-3，终端状态约束可写成如下形式：

；

步骤4-4，过程约束可写成如下形式：

其中， ；

步骤4-5，控制输入约束写成如下形式：

其中， ；

步骤4-6，障碍物约束写为：

其中， ；

步骤4-7，优化指标写成如下形式：

；

步骤4-8，经过线性化和离散化处理后，所述面向城市空间的多旋翼无人机路径规划问题归纳为如下形式：

其中， ；

对步骤4中所归纳的多旋翼无人机路径规划问题进行求解，过程如下：

步骤5-1，已知多旋翼无人机初始状态情况下，令， ， ， ，通过动力学递推得到一条初始参考轨迹X^*；

步骤5-2，将所述初始参考轨迹X^*带入步骤4中的多旋翼无人机路径规划问题，采用内点法进行求解，得到一条新的轨迹，并将该轨迹作为下一次计算的参考轨迹X^*；

步骤5-3，当得到的轨迹收敛以后，则得到最优解，即面向城市空间的多旋翼无人机能量消耗最优轨迹，完成面向城市空间的多旋翼无人机路径规划。