CN110989626B - 一种基于控制参数化的无人机路径规划方法 - Google Patents

Abstract

本发明公开了一种基于控制参数化的无人机路径规划方法，主要包括非线性规划建模、控制参数化、约束转录处理和梯度公式求解四个步骤。本发明综合考虑了3D环境中的障碍物约束以及无人机自身的动力学约束和性能指标约束，并采用两种障碍物建模方法使无人机路径规划能够满足城市及山地环境的应用，通过求解最优解的方式，在满足状态约束和控制约束条件下，获得无人机的最优时间无碰撞路径，通过该方法获得的无人机最优路径相比现有常规的RRT方法和A*方法的最优路径更加平滑，并且在无人机运动过程中电机转速变化更为稳定，运动轨迹平稳可行。

Description

一种基于控制参数化的无人机路径规划方法

技术领域

本发明涉及四旋翼无人机控制技术领域，具体地讲，是涉及一种基于控制参数化的无人机路径规划方法。

背景技术

四旋翼无人机具有可垂直升降，飞行速度快，轻便灵活的特点，因此在许多领域都得到了广泛应用。例如，随着无人机航拍技术不断发展，当自然灾害发生时，我们需要及时了解灾害情况，这就需要无人机进入受灾区域，通过影像分析受灾状况。此外，无人机还被用于交通监视领域，可以对城市路况进行实时监视，实现区域管控，确保交通畅通。对于突发的交通事故，可以及时传递信息，方便紧急救援。无人机任务复杂度不断提高，为了实现自主飞行，在执行任务之前的路径规划至关重要。传统的无人机路径规划技术一般只考虑环境中的障碍物约束，不考虑无人机本身的动力学约束，算法复杂度低，但存在着由于没有考虑无人机自身的动力学约束导致规划出的最短路径不一定可行的劣势。

现有的路径规划算法一般可以分为在线算法和离线算法。通常，在线算法的计算复杂度比较低。在线算法又包括基于采样的算法和基于结点的算法，其中比较典型的有快速搜索随机树(RRT)方法和A*算法。离线路径规划算法也被广泛研究，比如基于数学方法的凸优化算法、混合整数规划算法。还有基于仿生学方法的遗传算法、神经网络和粒子群算法。四旋翼无人机动力学模型具有非线性、欠驱动、强耦合的特点，而以上这些算法没有考虑无人机本身的动力学约束，因此，计算出的路径不是最优的。

RRT算法随机性强且搜索没有偏向性，得到的路径不一定为最优路径。如附图1所示，给出了在山地环境中RRT方法搜索的最优路径。A*寻路算法采用启发式搜索方法，避免了很多无谓的搜索，提高了效率，但是如果我们想搜索的更精确，就要将方格分割得更小，但是方格越多，搜索越慢，尤其是在三维空间中，在时间上是呈指数增长的。对于凸优化和混合整数规划这些数学方法，大多需要把运动学模型线性化处理，这样得到的最优路径只是近似解，不够精确。

发明内容

针对上述技术问题，本发明提供一种基于控制参数化的无人机路径规划方法。

为了实现上述目的，本发明采用的技术方案如下：

一种基于控制参数化的无人机路径规划方法，包括以下步骤：

(S1)非线性规划建模：根据无人机的非线性动力学方程选取状态变量和控制变量并将其改写为状态空间模型的形式，利用无人机自身的性能指标约束和环境中的障碍物约束，建模为无人机连续状态不等式约束，把无人机的初始状态和目标点作为边界约束条件，把无人机飞行时间作为优化的目标函数，将无人机路径规划问题转化为无人机的最小化飞行时间的非线性最优控制问题，求解最优的控制变量u(t)使得飞行时间T最小，并满足各项约束条件；

(S2)控制参数化：选取等间隔p+1的个点，把[0,T]分成p段，选取的点τ为0＝τ₀<τ₁<τ₂<…<τ_p-1<τ_p＝T，根据公式

