Trajectory optimization algorithm of vehicle-mounted rotor UAV with discrete mechanics and optimal control

Yanjun LU; Chang LIU; Xiaodong ZHANG; Dongyu WANG

doi:10.3969/j.issn.2095-1248.2025.05.007

Journal of Shenyang Aerospace University >

2025 , Vol. 42 >Issue 5: 53 - 59

DOI: https://doi.org/10.3969/j.issn.2095-1248.2025.05.007

Information Science and Engineering

Trajectory optimization algorithm of vehicle-mounted rotor UAV with discrete mechanics and optimal control

Yanjun LU ,
Chang LIU ,
Xiaodong ZHANG ,
Dongyu WANG

Expand

College of Automation，Shenyang Aerospace University，Shenyang 110136，China

Received date: 2024-10-09

Revised date: 2024-10-14

Accepted date: 2024-10-19

Online published: 2025-12-04

Fold

Abstract

In the context of cooperative operation between UAV and ground vehicle， rotor UAV often needs to pass through necessary points or avoid obstacles when executing related tasks， which increases the trajectory fluctuation and brings more challenges to path planning. To solve this problem， an improved discrete mechanics and optimal control （DMOC） trajectory optimization algorithm was proposed， which transformed the optimal control problem into a nonlinear programming problem. Considering the endurance time of vehicle-mounted rotor UAV， the shortest time was set as the optimization objective under guaranteed traversal conditions. In the process of the algorithm implementation， an approach was proposed to adjust the distance walk length according to the size of the error factor， and then approximate correlation integral， which effectively solved the problem of trajectory fluctuation. The experimental results show that this method can improve the smoothness of the optimal trajectory while ensuring that all the necessary points are traversed，and also provides a reference for the trajectory optimization of other agents.

Key words： discrete mechanics and optimal control; nonlinear programming; vehicle-mounted rotor UAV; distance walk length; trajectory optimization

Cite this article

Yanjun LU , Chang LIU , Xiaodong ZHANG , Dongyu WANG . Trajectory optimization algorithm of vehicle-mounted rotor UAV with discrete mechanics and optimal control[J]. Journal of Shenyang Aerospace University, 2025 , 42(5) : 53 -59 . DOI: 10.3969/j.issn.2095-1248.2025.05.007

旋翼无人机与地面车辆协同作业是空地协同的典型代表，其广泛应用于应急救援^［1］、智能交通^［2］和复杂环境探索^［3］等民用领域^［4］。现阶段由于电池技术的限制，无人机的续航时间都相对较短，无人机在执行相关任务的时候，有必要提前规划出一条最优路径，从而减少不必要的时间。

旋翼无人机执行相关任务时往往需要通过必经点或躲避障碍物，从而使轨迹的波动增多，给路径规划带来更多挑战。目前，关于轨迹优化算法的研究有很多，如迪杰斯特拉（Dijkstra）^［5］和A*算法^［6］等，每种算法都有其特定的应用场景和优势。四旋翼无人机是典型的欠驱动、多变量、强耦合系统^［7］，建立准确的数学模型较为困难，而DMOC不需要无人机的准确模型，可直接将无人机轨迹优化为非线性规划问题，并找到具有全局收敛性的全局最优解。Junge等^［8］在2005年提出一种离散力学最优控制方法，并证明了此方法的可行性。高磊等^［9］在目标函数中加入惩罚项使控制力平滑，解决了原始DMOC算法在求解优化问题时容易产生控制力震荡的问题，但在求解过程中容易陷入局部最小值。王丹^［10］提出多相DMOC，通过对轨迹优化进行分段处理，提高了最优轨迹生成效率，初步解决了局部最小值问题，并应用到了无人机上，但在处理较为平滑的函数时会因分段策略而增加计算时间。综上，DMOC还存在诸多不足需要改进。

