面向复杂未知环境的无人机吊运系统运动规划方法及系统

本发明涉及机器人运动规划，特别涉及一种面向复杂未知环境的无人机吊运系统运动规划方法及系统。

背景技术：

1、对于未知环境下的自主运输任务，无人机吊运系统与环境构成强耦合系统，难以实现安全自主飞行作业。现有大多研究均考虑已知静态环境下的运动规划方法，用于实现简单环境下的自主运输。然而，在许多实际运用中，将已知静态环境运用于无人机吊运系统是保守的，并且还需要指定特定路点。与传统路径规划方法相比，自适应启发式机器人动力学约束下的路径规划方法是一种有效的方法，能在未知环境下通过考虑整个系统的安全性提高轨迹优化的效率和系统安全性。尽管已经有一些无人机吊运系统自主运动规划的研究结果，但其运动规划效果通常忽略环境的相关影响，难以适应现实中未知复杂的环境。为了促进国家重大工程基础设施空中交互检测运维作业，亟需发展无人机吊运系统相关的运动规划、自主运输能力。

技术实现思路

1、基于上述问题，本发明技术方案提出一种面向复杂未知环境的无人机吊运系统运动规划方法及系统，其目的是为了解决背景技术中无人机吊运系统的运动规划的技术问题。

2、本发明提供的技术方案如下：

3、一方面，一种面向复杂未知环境的无人机吊运系统运动规划方法，包括如下步骤：

4、步骤s1：构建无人机吊运系统动力学模型；

5、步骤s2：根据动力学模型，推导无人机吊运系统微分平坦输出空间；

6、步骤s3：基于动力学模型以及无人机吊运系统微分平坦输出，采用混合a*算法，构造自适应启发搜索的机器人动力学约束下的路径规划算法；

7、步骤s4：利用无人机吊运系统微分平坦性质，规划无人机吊运系统微分平坦输出空间中的负载状态后，再根据无人机吊运系统的能量消耗和运行时间，构造用负载状态表示的负载轨迹的优化函数；

8、利用微分平坦性质中只需规划微分平坦输出空间中的变量即可实现全维度状态的规划，对于无人机吊运系统，其微分平坦输出空间由负载位置和无人机偏航角组成，那么只需要规划负载位置即可，由此优化函数可以描述成负载轨迹及其高阶导数的线性组合的形式；无人机偏航角的取值属于自行设定值；

9、优化函数是对负载轨迹进行优化，优化函数的构造是由系统的能量消耗以及消耗的时间构成，最终通过优化，可以获得满足约束的负载轨迹多项式；

10、步骤s5：获取多约束条件；

11、根据起点和期望的终点，设置起点与终点约束；基于无人机吊运系统动力学模型，依据负载和无人机耦合特性，设计动力学与绳长约束；利用步骤s3的路径规划算法得到的可行路径，提取无人机吊运系统所在环境中的安全空间，构建安全性约束；

12、步骤s6：在多约束下，对优化函数进行求解，获得安全可行的负载轨迹，提供给非线性负载轨迹跟踪控制器，实现无人机吊运系统在未知复杂环境下的自主运动规划。

13、全文中所提及的负载是指无人机吊运系统吊运的负载；

14、进一步地，所述无人机吊运系统动力学模型的具体构建过程如下：

15、步骤s101：利用牛顿-欧拉方法，构建无人机吊运系统动力学模型如下：

16、

17、

18、

19、

20、

21、

22、其中，公式(1)表示无人机吊运系统的负载的速度，公式(2)和(3)表示无人机吊运系统的负载摆动动力学，公式(4)表示无人机吊运系统的负载平移动力学，公式(5)和(6)表示无人机吊运系统旋转动力学；

23、mq表示无人机吊运系统质量，ml表示负载质量，l表示绳子长度，j表示常数惯性矩阵，g表示重力常数，均为无人机吊运系统的物理参数，其中表示实数集；vl表示惯性坐标系下无人机吊运系统的负载的线速度；表示负载速度；