将控制变量u(t)转化为多段分段的常量，定义

基于无人机飞行时间作为优化的目标函数，采用时间压缩的方法，无人机控制状态可转化为

其中，X为状态变量，s为被压缩的时间变量，当p足够大时，该式的次优解为无人机的最小化飞行时间的非线性最优控制问题的最优解；

(S3)约束转录处理：采用约束转录方法把无人机连续状态不等式约束转化为积分等式约束，并对该积分等式约束进行局部平滑处理，获得转化后的约束条件；

(S4)利用梯度下降算法对转化后的约束条件进行优化，将无人机轨迹规划问题转化为非线性优化问题，使用MISER3.2软件进行求解，获得无人机基于控制参数化的最优路径规划结果。

具体地，所述环境中的障碍物约束包括根据p-norm规则建立的用来模拟无人机在城市飞行环境的第一环境约束，以及用于模拟山地飞行环境的第二环境约束。

具体地，所述无人机自身的性能指标约束包括无人机终端约束、连续状态不等式约束和控制输入约束，其中连续状态不等式约束包括无人机海拔高度约束、速度约束和角度约束。

具体地，该基于控制参数化的无人机路径规划方法，还包括：

(S5)将步骤(S4)获得的无人机基于控制参数化的最优路径规划结果写入闭环PD控制器内对无人机进行路径规划控制。

与现有技术相比，本发明具有以下有益效果：

本发明综合考虑了3D环境中的障碍物约束以及无人机自身的动力学约束和性能指标约束，并采用两种障碍物建模方法使无人机路径规划能够满足城市及山地环境的应用，通过求解最优解的方式，在满足状态约束和控制约束条件下，获得无人机的最优时间无碰撞路径，通过该方法获得的无人机最优路径相比现有常规的RRT方法的最优路径，其最优路径误差更小，并且在无人机运动过程中的电机转速变化更为稳定，运动轨迹平稳可行。

附图说明

图1为常规RRT方法得到的路径规划图。

图2为本发明中控制参数化技术的示意图。

图3为本发明中局部平滑处理函数的说明图。

图4为本发明中无障碍物环境路径规划图。

图5为本发明中城市环境中的路径规划图。

图6为本发明中山地环境中的路径规划图。

图7为对基于本发明方法产生的最优路径的轨迹跟踪图。

图8为对基于RRT方法产生的最优路径的轨迹跟踪图。

图9为跟踪本发明方法产生的最优路径时电机转速的变化情况。

图10为跟踪RRT方法产生的最优路径时电机转速的变化情况。

具体实施方式

下面结合附图和实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。

实施例

如图1至图10所示，该基于控制参数化的无人机路径规划方法，主要包括非线性规划建模、控制参数化、约束转录处理和梯度公式求解四个步骤。

(S1)非线性规划建模：根据无人机的非线性动力学方程选取状态变量和控制变量并将其改写为状态空间模型的形式，利用无人机自身的性能指标约束和环境中的障碍物约束，建模为无人机连续状态不等式约束，把无人机的初始状态和目标点作为边界约束条件，把无人机飞行时间作为优化的目标函数，将无人机路径规划问题转化为无人机的最小化飞行时间的非线性最优控制问题，求解最优的控制变量u(t)使得飞行时间T最小，并满足各项约束条件。

其中，设定无人机运动学模型如下：

其中，x,y,z是无人机在空间中的位置坐标，θ,ψ,

是三个欧拉角，分别代表俯仰角、滚转角和偏航角，g是重力加速度，L是无人机电机到重心的臂长，m是无人机的总质量，I_i(i＝x,y,z)是无人机分别对每个轴的转动惯量，K_i(i＝1,…,6)是空气阻力系数，u_i(i＝1,2,3,4)是如下定义的虚拟的控制输入量：

F_i＝K_vω_i ²，(i＝1，2，3，4) (3)