本文针对空地协同作业环境下车载旋翼无人机的轨迹优化问题进行研究。原始DMOC算法在优化过程中仅聚焦于最小化路径耗时，在函数波动较大的情况下存在显著局限：步长设置过大易引发误差扩大，步长设置过小则显著增加计算时间复杂度。为有效解决这一问题，本文提出一种自适应步长调整策略的DMOC方法进行更高效、更精准的轨迹规划。

1 原始DMOC方法描述

一个普遍的优化问题可以描述为：在已知初始和终止状态下，求系统最少消耗时间t和系统的最小控制力

f

，使得目标函数

J

达到最优^［11］。

定义系统的状态方程

q ˙ (t) = f (q (t), τ (t))

（1）

式中：

q (t)

为系统的控制输入；

τ (t)

为系统的总消耗时间。

考虑一个空间

Q

中的力学系统，在时间

[t 0, t 1]

内，系统受力

f

的控制由初始状态

(q (t 0), q ˙ (t 0))

经曲线

q (t) ∈ Q

到达终止状态

(q (t 1), q ˙ (t 1))

的目标函数。

J (q, f) = Φ 0 (q (t 0), q ˙ (t 0)) + ∫ t 0 t 1 C (q (t), q ˙ (t), f (t)) d t + Φ 1 (q (t 1), q ˙ (t 1))

（2）

式中：

C

为动态成本函数；

Φ

为状态成本函数。

系统运动过程中的

q (t)

应满足Lagrange-D’Alembert（拉格朗日-达朗贝尔）^［8］原理，即

δ ∫ t 0 t 1 L (q (t), q ˙ (t)) d t + ∫ t 0 t 1 f (t) δ q (t) d t = 0

（3）

式中：所有变量

δ

q（·）均满足

δ q (t 0) = δ q (t 1) = 0

，

L

为拉格朗日量，定义为系统动能T与势能V的差，即：

L (q (t)

，

q ˙ (t)) = T (q ˙ (t)) - V q t

。

DMOC方法是对系统的目标函数和约束，同时直接利用拉格朗日-达朗贝尔原理离散化，将连续的状态空间

T Q

替换为离散状态空间

Q × Q

，将路径

q : 0,1 → Q

在时间点

[t 0, t 1]

上离散成

q d : {0, h, 2 h, ⋯, N h} → Q

，其中

h

为离散步长，

N

为初始到末态指定时间段内的离散步数，它们之间满足

t 1 - t 0 = N h

。DMOC简单的离散化过程如下^［12］：

1）离散的拉格朗日-达朗贝尔原理

离散的拉格朗日-达朗贝尔原理为在每个时间步长内，所有外力和惯性力对系统可能的虚位移所做的虚功之和为零。利用离散的拉格朗日-达朗贝尔原理对式（3）中的路径

q d

进行限制以满足

δ ∑ k = 0 N - 1 L d (q k, q k + 1) + ∑ k = 0 N - 1 (f k - δ q k + f k + q k + 1) = 0

（4）

其中：

L d (q k, q k + 1)

是离散的拉格朗日量，定义为

L d (q k, q k + 1) = h L (q k + 1 + q k 2, q k + 1 - q k h)

。

所有变量

q d

均满足

δ q 0 = δ q k = 0

。因此得到离散Euler-Lagrange（欧拉-拉格朗日）等式为

D 2 L d (q k - 1, q k) + D 1 L d (q k, q k + 1) + f k - 1 + + f k - = 0

（5）

式中：

D i

，i=1，2为对第

i

项求偏导；

f k - 1 +

和

f k -

分别为第

k

步的左、右离散力，且满足梯形算法

f k - = f k + = h 2 (f k + f k + 1)

近似相关积分^［10］，

k = 1,2, ⋯, N - 1

。

2）离散边界条件

为了保证内部点连续的边界条件，需要添加离散边界条件：

q (t 0) = q 0

，

q ˙ (t 0) = q ˙ 0

，

q (t 1) = q 1

，

q ˙ (t 1) = q ˙ 1

。详细的离散Legendre（勒让德）变换使用方法可参考文献［12］。使用离散勒让德变换，离散边界条件可表示为