24、表示惯性坐标系，ow表示惯性坐标系的原点，xw、yw、zw分别表示坐标系的x、y、z轴下负载的线速度；

25、q为由无人机吊运系统的负载坐标系原点ol指向无人机机体坐标系原点ob的单位向量，表示无人机吊运系统的负载的姿态；表示q相对时间的导数；是的转置；

26、表示无人机吊运系统吊运的负载坐标系，ol表示负载坐标系的原点，xl、yl、zl分别表示负载坐标系的x、y、z轴，表示无人机吊运系统中无人机机体坐标系，ob表示机体坐标系的原点，xb、yb、zb分别表示无人机机体坐标系的x、y、z轴；

27、表示惯性坐标系下无人机吊运系统的负载的线加速度，ω和分别表示无人机机体坐标系下的角速度和角加速度；ω表示负载的角速度，表示负载的角加速度，r表示无人机姿态的旋转矩阵，向量e3＝[0,0,1]t，和分别为广义控制力和控制力矩；定义操作符(·)^:表示对于任意a,满足(a)^b＝a×b，其中so(3)为欧氏群，操作符×表示叉乘运算；定义(·)^的逆为(·)∨；

28、步骤s102：以绳子拉力为中间变量，构建无人机吊运系统中无人机牛顿欧拉方程：

29、

30、其中，表示惯性坐标系下无人机吊运系统的线加速度，表示沿绳子上的拉力；

31、步骤s103：以绳子拉力为中间变量，构建如下无人机吊运系统的负载的牛顿欧拉方程：

32、

33、进一步地，所述无人机吊运系统微分平坦输出空间得推导过程为：

34、步骤s201：用无人机吊运系统的负载状态表示由无人机吊运系统的负载坐标系原点ol指向无人机机体坐标系原点ob的单位向量：

35、

36、其中，表示惯性坐标系下无人机吊运系统的负载的线加速度；||||为范数符号；

37、公式(9)表明q可以由负载状态进行表示；

38、由负载状态及其高阶导进行表示ω：

39、结合公式(3)和公式(9)，则ω可改写为：

40、

41、其中，表示q相对于时间的导数，ω表示负载摆动的角速度；

42、依据公式(9)和公式(10)，表明ω可以由负载状态及其高阶导进行表示；

43、步骤s202：引入如下约束条件：qtq＝1            (11)

44、依据公式(11)，t写成：t＝tqtq           (12)

45、结合公式(8)和公式(9)，t采用负载状态表示：

46、

47、其中，qt表示q的转置；

48、步骤s203：惯性坐标系下无人机吊运系统的位置xq用负载的位置xl及其高阶导的线性组合表示：

49、

50、步骤s204：根据公式(9)、(10)、(13)、(14)得出，无人机吊运系统的微分平坦输出空间为[xl,ψ]，其中ψ表示机体坐标系下的偏航角。

51、无人机吊运系统动力学中的所有状态可以由微分平坦输出空间中的状态的高阶导数的线性组合的形式描述。

52、进一步地，所述自适应启发搜索的机器人动力学约束下的路径规划算法的构造过程如下：

53、步骤s301：定义一个负载状态变量s＝[xl,vl]t，将负载的线加速度作为控制输入，构造动作集，构造形式如下：

54、

55、

56、其中，δt表示节点拓展时间间隔；动作集由设定的动力学约束[amin,amax]，amax、amin分别表示系统最大、最小加速度，均匀离散为na个，每个动作记为ud，并且允许的时间间隔[0,δt]离散为nt个；

57、步骤s302：构造节点拓展代价函数：

58、f(sc)＝g(sc)+h(sc)     (17)