式中，F_i(i＝1，2，3，4)是第i个电机产生的推力，C是推力和力矩的转化因子，ω_i(i＝1，2，3，4)是是第i个电机的转速，K_v是推力系数，i＝1，2，3，4表示四旋翼的四个电机；

状态变量定义如下：

控制变量定义如下：

u＝[u₁ u₂ u₃ u₄]^T (5)

将所述无人机运动学模型改写为状态空间表达式的形式如下：

所述环境中的障碍物约束有两种，第一种为根据p-norm规则建立的用来模拟无人机在城市飞行环境的第一环境约束，采用如下不等式表示：

式中，(x_c，y_c，z_c)是障碍物的中心坐标，a，b，c代表障碍物分别沿着x、y和z方向的半径，p_x，p_y和p_z是用来定义障碍物形状的整数。本实施例中设定p_x＝p_y＝p_z＝8，得到的障碍物是盒子的形状。

第二种为用于模拟山地飞行环境的第二环境约束，采用如下不等式表示：

式中，(x_i，y_i)是第i坐山峰的中心坐标，h_i代表第i坐山峰的峰值高度，x_si和y_si代表第i坐山峰分别沿着x轴和y轴方向的衰减量，n代表山峰的总数量。

所述无人机自身的性能指标约束包括无人机终端约束、连续状态不等式约束和控制输入约束，其中，连续状态不等式约束包括无人机海拔高度约束、速度约束和角度约束：

终端约束为

X(t)＝X_f， (9)

无人机海拔高度约束为

0≤z(t)≤z_max， (10)

速度约束为

角度约束为

控制输入约束为

u_min≤|u_i(t)|≤u_max，i＝1，2，3，4。 (13)

至此，无人机路径规划问题被建模为一个最优控制问题，在初始条件X₀下，求出最优的控制输入u(t)使得终端时间T最小，并且以上的各项约束均得到满足。

(S2)控制参数化：选取等间隔p+1的个点，把[0，T]分成p段，选取的点τ为：

0＝τ₀＜τ₁＜τ₂＜…＜T_p-1＜τ_p＝T， (14)

根据公式

将控制变量u(t)转化为多段分段的常量，如图2所示，

定义

基于无人机飞行时间作为优化的目标函数，问题为自由终端时间问题，采用时间压缩的方法，无人机控制状态可转化为

其中，X为状态变量，s为被压缩的时间变量，

当p足够大时，该式的次优解为无人机的最小化飞行时间的非线性最优控制问题的最优解的近似值；本实施例中取值p＝20。

(S3)约束转录处理：非线性规划中的连续状态不等式约束需要在每一个时刻都被满足，这样每一个约束都包含无穷多个约束，为此，本实施例采用约束转录方法把无人机连续状态不等式约束转化为积分等式约束，如下：

其中，n_g为连续状态不等式约束的个数。

由于得到的等式约束是不可微的，因此对该积分等式约束进行局部平滑处理，如图3所示，采用如下公式进行局部平滑处理：

获得转化后的约束条件：

其中，ε为平滑处理函数中大于0的系数，该系数越接近0，平滑函数与原约束的近似性越好，γ为平滑处理的允许误差，其越趋近于0，处理的精度越高。

经过步骤(S1)把无人机轨迹规划问题建模为非线性优化问题记为问题P，通过步骤(S2)和(S3)把该问题P转化为迭代寻优问题，记为问题P_ε，γ。

经过上述转化，问题可以通过以下梯度下降算法求解：

首先初始化参数为：ε＝10^-1，

ε_min＝10^-3。

第一步：求解问题P_ε，γ，求出最优解；

第二步：把最优解代入每一个约束，检查约束g_i(t)≥0的可行性；

第三步：如果第二步中所有的约束都满足则执行第五步，否则执行第四步；

第四步：设定

跳转至第一步；

第五步：设定

跳转至第一步；

算法迭代停止条件：当ε≤ε_min时，该算法停止。