D 2 L (q 0, q ˙ 0) + D 1 L d (q 0, q 1) + f 0 - = 0

（6）

- D 2 L (q N, q ˙ N) + D 2 L d (q N - 1, q N) + f N - 1 + = 0

（7）

3）离散目标函数

离散的拉格朗日量

C d

在每一个时间步长

k h, (k + 1) h

内，状态空间上的公式可以近似地描述为

C d (q k, q k + 1, f k, f k + 1) ≈ ∫ k h (k + 1) h C (q, q ˙, t) d t = h C (q k + 1 + q k 2, q k + 1 - q k h, f k + 1 + f k 2)

（8）

因此对式（2）目标函数整体离散化可表示为

J d (q d, f d) = ∑ k = 0 N - 1 C d (q k, q k + 1, f k, f k + 1)

（9）

2 基于变离散步长改进的DMOC算法

原始DMOC的近似相关积分使用的步长

h

为定值，但是由于函数往往未知，在给定精度条件下，步长

h

难以确定，

h

太大，精度达不到；

h

太小，计算量大导致系统延迟。为避免定步长在处理波动较多的函数时出现误差，提出基于自适应离散步长改进的DMOC方法，引入精度判断因子

ξ, ξ ∈ 0,1

，如式（10）所示。

ξ = h n 2 f k + 1 / 2 - h n 4 (f k + f k + 1) h n 2 f k + 1 / 2 + h n 4 (f k + f k + 1)

（10）

当式（10）中

h n 2 f k + 1 / 2 → 0

时，则

h n → 0

，

ξ ≈ 1

，精度判断因子

ξ

数值越接近1，近似相关积分越准确；当精度判断因子

ξ

大于阈值

ξ 0

时，开启自适应步长近似相关积分，否则使用定步长近似相关积分，考虑到计算量，本文

ξ 0

取0.5。

h 0 4 (f k + f k + 1), ξ ≤ ξ 0 h n 4 (f k + f k + 1) + h n 2 f k + 1 / 2, ξ > ξ 0

（11）

式中：

h n

为将步长

h

进行

n

等分。

加入自适应步长近似相关积分离散后的目标函数可表示为

J d (q d, f d) = ∑ k = 0 N - 1 C d (q k, q k + 1, f k, f k + 1), ξ ≤ ξ 0 ∑ k = 0 N - 1 C d (q k, q k + 1, q k + 1 / 2, f k, f k + 1, f k + 1 / 2), ξ > ξ 0

（12）

经过上述步骤，机械系统转换需满足式（5）—（7）3个约束条件，最终达到最小离散代价函数式（9）的非线性规划问题。

最后，在AMPL软件中建立求解模型，使用开源非线性控制解算器SQP进行求解^［13］，DMOC求解过程流程图如图1所示。

显示原图|下载原图ZIP|生成PPT

图1 DMOC求解过程流程图

3 四旋翼无人机器轨迹优化

3.1 坐标系定义和变换

四旋翼无人机是典型的非线性系统，具有高耦合、多变量的特点^［14］。为了清楚地描述无人机的当前姿态和空间位置，通常建立满足笛卡尔右手原则^［15］的机体坐标系

S b - o x y z

和惯性坐标系

S i - o x y z

，由机体坐标系

S b

转换到惯性坐标系

S i

的转换矩阵为

R B I = R z (ψ) R y (θ) R x (ϕ) = C θ C ψ S θ S ϕ C ψ - C ϕ S ψ C ϕ S θ C ψ + S ϕ S ψ C θ S ψ S ϕ S θ S ψ + C ϕ C ψ C ϕ S θ S ψ - S ϕ C ψ - S θ S ϕ C θ C ϕ C θ

（13）

式中：

C (·)

和

S (·)

分别为余弦函数和正弦函数，建立简化的四旋翼非线性动力学模型如式（14）所示。

x ˙ = v x y ˙ = v y z ˙ = v z v ˙ x = [U 1 (C ϕ S θ C ψ + S ϕ S ψ) - K d x ˙] / m v ˙ y = [U 1 (C ϕ S θ S ψ - S ϕ C ψ) - K d y ˙] / m v ˙ z = (U 1 C ϕ C θ - m g - K d z ˙) / m ϕ ˙ = p θ ˙ = q ψ ˙ = r p ˙ = I U 2 + (I y - I z) q r / I x q ˙ = I U 3 + (I z - I x) p r / I y r ˙ = I U 4 + (I x - I y) p q / I z

（14）

3.2 空地协同中四旋翼无人机任务描述

在空地协同飞行器任务（如图2所示）中，假设无人机在8 m高的平面上进行巡航任务，在（8，2.5，7）悬停点开始返航，在返航的过程中要躲避圆心为（4，1，0）、半径为0.6 m、高为6 m、的圆柱障碍区域，同时无人车在（0.6，5.5，0）出发以0.36 m/s的速度做直线运动，无人机最终降落到移动的无人车上。将实际问题需求利用改进的DMOC方法转化为数学问题，可描述为满足以下条件的非线性规划问题。

显示原图|下载原图ZIP|生成PPT

图2 空地协同飞行器任务

根据无人机特性，设无人机有三自由度，即坐标

(x, y, z)

，初始和终止条件为

(x (0), y (0), z (0)) = (8,2 . 5,7) (x ˙ (0), y ˙ (0), z ˙ (0)) = (0,0, 0)

（15）

(x (1), y (1), z (1)) = (0,5.5 - 0 . 36 t, 0) (x ˙ (1), y ˙ (1), z ˙ (1)) = (0,0, 0)

（16）

根据四旋翼无人机的非线性动力学方程式（14）确定该系统的状态向量为

