CN114089780B - 一种面向城市空间的多旋翼无人机路径规划方法 - Google Patents

一种面向城市空间的多旋翼无人机路径规划方法 Download PDF

Info

Publication number
CN114089780B
CN114089780B CN202210073036.7A CN202210073036A CN114089780B CN 114089780 B CN114089780 B CN 114089780B CN 202210073036 A CN202210073036 A CN 202210073036A CN 114089780 B CN114089780 B CN 114089780B
Authority
CN
China
Prior art keywords
unmanned aerial
rotor unmanned
aerial vehicle
follows
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210073036.7A
Other languages
English (en)
Other versions
CN114089780A (zh
Inventor
刘延杰
牛耕田
王超
陈忠
朱峰
韩东
刘程威
陆萍
刘佳
张原�
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CETC 28 Research Institute
Original Assignee
CETC 28 Research Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CETC 28 Research Institute filed Critical CETC 28 Research Institute
Priority to CN202210073036.7A priority Critical patent/CN114089780B/zh
Publication of CN114089780A publication Critical patent/CN114089780A/zh
Application granted granted Critical
Publication of CN114089780B publication Critical patent/CN114089780B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种面向城市空间的多旋翼无人机路径规划方法，具体步骤为：步骤1，确定多旋翼无人机动力学模型；步骤2，确定多旋翼无人机轨迹优化约束条件及优化指标；步骤3、建立多旋翼无人机动力学模型与障碍物约束线性化；步骤4、建立多旋翼无人机动力学方程及约束条件离散化；步骤5，求解完成面向城市空间的多旋翼无人机路径规划。面向城市环境进行约束条件设置，以能量消耗最低作为优化指标建立多旋翼无人机城市路径规划方程，通过线性化、离散化进行问题转换，通过内点法进行问题求解，降低了问题复杂度，提升了计算效率。有效提升了多旋翼无人机在城市环境下的应用效能。

Description

一种面向城市空间的多旋翼无人机路径规划方法
技术领域
本发明涉及一种无人机路径规划方法，特别是一种面向城市空间的多旋翼无人机路径规划方法。
背景技术
旋翼无人机是一种能够垂直起降、自主悬停的多旋翼飞行器，在航拍摄影、精确农业、电力巡检、物流运输等领域得到广泛应用。城市环境下，高层建筑物分布密集导致多旋翼无人机飞行障碍物多，人员车辆流动频繁导致多旋翼无人机追踪定位目标困难。此外，多旋翼无人机具有非线性、耦合性、多变量等特点，这些都给轨迹规划的准确性和实时性带来了挑战。因此，需要设计面向城市空间的多旋翼无人机路径规划算法以提升其在城市环境下的应用效能。
在已发展的多旋翼无人机轨迹优化算法中，在先技术（参见张博渊,宗群,鲁瀚辰等. 基于hp自适应伪谱法的多旋翼无人机编队轨迹优化. 中国科学：技术科学,2017,47:239–248）考虑多旋翼无人机的避障约束、状态量约束及控制输入量约束，采用可自适应选点的hp自适应伪谱法将最优控制问题转化为非线性规划问题进行求解。非线性规划问题求解复杂，轨迹规划的实时性难以保证。
在先技术（参见Optimal trajectory generation and robust flatness–basedtracking control of quadrotors）基于简化的动力学模型进行轨迹优化解算，同时采用B平面法进行轨迹跟踪算法设计。但基于简化的动力学模型降低了优化结果的精度，复杂优化指标的选取也进一步增加了计算量。
发明内容
发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种面向城市空间的多旋翼无人机路径规划方法。
为了解决上述技术问题，本发明公开了一种面向城市空间的多旋翼无人机路径规划方法，包括以下步骤：
步骤1，确定多旋翼无人机动力学模型；
步骤2，确定多旋翼无人机轨迹优化约束条件及优化指标；
步骤3，建立多旋翼无人机动力学模型与障碍物约束线性化；
步骤4，建立多旋翼无人机动力学方程及约束条件离散化；
步骤5，求解完成面向城市空间的多旋翼无人机路径规划。
步骤1中所述多旋翼无人机动力学模型如下：
Figure 555821DEST_PATH_IMAGE001
其中， x、 y和 z分别表示多旋翼无人机的三轴位置坐标，
Figure 650816DEST_PATH_IMAGE002
、
Figure 772356DEST_PATH_IMAGE003
和
Figure 548682DEST_PATH_IMAGE004
分别表示多旋翼无人机沿三轴的速度分量，
Figure 173698DEST_PATH_IMAGE005
、
Figure 705174DEST_PATH_IMAGE006
和
Figure 508086DEST_PATH_IMAGE007
分别表示三轴位置坐标关于时间的导数，
Figure 150420DEST_PATH_IMAGE008
、
Figure 629943DEST_PATH_IMAGE009
和
Figure 332320DEST_PATH_IMAGE010
分别表示三轴位置坐标关于时间的二阶导数；
Figure 897293DEST_PATH_IMAGE011
、
Figure 15422DEST_PATH_IMAGE012
和
Figure 113566DEST_PATH_IMAGE013
分别表示多旋翼无人机的俯仰角、滚转角和偏航角， p、q和 r分别表示多旋翼无人机的俯仰角速度、滚转角速度和偏航角速度，
Figure 986844DEST_PATH_IMAGE014
、
Figure 39114DEST_PATH_IMAGE015
和
Figure 960933DEST_PATH_IMAGE016
表示姿态角关于时间的一阶导数，
Figure 415048DEST_PATH_IMAGE017
、
Figure 459228DEST_PATH_IMAGE018
和
Figure 700591DEST_PATH_IMAGE019
表示姿态角关于时间的二阶导数；
Figure 222839DEST_PATH_IMAGE020
、
Figure 797040DEST_PATH_IMAGE021
和
Figure 746541DEST_PATH_IMAGE022
表示多旋翼无人机对应三个轴的转动惯量； m表示多旋翼无人机的质量，
Figure 711086DEST_PATH_IMAGE023
表示升力作用点，即多旋翼无人机螺旋桨中心到质心的距离；
Figure 302605DEST_PATH_IMAGE024
、
Figure 167530DEST_PATH_IMAGE025
、
Figure 287933DEST_PATH_IMAGE026
和
Figure 67670DEST_PATH_IMAGE027
分别表示多旋翼无人机的四个控制输入量， g表示重力加速度。
步骤2中多旋翼无人机的初始状态约束定义如下：
Figure 931721DEST_PATH_IMAGE028
其中，
Figure 152618DEST_PATH_IMAGE029
、
Figure 709501DEST_PATH_IMAGE030
和
Figure 710955DEST_PATH_IMAGE031
分别表示多旋翼无人机在初始时刻的三轴位置坐标，
Figure 611653DEST_PATH_IMAGE032
、
Figure 952635DEST_PATH_IMAGE033
和
Figure 680420DEST_PATH_IMAGE034
分别表示多旋翼无人机在初始时刻的沿x轴、y轴和z轴的速度分量，
Figure 903591DEST_PATH_IMAGE035
、
Figure 578286DEST_PATH_IMAGE036
和
Figure 836092DEST_PATH_IMAGE037
分别表示多旋翼无人机在初始时刻的姿态角，
Figure 967734DEST_PATH_IMAGE038
、
Figure 881463DEST_PATH_IMAGE039
和
Figure 891007DEST_PATH_IMAGE040
分别表示多旋翼无人机在初始时刻姿态角速度。
步骤2中多旋翼无人机的终端状态约束定义如下：
Figure 3320DEST_PATH_IMAGE041
其中，
Figure 276169DEST_PATH_IMAGE042
表示飞行时间，
Figure 208353DEST_PATH_IMAGE043
、
Figure 785703DEST_PATH_IMAGE044
和
Figure 752522DEST_PATH_IMAGE045
分别表示多旋翼无人机在
Figure 930694DEST_PATH_IMAGE042
时刻的三轴位置坐标，
Figure 615753DEST_PATH_IMAGE046
、
Figure 232679DEST_PATH_IMAGE047
和
Figure 257267DEST_PATH_IMAGE048
分别表示多旋翼无人机在
Figure 901612DEST_PATH_IMAGE042
时刻的沿 x轴、 y轴和z轴的速度分量，
Figure 73968DEST_PATH_IMAGE049
、
Figure 432268DEST_PATH_IMAGE050
和
Figure 373679DEST_PATH_IMAGE051
分别表示多旋翼无人机在
Figure 955970DEST_PATH_IMAGE042
时刻的姿态角，
Figure 350042DEST_PATH_IMAGE052
、
Figure 744989DEST_PATH_IMAGE053
和
Figure 806486DEST_PATH_IMAGE054
分别表示多旋翼无人机在
Figure 559679DEST_PATH_IMAGE042
时刻姿态角速度。
步骤2中多旋翼无人机的过程约束定义如下：
Figure 378730DEST_PATH_IMAGE055
其中，
Figure 875571DEST_PATH_IMAGE056
、
Figure 791574DEST_PATH_IMAGE057
、
Figure 417465DEST_PATH_IMAGE058
、
Figure 786130DEST_PATH_IMAGE059
、
Figure 86661DEST_PATH_IMAGE060
和
Figure 794854DEST_PATH_IMAGE061
分别表示多旋翼无人机飞行过程中各状态变量的最小值，
Figure 889849DEST_PATH_IMAGE062
、
Figure 745809DEST_PATH_IMAGE063
、
Figure 584452DEST_PATH_IMAGE064
、
Figure 911266DEST_PATH_IMAGE065
、
Figure 442742DEST_PATH_IMAGE066
和
Figure 520419DEST_PATH_IMAGE067
分别表示多旋翼无人机飞行过程中各状态变量的最大值。
步骤2中多旋翼无人机的控制输入约束定义如下：
Figure 100436DEST_PATH_IMAGE068
Figure 845538DEST_PATH_IMAGE069
Figure 984133DEST_PATH_IMAGE070
Figure 549107DEST_PATH_IMAGE071
其中，
Figure 995132DEST_PATH_IMAGE072
、
Figure 798003DEST_PATH_IMAGE073
、
Figure 405702DEST_PATH_IMAGE074
和
Figure 457971DEST_PATH_IMAGE075
分别表示四个控制输入量的最大值。
步骤2中多旋翼无人机的障碍物约束如下：
Figure 143905DEST_PATH_IMAGE076
其中，
Figure 863599DEST_PATH_IMAGE077
表示障碍物中心， a表示障碍物边缘沿x轴方向距离障碍物中心的距离， b表示障碍物边缘沿 y轴方向距离障碍物中心的距离， z表示障碍物边缘沿z轴方向距离障碍物中心的距离，
Figure 642200DEST_PATH_IMAGE078
表示多旋翼无人机距离障碍物的安全阈值；
优化指标定义如下：
Figure 916186DEST_PATH_IMAGE080
步骤3中所述多旋翼无人机动力学模型与障碍物约束线性化方法如下：
步骤3-1，定义新变量如下：
Figure 641697DEST_PATH_IMAGE081
Figure 215898DEST_PATH_IMAGE082
；
步骤3-2，将动力学方程写成线性化形式如下：
Figure 165399DEST_PATH_IMAGE083
其中， X*为任意参考轨迹， X表示状态变量，表达式为：
Figure 690796DEST_PATH_IMAGE084
U表示控制输入，表达式为：
Figure 954418DEST_PATH_IMAGE085
B的表达式如下：
Figure 648705DEST_PATH_IMAGE086
Figure 972370DEST_PATH_IMAGE087
表达式如下：
Figure 220949DEST_PATH_IMAGE088
Figure 350579DEST_PATH_IMAGE089
为
Figure 132327DEST_PATH_IMAGE087
的雅克比矩阵；
步骤3-3，将障碍物约束写成线性化形式如下：
Figure 626894DEST_PATH_IMAGE090
其中，
Figure 628348DEST_PATH_IMAGE091
表达式如下：
Figure 764931DEST_PATH_IMAGE092
步骤4中所述多旋翼无人机动力学方程及约束条件离散化方法如下：
步骤4-1，将时间区间
Figure 371493DEST_PATH_IMAGE093
分成N等份，时间步长为h，其表达式如下：
Figure 332234DEST_PATH_IMAGE094
；
步骤4-2，根据显式四阶龙格库塔公式，对动力学方程进行离散化如下：
Figure 758667DEST_PATH_IMAGE095
其中，
Figure 495679DEST_PATH_IMAGE096
和
Figure 691168DEST_PATH_IMAGE097
分别表示状态变量在第
Figure 324274DEST_PATH_IMAGE098
和第
Figure 300321DEST_PATH_IMAGE099
节点取值，
Figure 35100DEST_PATH_IMAGE100
、
Figure 881833DEST_PATH_IMAGE101
、
Figure 217000DEST_PATH_IMAGE102
和
Figure 149184DEST_PATH_IMAGE103
表达式如下
Figure 165681DEST_PATH_IMAGE104
Figure 132500DEST_PATH_IMAGE105
Figure 871524DEST_PATH_IMAGE106
Figure 494266DEST_PATH_IMAGE107
其中，
Figure 111192DEST_PATH_IMAGE108
和
Figure 932518DEST_PATH_IMAGE109
分别表示控制输入在第
Figure 547170DEST_PATH_IMAGE098
和第
Figure 453946DEST_PATH_IMAGE099
节点取值；
步骤4-3，终端状态约束可写成如下形式：
Figure 373098DEST_PATH_IMAGE110
；
步骤4-4，过程约束可写成如下形式：
Figure 48930DEST_PATH_IMAGE111
其中，
Figure 834484DEST_PATH_IMAGE112
；
步骤4-5，控制输入约束写成如下形式：
Figure 494135DEST_PATH_IMAGE113
Figure 390547DEST_PATH_IMAGE114
Figure 186464DEST_PATH_IMAGE115
其中，
Figure 438192DEST_PATH_IMAGE112
；
步骤4-6，障碍物约束写为：
Figure 522823DEST_PATH_IMAGE116
Figure 19663DEST_PATH_IMAGE117
其中，
Figure 935667DEST_PATH_IMAGE112
；
步骤4-7，优化指标写成如下形式：
Figure 63022DEST_PATH_IMAGE118
；
步骤4-8，经过线性化和离散化处理后，所述面向城市空间的多旋翼无人机路径规划问题归纳为如下形式：
Figure 166108DEST_PATH_IMAGE119
Figure 965174DEST_PATH_IMAGE120
其中，
Figure 735684DEST_PATH_IMAGE112
。
对步骤4中所归纳的多旋翼无人机路径规划问题进行求解，过程如下：
步骤5-1，已知多旋翼无人机初始状态情况下，令
Figure 33941DEST_PATH_IMAGE121
，
Figure 889902DEST_PATH_IMAGE122
，
Figure 728545DEST_PATH_IMAGE123
，
Figure 556824DEST_PATH_IMAGE124
，通过动力学递推得到一条初始参考轨迹X*；
步骤5-2，将所述初始参考轨迹X*带入步骤4中的多旋翼无人机路径规划问题，采用内点法进行求解，得到一条新的轨迹，并将该轨迹作为下一次计算的参考轨迹X*；
步骤5-3，当得到的轨迹收敛以后，则得到最优解，即面向城市空间的多旋翼无人机能量消耗最优轨迹，完成面向城市空间的多旋翼无人机路径规划。
有益效果：
本发明给出了一种面向城市空间的多旋翼无人机路径规划方法，通过对原始轨迹问题进行线性化、离散化处理，降低了问题求解的复杂度，提升了算法的计算效率，有效提升了多旋翼无人机对城市环境的适应能力。
附图说明
下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。
图1是本发明方法的流程图。
图2是多旋翼无人机燃耗最优轨迹示意图。
图3是多旋翼无人机燃耗最优轨迹x轴位移曲线示意图。
图4是多旋翼无人机燃耗最优轨迹y轴位移曲线示意图。
具体实施方式
为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。
实例1，如图1所示：
步骤一，确定多旋翼无人机动力学模型
Figure 321255DEST_PATH_IMAGE125
其中， x、 y、z分别表示多旋翼无人机的三轴位置坐标，
Figure 398933DEST_PATH_IMAGE126
、
Figure 41266DEST_PATH_IMAGE127
、
Figure 520789DEST_PATH_IMAGE128
分别表示多旋翼无人机沿三轴的速度分量，
Figure 160849DEST_PATH_IMAGE129
、
Figure 725823DEST_PATH_IMAGE130
、
Figure 171848DEST_PATH_IMAGE131
分别表示三轴位置坐标关于时间的导数，
Figure 207674DEST_PATH_IMAGE132
、
Figure 815373DEST_PATH_IMAGE133
、
Figure 867643DEST_PATH_IMAGE134
分别表示三轴位置坐标关于时间的二阶导数。
Figure 117359DEST_PATH_IMAGE135
、
Figure 305895DEST_PATH_IMAGE136
、
Figure 84495DEST_PATH_IMAGE137
分别表示多旋翼无人机的俯仰角、滚转角、偏航角，
Figure 325858DEST_PATH_IMAGE138
、
Figure 848106DEST_PATH_IMAGE139
、
Figure 422307DEST_PATH_IMAGE140
分别表示多旋翼无人机的俯仰角速度、滚转角速度、偏航角速度，
Figure 575071DEST_PATH_IMAGE141
、
Figure 601933DEST_PATH_IMAGE142
、
Figure 662292DEST_PATH_IMAGE143
表示姿态角关于时间的一阶导数，
Figure 91000DEST_PATH_IMAGE144
、
Figure 975517DEST_PATH_IMAGE145
、
Figure 427358DEST_PATH_IMAGE146
表示姿态角关于时间的二阶导数。
Figure 291409DEST_PATH_IMAGE147
、
Figure 574623DEST_PATH_IMAGE148
、
Figure 131506DEST_PATH_IMAGE149
表示多旋翼无人机对应三个轴的转动惯量。m表示多旋翼无人机的质量，
Figure 867381DEST_PATH_IMAGE150
表示升力作用点，即多旋翼无人机螺旋桨中心到质心的距离。
Figure 236920DEST_PATH_IMAGE151
、
Figure 374640DEST_PATH_IMAGE152
、
Figure 836846DEST_PATH_IMAGE153
、
Figure 325596DEST_PATH_IMAGE154
分别表示多旋翼无人机的四个控制输入量， g表示重力加速度。
步骤二，确定轨迹优化约束条件及优化指标
初始状态约束定义如下：
Figure 531449DEST_PATH_IMAGE028
其中，
Figure 726938DEST_PATH_IMAGE155
、
Figure 625624DEST_PATH_IMAGE156
、
Figure 100205DEST_PATH_IMAGE157
分别表示多旋翼无人机在初始时刻的三轴位置坐标，
Figure 844171DEST_PATH_IMAGE158
、
Figure 956483DEST_PATH_IMAGE159
、
Figure 963753DEST_PATH_IMAGE160
分别表示多旋翼无人机在初始时刻的沿x轴、y轴、z轴的速度分量，
Figure 161516DEST_PATH_IMAGE161
、
Figure 974752DEST_PATH_IMAGE162
、
Figure 941571DEST_PATH_IMAGE163
分别表示多旋翼无人机在初始时刻的姿态角，
Figure 883857DEST_PATH_IMAGE164
、
Figure 303337DEST_PATH_IMAGE165
、
Figure 920263DEST_PATH_IMAGE166
分别表示多旋翼无人机在初始时刻姿态角速度。
终端状态约束定义如下：
Figure 741588DEST_PATH_IMAGE041
其中，
Figure 356240DEST_PATH_IMAGE167
表示飞行时间，
Figure 263017DEST_PATH_IMAGE168
、
Figure 651010DEST_PATH_IMAGE169
、
Figure 858001DEST_PATH_IMAGE170
分别表示多旋翼无人机在
Figure 643554DEST_PATH_IMAGE167
时刻的三轴位置坐标，
Figure 37626DEST_PATH_IMAGE171
、
Figure 730776DEST_PATH_IMAGE172
、
Figure 526694DEST_PATH_IMAGE173
分别表示多旋翼无人机在
Figure 279886DEST_PATH_IMAGE167
时刻的沿x轴、y轴、z轴的速度分量，
Figure 863052DEST_PATH_IMAGE174
、
Figure 359892DEST_PATH_IMAGE175
、
Figure 275896DEST_PATH_IMAGE176
分别表示多旋翼无人机在
Figure 934410DEST_PATH_IMAGE167
时刻的姿态角，
Figure 303074DEST_PATH_IMAGE177
、
Figure 541289DEST_PATH_IMAGE178
、
Figure 75913DEST_PATH_IMAGE179
分别表示多旋翼无人机在
Figure 905329DEST_PATH_IMAGE167
时刻姿态角速度。
过程约束定义如下：
Figure 761289DEST_PATH_IMAGE180
其中，
Figure 803195DEST_PATH_IMAGE181
、
Figure 162632DEST_PATH_IMAGE182
、
Figure 694107DEST_PATH_IMAGE183
、
Figure 771785DEST_PATH_IMAGE184
、
Figure 912654DEST_PATH_IMAGE185
、
Figure 595439DEST_PATH_IMAGE186
分别表示多旋翼无人机飞行过程中各状态变量的最小值，
Figure 32237DEST_PATH_IMAGE187
、
Figure 862789DEST_PATH_IMAGE188
、
Figure 43235DEST_PATH_IMAGE189
、
Figure 580527DEST_PATH_IMAGE190
、
Figure 710198DEST_PATH_IMAGE191
、
Figure 28047DEST_PATH_IMAGE192
分别表示多旋翼无人机飞行过程中各状态变量的最大值。
控制输入约束定义如下：
Figure 746604DEST_PATH_IMAGE193
Figure 200720DEST_PATH_IMAGE194
Figure 182582DEST_PATH_IMAGE195
Figure 722148DEST_PATH_IMAGE196
其中，
Figure 509975DEST_PATH_IMAGE197
、
Figure 317132DEST_PATH_IMAGE198
、
Figure 266633DEST_PATH_IMAGE199
、
Figure 231178DEST_PATH_IMAGE200
分别表示四个控制输入量的最大值。
障碍物约束如下：
Figure 822697DEST_PATH_IMAGE201
其中，
Figure 985825DEST_PATH_IMAGE202
表示障碍物中心， a表示障碍物边缘沿x轴方向距离障碍物中心的距离， b表示障碍物边缘沿y轴方向距离障碍物中心的距离， z表示障碍物边缘沿z轴方向距离障碍物中心的距离。
Figure 106228DEST_PATH_IMAGE203
表示多旋翼无人机距离障碍物的安全阈值。
优化指标定义如下：
Figure 620385DEST_PATH_IMAGE079
步骤三、动力学模型与障碍物约束线性化
定义新变量如下：
Figure 186234DEST_PATH_IMAGE204
Figure 203868DEST_PATH_IMAGE205
将动力学方程写成线性化形式如下：
Figure 760752DEST_PATH_IMAGE206
其中， X*为任意参考轨迹。X表示状态变量，表达式为：
Figure 762206DEST_PATH_IMAGE207
U表示控制输入，表达式为：
Figure 429947DEST_PATH_IMAGE208
B的表达式如下：
Figure 505351DEST_PATH_IMAGE209
Figure 731670DEST_PATH_IMAGE210
表达式如下：
Figure 954841DEST_PATH_IMAGE211
Figure 160695DEST_PATH_IMAGE212
为
Figure 418501DEST_PATH_IMAGE210
的雅克比矩阵。
将障碍物约束写成线性化形式如下：
Figure 317187DEST_PATH_IMAGE213
其中，
Figure 965337DEST_PATH_IMAGE214
表达式如下：
Figure 974881DEST_PATH_IMAGE215
步骤四、动力学方程及约束条件离散化
将时间区间
Figure 851308DEST_PATH_IMAGE216
分成 N等份，时间步长为h，其表达式如下：
Figure 655316DEST_PATH_IMAGE217
根据显式四阶龙格库塔公式，对动力学方程进行离散化如下：
Figure 56341DEST_PATH_IMAGE218
其中，
Figure 603997DEST_PATH_IMAGE219
、
Figure 570816DEST_PATH_IMAGE220
分别表示状态变量在第
Figure 811305DEST_PATH_IMAGE221
和第
Figure 496364DEST_PATH_IMAGE222
节点取值，
Figure 549509DEST_PATH_IMAGE223
、
Figure 370834DEST_PATH_IMAGE224
、
Figure 47803DEST_PATH_IMAGE225
、
Figure 954579DEST_PATH_IMAGE226
表达式如下
Figure 47300DEST_PATH_IMAGE227
Figure 254291DEST_PATH_IMAGE228
Figure 69538DEST_PATH_IMAGE229
Figure 463610DEST_PATH_IMAGE230
其中，
Figure 360022DEST_PATH_IMAGE231
、
Figure 155939DEST_PATH_IMAGE232
分别表示控制输入在第
Figure 174711DEST_PATH_IMAGE221
和第
Figure 56079DEST_PATH_IMAGE222
节点取值。
终端状态约束可写成如下形式：
Figure 989138DEST_PATH_IMAGE110
过程约束可写成如下形式：
Figure 639562DEST_PATH_IMAGE233
其中，
Figure 563656DEST_PATH_IMAGE234
。
控制输入约束可写成如下形式：
Figure 197899DEST_PATH_IMAGE235
Figure 232852DEST_PATH_IMAGE236
Figure 737782DEST_PATH_IMAGE237
其中，
Figure 832777DEST_PATH_IMAGE234
。
障碍物约束可以写为：
Figure 390535DEST_PATH_IMAGE116
Figure 963599DEST_PATH_IMAGE117
其中，
Figure 854195DEST_PATH_IMAGE234
。
优化指标可写成如下形式：
Figure 323353DEST_PATH_IMAGE118
经过线性化和离散化处理后，一种面向城市空间的多旋翼无人机路径规划问题可以归纳成如下形式：
Figure 401031DEST_PATH_IMAGE119
Figure 308944DEST_PATH_IMAGE120
其中，
Figure 490264DEST_PATH_IMAGE234
。
步骤五，问题求解。
对步骤4中的面向城市空间的多旋翼无人机路径规划问题进行求解，具体求解过程如下：
1）已知多旋翼无人机初始状态情况下，令
Figure 927062DEST_PATH_IMAGE238
，
Figure 492035DEST_PATH_IMAGE239
，
Figure 672481DEST_PATH_IMAGE240
，
Figure 272089DEST_PATH_IMAGE241
，可以通过动力学递推得到一条初始参考轨迹X*。
2）将X*带入步骤四中的多旋翼无人机路径规划问题，采用内点法进行求解，得到一条新的轨迹，并将该轨迹作为下一次计算的参考轨迹X*。
3）当得到的轨迹收敛以后，则得到最优解，参见图2，即面向城市空间的多旋翼无人机能量消耗最优轨迹，其中，X轴位移曲线参见图3，Y轴位移曲线参见图4。
本发明提供了一种面向城市空间的多旋翼无人机路径规划方法的思路及方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (1)

1.一种面向城市空间的多旋翼无人机路径规划方法，其特征在于，包括以下步骤：
步骤1，确定多旋翼无人机动力学模型；
步骤2，确定多旋翼无人机轨迹优化约束条件及优化指标；
步骤3，建立多旋翼无人机动力学模型与障碍物约束线性化；
步骤4，建立多旋翼无人机动力学方程及约束条件离散化；
步骤5，求解完成面向城市空间的多旋翼无人机路径规划；
步骤1中所述多旋翼无人机动力学模型如下：
Figure 493741DEST_PATH_IMAGE001
其中， x、 y和 z分别表示多旋翼无人机的三轴位置坐标，
Figure 44546DEST_PATH_IMAGE002
、
Figure 763103DEST_PATH_IMAGE003
和
Figure 420480DEST_PATH_IMAGE004
分别表示多旋翼无人机沿三轴的速度分量，
Figure 199081DEST_PATH_IMAGE005
、
Figure 4226DEST_PATH_IMAGE006
和
Figure 729736DEST_PATH_IMAGE007
分别表示三轴位置坐标关于时间的导数，
Figure 536893DEST_PATH_IMAGE008
、
Figure 751974DEST_PATH_IMAGE009
和
Figure 716518DEST_PATH_IMAGE010
分别表示三轴位置坐标关于时间的二阶导数；
Figure 42458DEST_PATH_IMAGE011
、
Figure 205586DEST_PATH_IMAGE012
和
Figure 591568DEST_PATH_IMAGE013
分别表示多旋翼无人机的俯仰角、滚转角和偏航角， p、q和 r分别表示多旋翼无人机的俯仰角速度、滚转角速度和偏航角速度，
Figure 807523DEST_PATH_IMAGE014
、
Figure 671574DEST_PATH_IMAGE015
和
Figure 954788DEST_PATH_IMAGE016
表示姿态角关于时间的一阶导数，
Figure 511671DEST_PATH_IMAGE017
、
Figure 450808DEST_PATH_IMAGE018
和
Figure 852971DEST_PATH_IMAGE019
表示姿态角关于时间的二阶导数；
Figure 256270DEST_PATH_IMAGE020
、
Figure 685852DEST_PATH_IMAGE021
和
Figure 643444DEST_PATH_IMAGE022
表示多旋翼无人机对应三个轴的转动惯量； m表示多旋翼无人机的质量，
Figure 380456DEST_PATH_IMAGE023
表示升力作用点，即多旋翼无人机螺旋桨中心到质心的距离；
Figure 372683DEST_PATH_IMAGE024
、
Figure 209051DEST_PATH_IMAGE025
、
Figure 919519DEST_PATH_IMAGE026
和
Figure 693177DEST_PATH_IMAGE027
分别表示多旋翼无人机的四个控制输入量， g表示重力加速度；
步骤2中多旋翼无人机的初始状态约束定义如下：
Figure 539910DEST_PATH_IMAGE028
其中，
Figure 812760DEST_PATH_IMAGE029
、
Figure 744944DEST_PATH_IMAGE030
和
Figure 823758DEST_PATH_IMAGE031
分别表示多旋翼无人机在初始时刻的三轴位置坐标，
Figure 790577DEST_PATH_IMAGE032
、
Figure 467284DEST_PATH_IMAGE033
和
Figure 152343DEST_PATH_IMAGE034
分别表示多旋翼无人机在初始时刻的沿x轴、y轴和z轴的速度分量，
Figure 769269DEST_PATH_IMAGE035
、
Figure 590595DEST_PATH_IMAGE036
和
Figure 939668DEST_PATH_IMAGE037
分别表示多旋翼无人机在初始时刻的姿态角，
Figure 846444DEST_PATH_IMAGE038
、
Figure 267061DEST_PATH_IMAGE039
和
Figure 441428DEST_PATH_IMAGE040
分别表示多旋翼无人机在初始时刻姿态角速度；
步骤2中多旋翼无人机的终端状态约束定义如下：
Figure 23719DEST_PATH_IMAGE041
其中，
Figure 621054DEST_PATH_IMAGE042
表示飞行时间，
Figure 314203DEST_PATH_IMAGE043
、
Figure 110121DEST_PATH_IMAGE044
和
Figure 128893DEST_PATH_IMAGE045
分别表示多旋翼无人机在
Figure 446479DEST_PATH_IMAGE042
时刻的三轴位置坐标，
Figure 943320DEST_PATH_IMAGE046
、
Figure 859323DEST_PATH_IMAGE047
和
Figure 783417DEST_PATH_IMAGE048
分别表示多旋翼无人机在
Figure 355343DEST_PATH_IMAGE042
时刻的沿 x轴、 y轴和z轴的速度分量，
Figure 390296DEST_PATH_IMAGE049
、
Figure 160805DEST_PATH_IMAGE050
和
Figure 754336DEST_PATH_IMAGE051
分别表示多旋翼无人机在
Figure 344717DEST_PATH_IMAGE042
时刻的姿态角，
Figure 386622DEST_PATH_IMAGE052
、
Figure 11639DEST_PATH_IMAGE053
和
Figure 277535DEST_PATH_IMAGE054
分别表示多旋翼无人机在
Figure 620791DEST_PATH_IMAGE042
时刻姿态角速度；
步骤2中多旋翼无人机的过程约束定义如下：
Figure 997546DEST_PATH_IMAGE055
其中，
Figure 178867DEST_PATH_IMAGE056
、
Figure 615664DEST_PATH_IMAGE057
、
Figure 180638DEST_PATH_IMAGE058
、
Figure 626662DEST_PATH_IMAGE059
、
Figure 226271DEST_PATH_IMAGE060
和
Figure 833970DEST_PATH_IMAGE061
分别表示多旋翼无人机飞行过程中各状态变量的最小值，
Figure 620660DEST_PATH_IMAGE062
、
Figure 306594DEST_PATH_IMAGE063
、
Figure 760709DEST_PATH_IMAGE064
、
Figure 804889DEST_PATH_IMAGE065
、
Figure 78875DEST_PATH_IMAGE066
和
Figure 804386DEST_PATH_IMAGE067
分别表示多旋翼无人机飞行过程中各状态变量的最大值；
步骤2中多旋翼无人机的控制输入约束定义如下：
Figure 113007DEST_PATH_IMAGE068
Figure 826623DEST_PATH_IMAGE069
Figure 853485DEST_PATH_IMAGE070
Figure 913845DEST_PATH_IMAGE071
其中，
Figure 545815DEST_PATH_IMAGE072
、
Figure 666217DEST_PATH_IMAGE073
、
Figure 914796DEST_PATH_IMAGE074
和
Figure 44426DEST_PATH_IMAGE075
分别表示四个控制输入量的最大值；
步骤2中多旋翼无人机的障碍物约束
Figure 662113DEST_PATH_IMAGE076
如下：
Figure 156679DEST_PATH_IMAGE077
其中，
Figure 892554DEST_PATH_IMAGE078
表示障碍物中心， a表示障碍物边缘沿x轴方向距离障碍物中心的距离， b表示障碍物边缘沿 y轴方向距离障碍物中心的距离， z表示障碍物边缘沿z轴方向距离障碍物中心的距离，
Figure 825875DEST_PATH_IMAGE079
表示多旋翼无人机距离障碍物的安全阈值；
优化指标定义如下：
Figure 963595DEST_PATH_IMAGE080
；
步骤3中所述多旋翼无人机动力学模型中，动力学方程线性化表达式如下：
Figure 425800DEST_PATH_IMAGE081
其中， X*为任意参考轨迹， X表示状态变量， U表示控制输入，B为系数矩阵，
障碍物约束线性化形式如下：
Figure 350769DEST_PATH_IMAGE082
其中，
Figure 822201DEST_PATH_IMAGE083
为
Figure 814428DEST_PATH_IMAGE076
的雅可比矩阵；
步骤4中所述多旋翼无人机动力学方程及约束条件离散化方法如下：
步骤4-1，将时间区间
Figure 713114DEST_PATH_IMAGE084
分成N等份，时间步长为h，其表达式如下：
Figure 423581DEST_PATH_IMAGE085
；
步骤4-2，根据显式四阶龙格库塔公式，对动力学方程进行离散化如下：
Figure 636388DEST_PATH_IMAGE086
其中，
Figure 483121DEST_PATH_IMAGE087
和
Figure 316823DEST_PATH_IMAGE088
分别表示状态变量在第
Figure 249007DEST_PATH_IMAGE089
和第
Figure 62242DEST_PATH_IMAGE090
节点取值，
Figure 966744DEST_PATH_IMAGE091
、
Figure 207232DEST_PATH_IMAGE092
、
Figure 892292DEST_PATH_IMAGE093
和
Figure 243639DEST_PATH_IMAGE094
表达式如下
Figure 563499DEST_PATH_IMAGE095
Figure 178151DEST_PATH_IMAGE096
Figure 84927DEST_PATH_IMAGE097
Figure 505544DEST_PATH_IMAGE098
其中，
Figure 181376DEST_PATH_IMAGE099
和
Figure 763667DEST_PATH_IMAGE100
分别表示控制输入在第
Figure 859537DEST_PATH_IMAGE089
和第
Figure 552687DEST_PATH_IMAGE090
节点取值；
步骤4-3，终端状态约束可写成如下形式：
Figure 348604DEST_PATH_IMAGE101
；
步骤4-4，过程约束可写成如下形式：
Figure 836218DEST_PATH_IMAGE102
其中，
Figure 717586DEST_PATH_IMAGE103
；
步骤4-5，控制输入约束写成如下形式：
Figure 214426DEST_PATH_IMAGE104
Figure 130430DEST_PATH_IMAGE105
Figure 54523DEST_PATH_IMAGE106
其中，
Figure 656144DEST_PATH_IMAGE103
；
步骤4-6，障碍物约束写为：
Figure 691096DEST_PATH_IMAGE107
Figure 664868DEST_PATH_IMAGE108
其中，
Figure 759863DEST_PATH_IMAGE103
；
步骤4-7，优化指标写成如下形式：
Figure 350244DEST_PATH_IMAGE109
；
步骤4-8，经过线性化和离散化处理后，所述面向城市空间的多旋翼无人机路径规划问题归纳为如下形式：
Figure 454467DEST_PATH_IMAGE110
Figure 79483DEST_PATH_IMAGE111
其中，
Figure 47177DEST_PATH_IMAGE103
；
对步骤4中所归纳的多旋翼无人机路径规划问题进行求解，过程如下：
步骤5-1，已知多旋翼无人机初始状态情况下，令
Figure 124854DEST_PATH_IMAGE112
，
Figure 501609DEST_PATH_IMAGE113
，
Figure 246711DEST_PATH_IMAGE114
，
Figure 683509DEST_PATH_IMAGE115
，通过动力学递推得到一条初始参考轨迹X*；
步骤5-2，将所述初始参考轨迹X*带入步骤4中的多旋翼无人机路径规划问题，采用内点法进行求解，得到一条新的轨迹，并将该轨迹作为下一次计算的参考轨迹X*；
步骤5-3，当得到的轨迹收敛以后，则得到最优解，即面向城市空间的多旋翼无人机能量消耗最优轨迹，完成面向城市空间的多旋翼无人机路径规划。
CN202210073036.7A 2022-01-21 2022-01-21 一种面向城市空间的多旋翼无人机路径规划方法 Active CN114089780B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210073036.7A CN114089780B (zh) 2022-01-21 2022-01-21 一种面向城市空间的多旋翼无人机路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210073036.7A CN114089780B (zh) 2022-01-21 2022-01-21 一种面向城市空间的多旋翼无人机路径规划方法

Publications (2)

Publication Number Publication Date
CN114089780A CN114089780A (zh) 2022-02-25
CN114089780B true CN114089780B (zh) 2022-04-12

Family

ID=80309039

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210073036.7A Active CN114089780B (zh) 2022-01-21 2022-01-21 一种面向城市空间的多旋翼无人机路径规划方法

Country Status (1)

Country Link
CN (1) CN114089780B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116466746B (zh) * 2023-04-24 2023-09-29 浙江大学 四旋翼集群高速通过动态航路点的规划控制方法及装置
CN117631686B (zh) * 2023-12-07 2024-06-07 浙江大学 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104165627A (zh) * 2014-08-27 2014-11-26 电子科技大学 一种基于线性规划的实时动态航迹规划方法
CN106200662A (zh) * 2016-08-11 2016-12-07 国网四川省电力公司检修公司 一种无人机飞行控制***
CN108445898A (zh) * 2018-05-14 2018-08-24 南开大学 基于微分平坦特性的四旋翼无人飞行器***运动规划方法
CN112034887A (zh) * 2020-09-10 2020-12-04 南京大学 无人机躲避柱状障碍物到达目标点的最优路径训练方法
CN112148024A (zh) * 2020-08-20 2020-12-29 中国人民解放军海军航空大学 基于自适应伪谱法的无人机实时在线航迹规划方法
CN113759977A (zh) * 2021-09-18 2021-12-07 西北工业大学 基于优化的绳系多无人机协同搬运的避障轨迹规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104165627A (zh) * 2014-08-27 2014-11-26 电子科技大学 一种基于线性规划的实时动态航迹规划方法
CN106200662A (zh) * 2016-08-11 2016-12-07 国网四川省电力公司检修公司 一种无人机飞行控制***
CN108445898A (zh) * 2018-05-14 2018-08-24 南开大学 基于微分平坦特性的四旋翼无人飞行器***运动规划方法
CN112148024A (zh) * 2020-08-20 2020-12-29 中国人民解放军海军航空大学 基于自适应伪谱法的无人机实时在线航迹规划方法
CN112034887A (zh) * 2020-09-10 2020-12-04 南京大学 无人机躲避柱状障碍物到达目标点的最优路径训练方法
CN113759977A (zh) * 2021-09-18 2021-12-07 西北工业大学 基于优化的绳系多无人机协同搬运的避障轨迹规划方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
RRT* algorithm under holonomic conditions for UAVs;Minyang Kang 等;《2019 IEEE Intl Conf on Dependable, Autonomic and Secure Computing》;20191231;全文 *
城市低空环境中多旋翼无人机在线航线规划方法;吴宇 等;《控制与决策》;20211231;第36卷(第12期);全文 *
面向城市环境的四旋翼无人机在线避障航迹规划方法;成浩浩 等;《计算机科学》;20190430;第46卷(第4期);全文 *
面向城市飞行安全的无人机离散型多路径规划方法;胡莘婷 等;《航空学报》;20210625;第42卷(第6期);全文 *

Also Published As

Publication number Publication date
CN114089780A (zh) 2022-02-25

Similar Documents

Publication Publication Date Title
Das et al. Dynamic inversion with zero-dynamics stabilisation for quadrotor control
CN109062042B (zh) 一种旋翼飞行器的有限时间航迹跟踪控制方法
CN114089780B (zh) 一种面向城市空间的多旋翼无人机路径规划方法
CN108227491B (zh) 一种基于滑模神经网络的智能车轨迹跟踪控制方法
CN110320925B (zh) 基于高阶干扰观测器的四旋翼飞行器安全控制方法
CN108873929B (zh) 一种固定翼飞机自主着舰方法及***
CN111538255B (zh) 一种反蜂群无人机的飞行器控制方法及***
CN109597303A (zh) 一种复合式旋翼飞行器全模式飞行控制方法
CN111273688B (zh) 基于事件触发的四旋翼无人机一致性编队控制方法
CN110673623B (zh) 一种基于双环pd控制算法控制的四旋翼无人机着陆方法
CN104536448B (zh) 一种基于Backstepping法的无人机姿态***控制方法
CN112578805A (zh) 一种旋翼飞行器的姿态控制方法
CN115657730B (zh) 一种大规模多旋翼无人机鲁棒分簇编队控制方法
Das et al. Sliding mode approach to control quadrotor using dynamic inversion
CN112947572A (zh) 一种基于地形跟随的四旋翼飞行器自适应运动规划方法
CN106647264A (zh) 一种基于控制约束的扩展鲁棒h∞的无人机控制方法
CN115556111A (zh) 基于变惯性参数建模的飞行机械臂耦合扰动控制方法
CN113156985A (zh) 基于预设性能的固定翼无人机避障鲁棒抗扰飞行控制方法
CN107247464A (zh) 一种四旋翼无人飞行器的状态受限控制方法和***
Min et al. Formation tracking control of multiple quadrotors based on backstepping
CN116382332A (zh) 一种基于ude的战斗机大机动鲁棒飞行控制方法
CN113885549B (zh) 基于维度裁剪的ppo算法的四旋翼姿态轨迹控制方法
CN116203840A (zh) 可重复使用运载器自适应增益调度控制方法
CN115344056A (zh) 一种复杂操纵面飞机的智能飞行控制方法及应用
CN112161626B (zh) 一种基于航路跟踪映射网络的高可飞性航路规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant