hp自适应RPM在四旋翼无人机轨迹优化中的应用

(西南交通大学 机械工程学院，四川 成都 610031)

近几年，随着四旋翼无人机的普及，四旋翼无人机的应用和研究越来越多。在有障碍的环境中提高四旋翼无人机的可操作性和位置精度需要对四旋翼无人机进行飞行轨迹优化[1]。在现有的四旋翼无人机轨迹优化研究中大多是把四旋翼无人机放在完全空旷的环境中，在轨迹优化过程中没有考虑四旋翼无人机的避障问题，或者将四旋翼无人机假设为一个质点，没有考虑四旋翼无人机在飞行过程中的姿态问题[2]。本文针对上述问题，考虑四旋翼无人机在有障碍和控制四旋翼无人机姿态的条件下，离线优化四旋翼无人机的轨迹。

四旋翼无人机轨迹优化问题的本质是带有控制变量、状态变量、约束的非线性最优控制问题。这类问题之前是通过解析解来进行求解，但是随着系统的复杂性越来越高，其解析解的求解难度越来越大[3]。随着计算机的普遍使用，非线性最优控制问题的数值方法逐渐代替解析法，采用数值方法中的hp自适应Radau伪谱法(Radau Pseudospectral Method，RPM)进行四旋翼无人机避障轨迹优化，具有全局优化、收敛速度快、对初值不敏感等优点[4]。并已经应用于工程实际中，在2006年和2007年，美国的Ross等人利用伪谱法实现了国际空间站在“零燃料”下大角度姿态机动，验证了伪谱法的可行性[5]。hp自适应是结合了有限元区间划分的思想将区间长度h和基函数阶次p进行自适应配置以更高的精度、更小的计算量逼近状态变量和控制变量。

1 四旋翼无人机动力学模型

在建立四旋翼无人机的动力学模型时，以机体质心为坐标原点，符合右手定则的坐标系B(O,X,Y,Z)为机体坐标系，以固结于地面和符合右手定则的坐标系E(O,X,Y,Z)为惯性坐标系，四旋翼无人机的坐标示意图如图1所示。四旋翼无人机在空中无约束运动，存在6个自由度，选用12个状态变量进行描述，分别为:位置(x，y，z)、速度(vx，vy，vz)、姿态(φ，θ，ψ)、角速度(p，q，r)。参考文献[6]，整理可得四旋翼无人机的非线性动力学模型为

(1)

式中，l为四旋翼无人机电机中心到质心的距离；m为四旋翼无人机的质量；U1,U2,U3,U4分别为四旋翼无人机的4个输入控制量；Ix,Iy,Iz分别为四旋翼无人机相对x,y,z轴的转动惯量；g为重力加速度。

图1 四旋翼无人机坐标系示意图

根据空气动力学原理，一般认为单独的一个螺旋桨所能提供的动力与转速的平方成正比，即F=ktω2，M=kdω2，kt,kd为比例系数。根据四旋翼无人机的运动原理控制量Ui(i=1,2,3,4)与电机转速的关系如下：

(2)

经查阅相关资料和实际测量，四旋翼无人机避障轨迹优化时所用到的参数如系统参数表1所示[7]。

表1 系统参数表

2 最优化问题描述

以实验室四旋翼无人机为研究对象，四旋翼无人机实物图如图2所示。

图2 四旋翼无人机实物图

假设四旋翼无人机在障碍已知的环境中飞行，根据四旋翼无人机的动力学模型、约束和优化目标，以及路径约束、边界约束、控制量约束，进行轨迹优化。根据实验室四旋翼无人机的性能参数，路径约束为

(3)

根据系统参数表1和式(2)可得控制量约束为

(4)

以四旋翼无人机的飞行路径最短为优化目标，可以降低四旋翼无人机的整体速度，使其更平稳飞行，即

minJ=L

(5)

3 优化方法

3.1 遗传算法进行轨迹优化

图3 遗传算法所得三维轨迹俯视图

3.2 hp自适应RPM理论

3.2.1 RPM理论

由于遗传算法的随机性，每次计算结果并不完全一致，可能存在“伪最优轨迹”。采用hp自适应RPM进行四旋翼无人机避障轨迹优化。以LGR配点，将连续的最优控制问题离散化为非线性规划问题，在离散点之间以拉格朗日多项式逼近状态变量和控制变量，拉格朗日插值多项式的导数将连续的动力学微分方程转化为离散的代数约束，在离散点处求控制变量和状态变量最优解[11]。hp自适应是应用有限元思想，在全局配点离散的基础上判断各个离散点之间的误差，在不连续或非光滑处采用h法增加分段数目，在光滑处采用p方法增加配点数获得指数的收敛效果[12]。

伪谱法的一般步骤如下[13-17]。

① 将系统状态方程的时间域转换到伪谱法的时间域。

(6)

② LGR配点，包含起点和K个LGR点，K维的LGR点是K阶Legendre多项式与K-1阶Legendre多项式和的零点，即PK(τ)+PK-1(τ)=0的根，PK(τ)为K阶Legendre多项式，其表达式的递推公式为

P0(τ)=1
P1(τ)=τ
nPn(τ)=(2n-1)τPn-1(τ)-(n-1)Pn-2(τ)(n≥2)

(7)

③ 根据式(7)求解K个LGR点和τ=-1共K+1个离散点，以拉格朗日插值多项式为基函数近似状态变量和控制变量如下：

(8)

且

其中，Lj(τ)为拉格朗日基函数；X(τj)为第j个离散点。

(9)

其中，U(τj)为第j个离散点。

④ 对式(8)求导将动力学微分方程离散化。

(10)

其中，

⑤ 以Gauss积分逼近终端状态为

(11)

经过上述五步转化即可将连续的最优控制问题转化为离散的非线性问题，可以利用非线性求解器(IPOPT、SNOPT等)进行求解。

3.2.2 hp自适应规则

① 判断在评判点处的约束误差εmax是否均小于εp，若小于则停止计算，输出计算结果，否则进行第②步。

⑤ 返回第①步进行迭代计算，直到满足精度要求或者达到最大迭代次数。

4 系统仿真

图4 伪谱法所得三维轨迹俯视图

图5 四旋翼无人机XYZ方向的速度

图6 四旋翼无人机姿态

图7 四旋翼无人机输入控制量

通过分析图4～图7，四旋翼无人机能够在规定时间和约束条件下到达规定的状态，通过hp自适应RPM优化的避障轨迹能够实现避障功能，各项指标均在容许范围之内。位移、速度、姿态等没有达到约束边界，但是控制量存在达到约束值的时刻，说明四旋翼无人机系统的动力不足，响应较慢，可以通过提高四旋翼无人机的动力，实现四旋翼无人机的平稳、快速飞行。另外分析轨迹俯视图(见图4)和四旋翼无人机速度图(见图5)可以发现，四旋翼无人机避障轨迹在靠近障碍时速度会降低，可避免四旋翼无人机在障碍附近因为避障而失控发生碰撞。

5 结束语

本文基于hp自适应RPM的四旋翼无人机避障轨迹优化进行研究，分析了四旋翼无人机的动力学模型和获取实验室四旋翼无人机的部分参数，利用遗传算法和hp自适应RPM分别进行四旋翼无人机避障轨迹优化。实验结果表明，遗传算法存在随机性，可能会出现“伪最优解”。通过对比hp自适应RPM求解的四旋翼无人机避障轨迹路径比遗传算法要短，在优化过程中充分考虑了四旋翼无人机的各性能指标，包括各向位移、速度、姿态、控制变量等，能够使四旋翼无人机更平稳、安全地飞达目的地。