59、其中，f(sc)是节点的全部成本，g(sc)表示从开始节点s0到当前节点sc已经消耗的成本，h(sc)表示从当前节点sc到目标节点sg估计消耗的成本；基于路径规划消耗的能量和时间，代价函数采用以下公式表述：

60、

61、在每个扩展过程中，给定动作输入ud和扩展时间间隔δt，运动基元的成本表示为ei＝(||ud||2+ρ)δt，并且如果从开始节点s0到当前节点sc的最优路径由m个运动基元组成，ts表示从起点到当前节点消耗的时间；ρ表示权重系数；

62、则从开始节点s0到当前节点sc消耗的代价g(sc)计算公式如下：

63、

64、其中，udi表示每个运动基元的输入，δti表示运动基元间的时间间隔，ρ表示权重系数；m表示将连续的函数公式(18)离散为m份的公式(19)；

65、运动基元可以理解为符合约束的一小段路径，将从起点到终点的运动基元连接起来就是最终的路径；

66、从开始节点s0到当前节点sc预计花费的代价启发式函数hc表示为：

67、hc＝p(sc,sg)+kt(st-τ)+v(sc,sq)    (20)

68、其中，kt是权重，st是期望扩展时间之和；v(sc,sq)是确定负载和无人机吊运系统的无人机之间是否存在障碍物的函数，其中sq是由微分平坦推导出的无人机吊运系统的无人机位置；p(sc,sg)表示从当前节点到目标节点之间的估计代价；

69、步骤s303：构造节点拓展方程，构造形式如下：

70、依据无人机吊运系统的负载的牛顿欧拉方程，节点拓展方程改写为：

71、

72、其中，xk是负载下一个扩展运动状态，xk-1是负载当前状态，ud是离散控制输入，δt是持续时间；给定ud和δt，在xk-1的基础上扩展下一个节点；

73、步骤s304：基于公式(21)进行节点扩展，基于公式(18)、(19)、(20)进行代价计算，找到代价最低的节点，并判断对应的无人机位置；同时逐步查询地图，检测负载与无人机之间是否存在障碍物；如果安全，则将所找到的节点添加到可行路径的尾部，并以可行路径尾部节点作为父节点，重复步骤s304，直到搜索到目标。

74、基于公式(18)、(19)、(20)、(21)，可以实现在较短的时间内搜索出一条负载动力学可行的并且可以保证整个系统安全性的路径。

75、a1初始化一个队列用于存储路径上的节点，起始队列为空，将起点添加到队列，以队列尾最后一个节点为父节点；

76、a2根据公式18192021，找到父节点周围代价最低的节点，计算对应的无人机位置；

77、a3判断无人机与负载之间是否存在障碍物：

78、a4不存在障碍物，则将该节点存在队列末尾，以队列尾最后一个节点为父节点，重复s2-s3直到搜索到终点，如果存在障碍物，则舍弃当前节点重新搜索；

79、进一步地，用负载状态表示的负载轨迹的优化函数的构造过程如下：

80、步骤s401：利用基于多项式轨迹描述的方法，负载轨迹描述如下：

81、pl(t):＝[px(t),py(t),pz(t)],pl(t)＝cβ(t)  (22)

82、其中，pl(t)表示负载轨迹，px(t),py(t),pz(t)表示pl(t)在惯性坐标系各坐标轴的分量，c是多项式系数；β(t)是自然时间基；

83、步骤s402：根据公式(14)，负载的位置与无人机的加速度相耦合，负载轨迹的优化函数如下：

84、

85、其中，表示构建的负载轨迹的优化函数，表示无人机吊运系统的无人机能量消耗，κ表示时间对应的权重，tf表示为规划消耗的总时间。

86、负载轨迹的优化函数是为了减小无人机的能量消耗，以及时间消耗；

87、进一步地，所述多约束包括起点、终点约束、动力学约束、以及安全性约束绳长约束，具体构造过程如下：

88、步骤s501：为满足起点和终点为用户自定义位置，构造起点和终点约束，构造形式如下：