x (t) = [x y z

v x

v y

v z

ϕ

ϕ

θ

ψ

p

q

r

］^T （17）

系统控制输入向量为

U = U 1 U 2 U 3 U 4 T

（18）

为保证无人机的飞行安全，无人机姿态角限制在

- π 2, π 2

区间，无人机控制输入限制在

0,12 000 r / m i n

区间。

无人机的运动由连续时间状态方程描述为

x ˙ (t) = f (x (t), u (t), t)

（19）

式中：

x (t)

为无人机的状态向量；

u (t)

为控制输入向量；t为时间。

系统的拉格朗日量函数定义为

L (q, q ˙) = 12 m (x ˙ 2 + y ˙ 2 + z ˙ 2) - m g h

，其中

q = (x, y, z)

，

m

为无人机质量。在控制力

f x

、

f y

、

f z

作用下，无人机从起始点迅速抵达目的地。为使无人机系统消耗时间最短，系统的目标函数为

J = τ + λ ∫ t 0 t 1 (f x 2 + f y 2 + f z 2) d t

（20）

其中：

τ

为从初始时间

t 0

到最终时间

t 1

的时间差，初始时间

t 0 = 0 s

；

λ

为一个权重因子，平衡时间成本和控制成本。

控制力表示为

f (t) = U 1 (C ϕ S θ C ψ + S ϕ S ψ) U 1 (C ϕ S θ S ψ - S ϕ C ψ) U 1 C ϕ C θ - m g

（21）

利用

h 2 (f k + f k + 1)

梯形算法近似相关积分对系统进行离散化。

离散后的目标函数为

J d = τ + λ h 4 ∑ k = 0 N - 1 f x, k + 1 + f x, k 2 + f y, k + 1 + f y, k 2 + f z, k + 1 + f z, k 2

（22）

加入自适应步长近似相关积分离散后的目标函数为

J d = τ + λ h 0 4 ∑ k = 0 N - 1 f x, k + 1 + f x, k 2 + f y, k + 1 + f y, k 2 + f z, k + 1 + f z, k 2, ξ ≤ ξ 0 τ + λ h n 4 ∑ k = 0 N - 1 f x, k + 1 + f x, k 2 + f y, k + 1 + f y, k 2 + f z, k + 1 + f z, k 2 + λ h n 2 ∑ k = 0 N - 1 f x, k + 1 / 2 2 + f y, k + 1 / 2 2 + f z, k + 1 / 2 2, ξ > ξ 0

（23）

离散后的约束条件为

M h 2 x (k) - x (k - 1) - x (k + 1) 2 y (k) - y (k - 1) - y (k + 1) 2 z (k) - z (k - 1) - z (k + 1) + h 4 2 f x (k) + f x (k - 1) + f x (k + 1) 2 f y (k) + f y (k - 1) + f y (k + 1) 2 f z (k) + f z (k - 1) + f z (k + 1) = 0 M x ˙ (0) y ˙ (0) z ˙ (0) + M h x (0) - x (1) y (0) - y (1) z (0) - z (1) + h 4 f x (0) + f x (1) f y (0) + f y (1) f z (0) + f z (1) = 0 - M x ˙ (N) y ˙ (N) z ˙ (N) + M h x (N) - x (N - 1) y (N) - y (N - 1) z (N) - z (N - 1) + h 4 f x (N) + f x (N - 1) f y (N) + f y (N - 1) f z (N) + f z (N - 1) = 0

（24）

式中：

M = m 00 0 m 0 00 J

为系统的质量矩阵。

考虑到无人机的特殊性，在实际应用环境中难免会存在禁飞区或障碍物，所以在实验中设定禁飞区圆心为（4，1，0）、半径为0.6 m、高为6 m的圆柱形区域。

4 仿真实验与分析

仿真实验以四旋翼无人机为仿真对象，首先在AMPL中创建求解模型，结合非线性控制解算器SQP进行求解，最后在MATLB中拟合出最优轨迹（求解环境：Intel core i5-11400 @2.60 GHz，8 GB内存，操作系统为Windows 11，处理器为64 bit）。

对改进的DMOC、原始DMOC生成的轨迹和路径规划Dijkstra算法三者进行对比。数值仿真试验统一使用解算器SQP进行求解，退出迭代条件为满足违反约束条件和目标函数的容许误差小于

10 - 6

。生成的改进DMOC、原始DMOC和Dijkstra算法产生的三维轨迹以及轨迹的X-Y关系分别如图3、4所示，轨迹的姿态-时间关系对比如图5所示，主要数据对比如表1所示。

显示原图|下载原图ZIP|生成PPT

图3 改进DMOC、原始DMOC和Dijkstra算法产生的三维轨迹

显示原图|下载原图ZIP|生成PPT

图4 轨迹的 X-Y 关系

显示原图|下载原图ZIP|生成PPT

图5 轨迹的姿态-时间关系对比图

表1 主要数据对比

算法	离散点数/个	迭代次数/次	计算时间/s
改进DMOC	20	130	0.203 0
DMOC	24	121	0.238 0
Dijkstra	120	1 004	70.324 5

在同样的初始条件、约束条件、边界条件下，改进DMOC、原始DMOC和Dijkstra算法均可找到相对光滑的最优轨迹。在图4轨迹的X-Y图中可以明显看出，Dijkstra算法直接穿越了障碍区域，而改进DMOC和原始DMOC都完全绕过了障碍区域。通过图4可以得出，在降落末端，由于降落目标点变化，需要无人机实时做出更改，会存在较大的波动，改进DMOC明显优于原始DMOC，更接近需要降落的移动目标点。由于无人机姿态角限制在

- π 2, π 2

区间，所以，根据图5可知，Dijkstra算法的横滚和俯仰姿态均出现不可控现象，原始DMOC算法在俯仰姿态控制中出现一小段不可控现象，而改进DMOC的姿态变化曲线满足约束条件完全可控。根据表1可知，相比于原始的DMOC算法，加入精度判断因子的改进DMOC算法，在求解的过程中所需离散点个数更少，计算量更小，迭代次数更多，所用时间更短，得到的轨迹曲线更加准确、光滑、可控。

5 结论

本文利用原始DMOC方法将最优控制问题转化为非线性规划问题，并应用到了空地协同车载无人机轨迹优化问题中。原始DMOC采用的定步长离散化，在处理波动剧烈的曲线时容易导致误差和计算量大问题，本文提出变步长改进的DMOC算法，在离散的过程中根据误差因子大小调整离散步长。实验结果表明，相比原始DMOC和Dijkstra轨迹优化算法，在轨迹优化过程中，该方法在保证遍历所有必经点的同时，更加平滑，用时更短，可为车载无人机提供最优轨迹，具有一定的实际工程应用价值。

References

Publishing order | Descend order by publishing year | Descend order by cited within

[1]	王宇辰.小型无人直升机飞行控制系统及其试验台的研究［D］.北京：北京理工大学，2015.

[2]	樊宝安.基于高速公路违章检测的四旋翼无人机平台的设计与实现［D］.南京：南京邮电大学，2020.

[3]	高宏建，汪阳生，陈霖周廷，等.复杂风场环境对四旋翼无人机运动特性影响分析［J］.计算机测量与控制，2025，33（2）：192-202，211.

[4]	袁泽慧，陈国栋，李世中.四旋翼在移动平台视觉导引自主降落研究综述［J］.兵器装备工程学报，2023，44（4）：60-67.

[5]	Dhulkefl E， Durdu A， Terzioğlu H，et al.Dijkstra algorithm using UAV path planning［J］.Konya Journal of Engineening Sciences，2020，8：92-105.

[6]	孙岩霆，王荣杰，蒋德松.融合A^*与DWA算法的水面船艇动态路径规划［J］.仪器仪表学报，2024，45（1）：301-310.

[7]	刘昌龙.四旋翼无人机建模与控制问题研究［D］.武汉：湖北工业大学，2016.

[8]	Junge O， Marsden J E， Ober-Blöbaum S.Discrete mechanics and optimal control［J］.IFAC Procee-dings Volumes，2005，38（1）：538-543.

[9]	高磊，于文洁.一种带梯度惩罚项的离散力学最优控制方法［J］.力学与实践，2015，37（4）：499-502，498.

[10]	王丹.基于DMOC的四旋翼飞行器轨迹优化与控制算法研究［D］.北京：北京理工大学，2015.

[11]	Ober-Blöbaum S， Junge O， Marsden J E.Discrete mechanics and optimal control：an analysis［J］.ESAIM：Control，Optimisation and Calculus of Variations，2011，17（2）：322-352.

[12]	Zhang W Z， Inanc T.A tutorial for applying DMOC to solve optimization control problems［C］//2010 IEEE International Conference on Systems，Man and Cybernetics.Turkey：IEEE，2010：1857-1862.

[13]	Wächter A， Biegler L T.On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming［J］.Mathematical Programming，2006，106（1）：25-57.

[14]	Zu Y Y， Zhang W Z， Shan J Y.A trajectory design method for quadrotor based on DMOC method and Dubins path algorithm［C］//2017 36th Chinese Control Conference.Dalian：IEEE，2017：2528-2533.

[15]	王佩臣，王强，张可为，等. 四旋翼无人飞行器控制律设计与仿真［J］. 黑龙江工程学院学报，2023，37（2）：1671-4679.

Options

Outlines

模态框（Modal）标题

Abstract

Cite this article

1 原始DMOC方法描述

2 基于变离散步长改进的DMOC算法

图1 DMOC求解过程流程图

3 四旋翼无人机器轨迹优化

3.1 坐标系定义和变换

3.2 空地协同中四旋翼无人机任务描述

图2 空地协同飞行器任务

4 仿真实验与分析

图3 改进DMOC、原始DMOC和Dijkstra算法产生的三维轨迹

图4 轨迹的 X-Y 关系

图5 轨迹的姿态-时间关系对比图

表1 主要数据对比

5 结论

References