由于本实施例基于梯度下降算法，所以要求解目标函数、终端约束以及不等式约束的梯度公式，本实施例中还给出了相关定理的推导过程。此处将导出代价函数和连续状态不等式约束对k的梯度公式，其中k是问题P_ε，γ的解。

定理1.代价函数的梯度公式为：

λ₀代表如下协态方程的解：

边界条件为：

λ₀(1)＝0 (24)。

定理2.连续状态不等式约束的梯度公式为：

λ_i(s)是以下协态方程的解：

它的边界条件为：

定理3.终端等式约束求梯度的函数为：

式中λ(s)是如下协态方程的解：

边界条件如下：

(S4)利用梯度下降算法对转化后的约束条件进行优化(如上述定理1、2、3)，将无人机轨迹规划问题转化为非线性优化问题，使用MISER3.2软件进行求解，获得无人机基于控制参数化的最优路径规划结果，并反馈至无人机控制系统执行。

(S5)将步骤(S4)获得的无人机基于控制参数化的最优路径规划结果写入闭环PD控制器内对无人机进行路径规划控制。

基于上述方法，本实施例还提供了数值仿真结果分析：通过如下四个例子对本发明方法进行了验证，所采用的无人机的性能指标相关参数如表一所示。

参数	取值	参数	取值
				L(m)	0.2	K<sub>1</sub>(N/m/s)	0.06
M(kg)	1.5	K<sub>2</sub>(N/m/s)	0.06
				g(m/s<sup>2</sup>)	9.8	K<sub>3</sub>(N/m/s)	0.09
I<sub>x</sub>(kg·s<sup>2</sup>)	0.0075	K<sub>4</sub>(N/m/s)	0.002
				I<sub>y</sub>(kg·s<sup>2</sup>)	0.0075	K<sub>5</sub>(N/m/s)	0.002
I<sub>z</sub>(kg·s<sup>2</sup>)	0.013	K<sub>6</sub>(N/m/s)	0.1

表一 无人机的性能指标参数

例子一，模拟了无人机在无障碍物环境中的飞行，如图4所示；

例子二，模拟了无人机在城市环境中的飞行(俯视)，如图5所示；

例子三，模拟了无人机在山地环境中的飞行，如图6所示；数值仿真结果表明，本方法能使无人机在以上环境中找到一条无碰撞的时间最优路径，并且保证在飞行过程中所有的状态约束都能得到满足。

例子四中，通过闭环PD控制器对本方法产生的最优路径与RRT方法产生的最优路径分别进行轨迹跟踪，如图7所示，本方法应用于四旋翼无人机上时通过轨迹跟踪方式获得的跟踪轨迹与本方法计算获得的最优路径的参考轨迹基本重合，误差小，如图8所示，常规RRT方法通过轨迹跟踪方式获得的跟踪轨迹与其参考轨迹存在明显偏差，尤其是上升到一定高度后无人机的实际飞行轨迹明显出现脱离所计算获得的参考轨迹的情况，其误差较大，对比可知，本方法的路径规划方法跟踪误差更小；如图9和图10所示对比可知，本方法在跟踪过程中电机转速变化更平稳。

上述实施例仅为本发明的优选实施例，并非对本发明保护范围的限制，但凡采用本发明的设计原理，以及在此基础上进行非创造性劳动而做出的变化，均应属于本发明的保护范围之内。

Claims (8)

1.一种基于控制参数化的无人机路径规划方法，其特征在于，包括以下步骤：

(S1)非线性规划建模：根据无人机的非线性动力学方程选取状态变量和控制变量并将其改写为状态空间模型的形式，利用无人机自身的性能指标约束和环境中的障碍物约束，建模为无人机连续状态不等式约束，把无人机的初始状态和目标点作为边界约束条件，把无人机飞行时间作为优化的目标函数，将无人机路径规划问题转化为无人机的最小化飞行时间的非线性最优控制问题，求解最优的控制变量u(t)使得飞行时间T最小，并满足各项约束条件；