89、

90、其中，表示第0段负载轨迹0时刻的位置，表示第s-1段负载轨迹在tf时刻的位置，是用户自定的起点和终点，s表示分段多项式的段数；

91、步骤s502：为防止系统超出动力学的限制，构造动力学约束，构造形式如下：

92、

93、其中，vm,am,jm,sm是设定的最大速度、最大加速度、最大加加速度以及最大加加加速度；

94、加加速度是速度的二阶导数，加加加速度是速度的三阶导数；

95、步骤s503：为了保证无人吊运系统的安全性，将最终优化的轨迹约束在安全区域内保证系统的安全性，构造安全性约束，安全性约束构造形式如下：

96、

97、其中，是根据环境信息获取到的无障碍物的安全空间；

98、根据搜索到连接起点和终点的路径，查询地图信息可以获取到地图中的安全区域；

99、步骤s504：为保证绳索始终处于绷紧状态，构造绳长约束，构造形式如下：

100、

101、其中，dl,du是绳子长度的允许范围的上下限，pl(t)表示t时刻负载的位置，pq(t)表示t时刻无人机吊运系统的无人机的位置。

102、根据公式(23)，以公式(24)、公式(25)、公式(26)和公式(27)，可以优化生成一条动力学可行的、安全的、符合无人机吊运系统的运动轨迹，实现无人机吊运系统在未知复杂环境下的自主运动规划，提升自主运输能力。

103、第二方面，一种基于面向复杂未知环境的无人机吊运系统运动规划方法的系统，包括：

104、无人机吊运系统动力学模型：根据动力学理论，构建无人机吊运系统的动力学模型；

105、微分平坦输出空间推导模块：根据动力学模型，推导无人机吊运系统微分平坦输出空间；

106、构造路径规划算法模块：基于动力学模型以及无人机吊运系统微分平坦输出，采用混合a*算法，构造自适应启发搜索的机器人动力学约束下的路径规划算法；

107、负载轨迹函数优化模块：利用无人机吊运系统微分平坦性质，规划无人机吊运系统微分平坦输出空间中的负载状态后，再根据无人机吊运系统的能量消耗和运行时间，构造用负载状态表示的负载轨迹的优化函数；

108、多约束构造模块：基于无人机吊运系统动力学模型，依据负载和无人机耦合特性，设计动力学与绳长约束；基于构造路径规划算法模块构造的路径规划算法得到的可行路径，提取无人机吊运系统所在环境中的安全空间，构建安全性约束；

109、求解模块：在多约束下，对负载轨迹的优化函数进行求解，获得安全可行的负载轨迹，提供给非线性负载轨迹跟踪控制器，实现无人机吊运系统在未知复杂环境下的自主运动规划。

110、第三方面，一种电子终端，至少包括：

111、一个或多个处理器；

112、存储了一个或多个计算机程序的存储器；

113、其中，所述处理器调用所述计算机程序以执行：

114、上述一种面向复杂未知环境的无人机吊运系统运动规划方法的步骤。

115、第四方面，一种计算机可读存储介质，存储了计算机程序，所述计算机程序被处理器调用以执行：

116、所述一种面向复杂未知环境的无人机吊运系统运动规划方法的步骤。

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

118、1)与现有方案相比，提出一种自适应启发搜索的机器人动力学约束下的路径规划算法，确保在负载路径搜索的过程中，保证整个系统的安全性，从而提高优化的效率。

119、2)提出了一种安全轨迹优化算法，用于生成无人机吊运系统的时空最优轨迹。

120、3)将运动规划问题解耦为两层，一层为全局路径规划，获得一条安全可行的路径，另一层为轨迹生成与优化，根据前一层获得的可行路径，结合环境信息，求解多约束下的安全运动轨迹；使得所提出的算法整合在分层运动规划框架下，实现无人机吊运系统在无先验地图未知环境中自主运动规划。