(S2)控制参数化：选取等间隔的p+1个点，把[0，T]分成p段，选取的点τ为0＝τ₀<τ₁<τ₂<…<τ_p-1<τ_p＝T，根据公式

i＝1,2,3,4将控制变量u(t)转化为多段分段的常量，定义

基于无人机飞行时间作为优化的目标函数，采用时间压缩的方法，无人机控制状态可转化为

其中，X为状态变量，s为被压缩的时间变量，当p足够大时，该式的次优解为无人机的最小化飞行时间的非线性最优控制问题的最优解；

(S3)约束转录处理：采用约束转录方法把无人机连续状态不等式约束转化为积分等式约束，并对该积分等式约束进行局部平滑处理，获得转化后的约束条件；

(S4)利用梯度下降算法对转化后的约束条件进行优化，将无人机轨迹规划问题转化为非线性优化问题，使用MISER3.2软件进行求解，获得无人机基于控制参数化的最优路径规划结果。

2.根据权利要求1所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S1)中，根据无人机的非线性动力学方程选取状态变量和控制变量并将其改写为状态空间模型形式的过程为：

设定无人机运动学模型如下：

其中，x,y,z是无人机在空间中的位置坐标，θ，ψ,

是三个欧拉角，分别代表俯仰角、滚转角和偏航角，g是重力加速度，L是无人机电机到重心的臂长，m是无人机的总质量，I_x,I_y,I_z是无人机分别对每个轴的转动惯量，K₁，K₂,K₃，K₄，K₅，K₆是空气阻力系数，u₁,u₂,u₃,u₄是如下定义的虚拟的控制输入量：

F_i＝K_vω_i ²,i＝1,2,3,4

式中，F_i是第i个电机产生的推力，C是推力和力矩的转化因子，ω_i是第个电机的转速，K_v是推力系数；

状态变量定义如下：

控制变量定义如下：

u＝[u₁ u₂ u₃ u₄]^T

将所述无人机运动学模型改写为状态空间表达式的形式如下：

3.根据权利要求2所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S1)中，环境中的障碍物约束包括根据p-norm规则建立的用来模拟无人机在城市飞行环境的第一环境约束，采用如下不等式表示：

式中，(x_c,y_c，z_c)是障碍物的中心坐标，a，b，c代表障碍物分别沿着x、y和z方向的半径，p_x，p_y和p_z是用来定义障碍物形状的整数。

4.根据权利要求2所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S1)中，环境中的障碍物约束包括用于模拟山地飞行环境的第二环境约束，采用如下不等式表示：

式中，(x_i，y_i)是第i坐山峰的中心坐标，h_i代表第i坐山峰的峰值高度，x_si和y_si代表第i坐山峰分别沿着x轴和y轴方向的衰减量，n代表山峰的总数量。

5.根据权利要求2所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S1)中，无人机自身的性能指标约束包括无人机终端约束、连续状态不等式约束和控制输入约束，其中，

终端约束为X(t)＝X_f，

连续状态不等式约束包括无人机海拔高度约束0≤z(t)≤z_max，速度约束

角度约束θ_min≤θ(t)≤θ_max，ψ_min≤ψ(t)≤ψ_max，

控制输入约束为u_min≤|u_i(t)|≤u_max,i＝1,2,3,4。

6.根据权利要求2所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S3)中，由无人机连续状态不等式约束转化的积分等式约束为

其中n_g为连续状态不等式约束的个数。

7.根据权利要求6所述的一种基于控制参数化的无人机路径规划方法，其特征在于，所述步骤(S3)中，采用如下公式进行局部平滑处理：

由此获得的转化后的约束条件为

其中，ε为平滑处理函数中大于0的系数，γ为平滑处理的允许误差。

8.根据权利要求1～7任一项所述的一种基于控制参数化的无人机路径规划方法，其特征在于，还包括：

(S5)将步骤(S4)获得的无人机基于控制参数化的最优路径规划结果写入闭环PD控制器内对无人机进行路径规划控制。

Images