CN113485418B - Flexible rope system constraint multi-robot track generation method - Google Patents

Flexible rope system constraint multi-robot track generation method Download PDF

Info

Publication number
CN113485418B
CN113485418B CN202110749405.5A CN202110749405A CN113485418B CN 113485418 B CN113485418 B CN 113485418B CN 202110749405 A CN202110749405 A CN 202110749405A CN 113485418 B CN113485418 B CN 113485418B
Authority
CN
China
Prior art keywords
aircraft
optimization
track
initial
trajectory
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
CN202110749405.5A
Other languages
Chinese (zh)
Other versions
CN113485418A (en
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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110749405.5A priority Critical patent/CN113485418B/en
Publication of CN113485418A publication Critical patent/CN113485418A/en
Application granted granted Critical
Publication of CN113485418B publication Critical patent/CN113485418B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10Simultaneous control of position or course in three dimensions
    • G05D1/101Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G05D1/104Simultaneous control of position or course in three dimensions specially adapted for aircraft involving a plurality of aircrafts, e.g. formation flying
    • YGENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02TCLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00Road transport of goods or passengers
    • Y02T10/10Internal combustion engine [ICE] based vehicles
    • Y02T10/40Engine management systems

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)
  • Traffic Control Systems (AREA)

Abstract

The invention relates to a track generation method for a flexible rope system constrained multi-robot, and belongs to the field of robot track planning research. The whole aircraft cooperative handling system is regarded as a sphere, and the length of a rope is taken as the radius; for each aircraft, setting a random expansion vector of RRT, setting a starting point and a terminating point, and performing path search to obtain a group of discrete vectors as an initial track; carrying out inverse solution on the continuous data of each dimension, and obtaining a corresponding B spline curve control point parameter as an optimized initial parameter; and selecting all B spline curve control points as optimization variables, defining the track optimization of the aircraft as a nonlinear optimization problem, and carrying out nonlinear optimization on the optimization problem to solve the optimal control point value of the B spline curve of each dimensionality in the optimization vector, thereby obtaining the polynomial expression of each dimensionality and solving the flight path of the whole aircraft formation in real time.

Description

一种柔性绳系约束多机器人的轨迹生成方法A Trajectory Generation Method for Multiple Robots Constrained by Flexible Ropes

技术领域technical field

本发明属于机器人轨迹规划研究领域,具体涉及一种用于柔性绳系约束的多机器人的轨迹生成方法。The invention belongs to the research field of robot trajectory planning, and in particular relates to a trajectory generation method for multiple robots constrained by flexible ropes.

背景技术Background technique

近年来,多机器人协同工作变得越来越常见,常见的应用环境有多无人机编队表演、多智能小车编队表演、无人机与智能小车空地协同作业、多机械臂联合维修等等。使用多个机器人相互配合,协同作业可以增加系统的稳定性、提高系统的工作强度并且能够使系统完成更多复杂的任务。同样多机器人协作带来的就是系统的复杂度上升。多机器人协同作业一个常见的问题就是机器人的轨迹规划问题,尤其是涉及多个机器人后,机器人相互之间在时空上要避免碰撞,完成任务的同时也要保证机器人自身的安全。所以对于机器人的轨迹规划成为了一种必然的需求。In recent years, multi-robot collaborative work has become more and more common. Common application environments are multi-UAV formation performances, multi-intelligent car formation performances, drones and smart cars. Collaborative work in the air, multi-arm joint maintenance, etc. Using multiple robots to cooperate with each other can increase the stability of the system, improve the work intensity of the system, and enable the system to complete more complex tasks. The same multi-robot collaboration brings about an increase in the complexity of the system. A common problem in multi-robot collaborative operation is the trajectory planning of robots, especially when multiple robots are involved, the robots must avoid collisions with each other in space and time, and ensure the safety of the robot itself while completing the task. Therefore, the trajectory planning of robots has become an inevitable requirement.

如今,一种多飞行器协同搬运的物资的任务逐渐展现出来,相比较于单飞行器,多飞行器协同作业能够提供更好的系统通用性、安全性和可部署性,并且能够降低系统的整体成本。在飞行器运输负载这一应用场景上,单飞行器的负载能力有限或过于昂贵,能量消耗速度快。而使用多飞行器协同搬运负载可以减少系统的整体成本,增加系统的运输能力和鲁棒性。代价是需要复杂的轨迹规划算法来生成各个飞行器的轨迹,要在保证系统整体负载能力的前提下还能够达到各飞行器与负载避障的效果。Today, the task of multi-aircraft cooperative handling of materials has gradually emerged. Compared with single-aircraft, multi-aircraft cooperative operation can provide better system versatility, safety, and deployability, and can reduce the overall cost of the system. In the application scenario of aircraft transporting loads, the load capacity of a single aircraft is limited or too expensive, and the energy consumption is fast. The use of multiple aircraft to carry loads cooperatively can reduce the overall cost of the system and increase the transportation capacity and robustness of the system. The price is that a complex trajectory planning algorithm is required to generate the trajectory of each aircraft, and the effect of obstacle avoidance of each aircraft and load must be achieved on the premise of ensuring the overall load capacity of the system.

目前面向多飞行器编队飞行任务的轨迹规划方法一般有两种:一种是先生成一架飞行器的离线飞行轨迹,然后通过编队阵型仿射变换推导出其余飞行器的离线飞行轨迹,然后在线飞行过程中通过局部轨迹优化实时调整各飞行器的位置以达到避障的目的。例如,申请号为CN201910173841.5的中国专利提出了一种基于模型预测控制的多飞行器编队协同控制方法,他在首先根据多飞行器编队控制相关约束,初始化任务要求和相关控制参数,然后仅仅对领航飞行器进行初步航迹规划,之后就直接进入了在线的轨迹实施优化过程。这种方式生成的离线轨迹可能会使队形中的某些飞行器不满足避障约束,然而多飞行器绳索连接协同搬运负载的情况要考虑更多的约束并且约束的精度要求更高,此时将更多的轨迹优化压力放在了在线飞行过程中,对低性能的嵌入式平台要求更高,增加了这种协同运输任务的难度。另一种是先使用一种不考虑运动学的路径搜索方法生成一条飞行器的初始飞行轨迹,然后扩展出多条飞行器的路径,或者不指定初始轨迹,接下来再进行多飞行器的协同路径规划。例如申请号为CN201910395051.1的中国专利提出了一种多飞行器多蚁群协同搜索目标方法,他并没有生成一个初始的轨迹就直接使用蚁群算法进行多飞行器协同轨迹优化。这种方法使得非优化算法容易陷入局部极小值,并且在有狭窄障碍物时会降低优化成功率。At present, there are generally two kinds of trajectory planning methods for multi-aircraft formation flight missions: one is to first generate the offline flight trajectory of one aircraft, and then deduce the offline flight trajectories of the remaining aircraft through the affine transformation of the formation formation. Local trajectory optimization adjusts the position of each aircraft in real time to achieve obstacle avoidance. For example, the Chinese patent application number CN201910173841.5 proposes a multi-aircraft formation collaborative control method based on model predictive control. It first initializes the task requirements and related control parameters according to the constraints related to the multi-aircraft formation control, and then only controls the piloting. The aircraft performs preliminary trajectory planning, and then directly enters the online trajectory implementation optimization process. The offline trajectories generated in this way may make some aircraft in the formation not meet the obstacle avoidance constraints. However, in the case of multi-rope connection and coordinated load handling, more constraints should be considered and the accuracy of the constraints should be higher. In this case, the More trajectory optimization pressure is placed on the online flight process, and the low-performance embedded platform is more demanding, which increases the difficulty of this collaborative transportation task. The other is to first use a path search method that does not consider kinematics to generate the initial flight trajectory of an aircraft, and then expand the paths of multiple aircraft, or do not specify the initial trajectory, and then perform collaborative path planning for multiple aircraft. For example, the Chinese patent with the application number CN201910395051.1 proposes a multi-aircraft multi-ant colony collaborative search target method, which directly uses the ant colony algorithm to optimize the multi-aircraft collaborative trajectory without generating an initial trajectory. This approach makes non-optimized algorithms prone to falling into local minima and reduces the optimization success rate when there are narrow obstacles.

发明内容SUMMARY OF THE INVENTION

要解决的技术问题technical problem to be solved

为了避免现有技术的不足之处,本发明提出一种针对多飞行器用柔性绳索连接负载并协同飞行下生成飞行器轨迹的方法。旨在实现在已知栅格地图的情况下,给出多飞行器协同运输负载的飞行轨迹。In order to avoid the deficiencies of the prior art, the present invention proposes a method for connecting loads with flexible ropes for multiple aircrafts and generating aircraft trajectories under coordinated flight. The purpose is to give the flight trajectory of multi-aircraft cooperatively transporting loads in the case of a known grid map.

技术方案Technical solutions

一种柔性绳系约束多机器人的轨迹生成方法,其特征在于步骤如下:A method for generating a trajectory for multiple robots constrained by a flexible rope system is characterized in that the steps are as follows:

步骤1:生成整体队形初始路径Step 1: Generate the initial path of the overall formation

1.1)获取地图信息:通过计算机视觉获取实际环境的图像信息,然后生成一个ESDF全局栅格地图,所述的ESDF全局栅格地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值;1.1) Obtain map information: obtain image information of the actual environment through computer vision, and then generate an ESDF global grid map, which can provide the distance from each grid to the nearest obstacle and the distance from the obstacle the gradient value of ;

1.2)定义RRT*路径搜索算法参数:将整个阵型视为一个以载荷位置为球心,以绳索长度为半径的球体;对于每一个飞行器,分别在竖直方向和水平方向上定义一个偏转角;每一个飞行器的位置都可以通过载荷位置以及两个偏转角求出;这种球体的优化参数只需要包括载荷的绝对位置和飞行器相对于载荷的角度值即可求解出所有飞行器的绝对位置;从而设定一个3+2n维的向量作为RRT*的随机扩展向量,3+2n维即载荷三维位置以及n个飞行器的两个偏转角;1.2) Define the parameters of the RRT* path search algorithm: regard the entire formation as a sphere with the load position as the center and the rope length as the radius; for each aircraft, define a deflection angle in the vertical and horizontal directions respectively; The position of each aircraft can be obtained by the load position and two deflection angles; the optimization parameters of this sphere only need to include the absolute position of the load and the angle value of the aircraft relative to the load to obtain the absolute position of all aircraft; thus Set a 3+2n-dimensional vector as the random extension vector of RRT*, the 3+2n-dimension is the three-dimensional position of the load and the two deflection angles of n aircraft;

1.3)生成初始飞行轨迹:设定好起点和终止点,进行路径搜索,得到一组离散的向量,作为初始轨迹;1.3) Generate the initial flight trajectory: set the starting point and the end point, conduct a path search, and obtain a set of discrete vectors as the initial trajectory;

步骤2:求解轨迹优化初始解参数Step 2: Solve the trajectory to optimize the initial solution parameters

2.1)对相邻的两个向量间的每个维度分别进行插值,在每个维度上得到一组近似连续的值;2.1) Interpolate each dimension between two adjacent vectors respectively, and obtain a set of approximately continuous values in each dimension;

2.2)对每个维度的连续数据进行反解,求得对应的B样条曲线控制点参数作为优化的初始参数;2.2) Inversely solve the continuous data of each dimension, and obtain the corresponding B-spline curve control point parameters as the initial parameters of optimization;

步骤3:优化出满足约束条件的飞行轨迹Step 3: Optimize the flight trajectory that satisfies the constraints

3.1)选择所有的B样条曲线控制点作为优化变量,输入步骤2中得到的B样条曲线控制点参数作为初始解;3.1) Select all B-spline curve control points as optimization variables, and input the B-spline curve control point parameters obtained in step 2 as the initial solution;

3.2)将飞行器的轨迹优化定义为一个非线性优化问题,优化问题代价包括轨迹平滑度代价、终点约束、动力学可行性约束、飞行器间防碰撞约束、飞行器阵型避障约束以及推力约束;3.2) Define the trajectory optimization of the aircraft as a nonlinear optimization problem, and the optimization problem costs include trajectory smoothness cost, end point constraints, dynamic feasibility constraints, collision avoidance constraints between aircraft, aircraft formation obstacle avoidance constraints, and thrust constraints;

3.3)对步骤3.2)的优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值,从而得到每个维度的多项式表达式,实时解出整个飞行器编队的飞行路径。3.3) Non-linear optimization of the optimization problem in step 3.2) can solve the optimal control point value of the B-spline curve in each dimension in the optimization vector, so as to obtain the polynomial expression of each dimension, and solve the entire aircraft in real time The flight path of the formation.

优选地:步骤1.2)中的n为2。Preferably: n in step 1.2) is 2.

优选地:步骤3.2)中优化问题代价可表示为下式:Preferably: the cost of the optimization problem in step 3.2) can be expressed as the following formula:

ftotal=λ1fs2fi3fc4fe+(λ5fv6fa7fw8fwa)+λ9ff f total1 f s2 f i3 f c4 f e +(λ 5 f v6 f a7 f w8 f wa )+λ 9 f f

Figure BDA0003145468600000041
Figure BDA0003145468600000041

Figure BDA0003145468600000042
Figure BDA0003145468600000042

Figure BDA0003145468600000043
Figure BDA0003145468600000043

Figure BDA0003145468600000044
Figure BDA0003145468600000044

Figure BDA0003145468600000045
Figure BDA0003145468600000045

Figure BDA0003145468600000046
Figure BDA0003145468600000046

Figure BDA0003145468600000047
Figure BDA0003145468600000047

Figure BDA0003145468600000048
Figure BDA0003145468600000048

Figure BDA0003145468600000049
Figure BDA0003145468600000049

其中q为9维向量,amax为最大加速度,vmax为最大速度,wmax为最大角速度,wamax为最大角加速度,dmin为飞行器间最小距离,dmax为飞行器间最大距离,obsmin为避障允许最小距离,qend为飞行终点qend,dnear为从ESDF地图中可以获得每个点到距离最近的障碍物的距离,λ1、λ2、λ3、λ4、λ5、λ6、λ7、λ8、λ9为各个代价的权值。where q is a 9-dimensional vector, a max is the maximum acceleration, v max is the maximum velocity, w max is the maximum angular velocity, wa max is the maximum angular acceleration, d min is the minimum distance between aircraft, d max is the maximum distance between aircraft, obs min The minimum distance allowed for obstacle avoidance, q end is the flight end q end , d near is the distance from each point to the nearest obstacle from the ESDF map, λ 1 , λ 2 , λ 3 , λ 4 , λ 5 , λ 6 , λ 7 , λ 8 , and λ 9 are the weights of each cost.

有益效果beneficial effect

本发明提出的一种柔性绳系约束多机器人的轨迹生成方法,具有以下益处:A trajectory generation method for multiple robots constrained by a flexible rope system proposed by the present invention has the following benefits:

1、使用了两阶段轨迹规划算法,首先用RRT*寻找到一个初始轨迹解,然后在使用非线性优化对初始轨迹解进行优化,这样可以增快优化的速度并且更容易找到全局最优解而不是陷入局部最优解;1. A two-stage trajectory planning algorithm is used. First, use RRT* to find an initial trajectory solution, and then use nonlinear optimization to optimize the initial trajectory solution, which can speed up the optimization and make it easier to find the global optimal solution. not trapped in a local optimal solution;

2、将整个飞行器协同搬运系统看做一个球体,以载荷位置为圆心,以绳长为半径。将绳长约束转化为位置解算参数,简化了优化所需的参数数量和优化复杂性;2. Consider the entire aircraft cooperative handling system as a sphere, with the load position as the center and the rope length as the radius. Converting rope length constraints into position solution parameters simplifies the number of parameters and optimization complexity required for optimization;

3、使用ESDF地图代替普通的栅格地图,容易获取到每个飞行器到最近障碍物的距离,并且可以获得该点处远离障碍物的梯度值,利于避障代价的计算。3. Using the ESDF map instead of the ordinary grid map, it is easy to obtain the distance from each aircraft to the nearest obstacle, and the gradient value of the point away from the obstacle can be obtained, which is beneficial to the calculation of the obstacle avoidance cost.

4、考虑到了每个飞行器的推力约束,使得优化的轨迹尽可能的减少整个系统的能量消耗。4. Taking into account the thrust constraints of each aircraft, the optimized trajectory can reduce the energy consumption of the entire system as much as possible.

5、通过调节每一次的优化的距离以及优化的迭代次数,使得该优化算法既可以用于离线的全局规划,也可以用于在线的局部规划。5. By adjusting the distance of each optimization and the number of optimization iterations, the optimization algorithm can be used for both offline global planning and online local planning.

附图说明Description of drawings

附图仅用于示出具体实施例的目的,而并不认为是对本发明的限制,在整个附图中,相同的参考符号表示相同的部件。The drawings are for the purpose of illustrating specific embodiments only and are not to be considered limiting of the invention, and like reference numerals refer to like parts throughout the drawings.

图1多飞行器协同搬运负载系统整体图;Figure 1. Overall diagram of the multi-aircraft cooperative handling load system;

图2系统整体算法流程图;Figure 2 is a flow chart of the overall algorithm of the system;

图3仿真结果图。Figure 3 shows the simulation results.

具体实施方式Detailed ways

为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图和实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。此外,下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, but not to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

本发明所采用的技术方案包括以下步骤:The technical scheme adopted in the present invention comprises the following steps:

1)生成整体队形初始路径;1) Generate the initial path of the overall formation;

2)求解轨迹优化初始解参数;2) Solve the trajectory to optimize the initial solution parameters;

3)优化出满足约束条件的飞行轨迹;3) Optimize the flight trajectory that satisfies the constraints;

所述的步骤1)生成整体队形初始路径包含以下子步骤:The step 1) generating the initial path of the overall formation includes the following sub-steps:

1.1)获取地图信息:通过计算机视觉获取实际环境的图像信息,然后生成一个ESDF全局栅格地图。ESDF地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值;1.1) Obtain map information: Obtain image information of the actual environment through computer vision, and then generate an ESDF global grid map. The ESDF map can provide the distance from each grid to the nearest obstacle and the gradient value away from the obstacle;

1.2)定义RRT*路径搜索算法参数:将整个阵型视为一个以载荷位置为球心,以绳索长度为半径的球体。对于每一个飞行器,分别在竖直方向和水平方向上定义一个偏转角。每一个飞行器的位置都可以通过载荷位置以及两个偏转角求出。这种球体的优化参数只需要包括载荷的绝对位置和飞行器相对于载荷的角度值即可求解出所有飞行器的绝对位置。从而设定一个3+2n维的向量(载荷三维位置以及n个飞行器的两个偏转角)作为RRT*的随机扩展向量;1.2) Define the parameters of the RRT* path search algorithm: treat the entire formation as a sphere with the load position as the center and the rope length as the radius. For each aircraft, a deflection angle is defined in the vertical and horizontal directions, respectively. The position of each aircraft can be obtained from the load position and the two deflection angles. The optimization parameters of this sphere only need to include the absolute position of the load and the angle value of the aircraft relative to the load to solve the absolute position of all aircraft. Thus, a 3+2n-dimensional vector (the three-dimensional position of the load and the two deflection angles of n aircraft) is set as the random extension vector of RRT*;

1.3)生成初始飞行轨迹:设定好起点和终止点,进行路径搜索,得到一组离散的向量,作为初始轨迹。1.3) Generate the initial flight trajectory: set the starting point and the end point, conduct a path search, and obtain a set of discrete vectors as the initial trajectory.

所述的步骤2)求解轨迹优化初始解参数包含以下子步骤:The described step 2) solving the trajectory optimization initial solution parameters includes the following sub-steps:

2.1)步骤1)中得到的向量每一个维度都是解耦的,对相邻的两个向量间的每个维度分别进行插值,在每个维度上得到一组近似连续的值;2.1) Each dimension of the vector obtained in step 1) is decoupled, and each dimension between two adjacent vectors is interpolated respectively, and a set of approximately continuous values is obtained in each dimension;

2.2)对每个维度的连续数据进行反解,求得对应的B样条曲线控制点参数作为优化的初始参数。2.2) Perform an inverse solution on the continuous data of each dimension, and obtain the corresponding B-spline curve control point parameters as the initial parameters for optimization.

所述的步骤3)优化出满足约束条件的轨迹包含以下子步骤The step 3) optimizes the trajectory that satisfies the constraints and includes the following sub-steps

3.1)选择所有的B样条曲线控制点作为优化变量,输入步骤2)中得到的B样条曲线控制点参数作为初始解;3.1) Select all B-spline curve control points as optimization variables, and input the B-spline curve control point parameters obtained in step 2) as the initial solution;

3.2)将飞行器的轨迹优化定义为一个非线性优化问题,优化问题代价包括轨迹平滑度代价、终点约束、动力学可行性约束、飞行器间防碰撞约束、飞行器阵型避障约束以及推力约束;3.2) Define the trajectory optimization of the aircraft as a nonlinear optimization problem, and the optimization problem costs include trajectory smoothness cost, end point constraints, dynamic feasibility constraints, collision avoidance constraints between aircraft, aircraft formation obstacle avoidance constraints, and thrust constraints;

3.3)通过步骤3.1)、3.2)、3.3)将整个路径规划问题转换为一个非线性优化问题,并确定了优化的一组良好的初始解、优化问题以及优化约束,对该优化问题进行非线性优化即可解出优化向量中每个维度的B样条曲线的最优控制点值,从而得到每个维度的多项式表达式,实时解出整个飞行器编队的飞行路径。3.3) Through steps 3.1), 3.2), and 3.3), the entire path planning problem is transformed into a nonlinear optimization problem, and a set of good initial solutions, optimization problems and optimization constraints for optimization are determined, and the optimization problem is nonlinear. The optimization can solve the optimal control point value of the B-spline curve of each dimension in the optimization vector, so as to obtain the polynomial expression of each dimension, and solve the flight path of the entire aircraft formation in real time.

上述步骤具体如下:The above steps are as follows:

1)生成载荷的飞行轨迹包含以下子步骤:1) The flight trajectory for generating the payload consists of the following sub-steps:

1.1)获取地图信息:通过计算机视觉获取实际环境的图像信息,然后生成一个ESDF全局栅格地图。ESDF地图可以提供每一个栅格到最近的障碍物的距离以及远离障碍物的梯度值;1.1) Obtain map information: Obtain image information of the actual environment through computer vision, and then generate an ESDF global grid map. The ESDF map can provide the distance from each grid to the nearest obstacle and the gradient value away from the obstacle;

1.2)定义RRT*路径搜索算法参数:如图1所示,整个飞行器协同搬运阵型被视作一个球体。定义负载的三维坐标为[x y z],每个无人机的两个偏转角分别为

Figure BDA0003145468600000071
在本发明中以三架无人机为例,则整个阵型的状态可以用一个9维向量
Figure BDA0003145468600000072
表示。设绳子长度为r,对于任意一个飞行器,其三维坐标可以由公式(1)解出,1.2) Define the parameters of the RRT* path search algorithm: As shown in Figure 1, the entire aircraft cooperative handling formation is regarded as a sphere. Define the three-dimensional coordinates of the load as [xyz], and the two deflection angles of each drone are respectively
Figure BDA0003145468600000071
In the present invention, taking three UAVs as an example, the state of the entire formation can use a 9-dimensional vector
Figure BDA0003145468600000072
express. Let the length of the rope be r, for any aircraft, its three-dimensional coordinates can be solved by formula (1),

Figure BDA0003145468600000073
Figure BDA0003145468600000073

1.3)生成初始飞行轨迹:将这个9维向量作为RRT*轨迹搜索算法的输入向量,每一个维上每次随机生成一个偏移值,然后以负载的位置作为扩展方向,每一次扩展后判断阵型是否碰撞到障碍物,直到负载的位置到达终点,最终得到m个离散的向量q(m),表示了阵型飞行的路径以及每个时刻的阵型状态;1.3) Generate the initial flight trajectory: use this 9-dimensional vector as the input vector of the RRT* trajectory search algorithm, randomly generate an offset value in each dimension, and then use the position of the load as the expansion direction, and determine the formation after each expansion. Whether to collide with an obstacle until the position of the load reaches the end point, and finally get m discrete vectors q(m), which represent the flight path of the formation and the formation state at each moment;

2)求解轨迹优化初始解参数包含以下子步骤:2) Solving the trajectory optimization initial solution parameters includes the following sub-steps:

2.1)在步骤1)中得到的向量每一个维度都是解耦的,对相邻的两个向量间的每个维度分别进行插值,在每个维度上得到一组近似连续的值;2.1) Each dimension of the vector obtained in step 1) is decoupled, and each dimension between two adjacent vectors is interpolated respectively, and a set of approximately continuous values is obtained in each dimension;

2.2)对每个维度的连续数据进行拟合成多项式,然后对多项式进行反解,求得对应的B样条曲线控制点,将这些控制点作为优化的初始参数;2.2) Fit the continuous data of each dimension into a polynomial, then reverse the polynomial to obtain the corresponding B-spline curve control points, and use these control points as the initial parameters of optimization;

3)优化出满足约束条件的轨迹包含以下子步骤:3) Optimizing the trajectory that satisfies the constraints includes the following sub-steps:

3.1)选择所有的B样条曲线控制点作为优化变量,输入步骤2)中得到的B样条曲线控制点参数作为初始解,定义优化变量为i个9维向量q(i)最大加速度为amax,最大速度为vmax,最大角速度为wmax,最大角加速度为wamax,飞行器间最小距离为dmin,飞行器间最大距离为dmax,避障允许最小距离obsmin,飞行终点qend,从ESDF地图中可以获得每个点到距离最近的障碍物的距离为dnear,由飞行器的当前阵型排布可以反解出每个飞行器给绳子的拉力F;3.1) Select all B-spline curve control points as optimization variables, input the B-spline curve control point parameters obtained in step 2) as the initial solution, and define the optimization variables as i 9-dimensional vectors q(i) The maximum acceleration is a max , the maximum speed is v max , the maximum angular velocity is w max , the maximum angular acceleration is wa max , the minimum distance between aircrafts is d min , the maximum distance between aircraft is d max , the minimum allowable distance for obstacle avoidance is obs min , the flight end point q end , The distance from each point to the nearest obstacle can be obtained from the ESDF map as d near , and the pulling force F of each aircraft to the rope can be inversely calculated from the current formation of the aircraft;

3.2)定义轨迹优化问题包括轨迹平滑度代价、动力学可行性代价、飞机间防碰撞代价、飞机阵型避障代价、飞行终点到达度代价以及推力代价,如公式(2)所示3.2) Define the trajectory optimization problem including trajectory smoothness cost, dynamic feasibility cost, anti-collision cost between aircraft, aircraft formation obstacle avoidance cost, flight end arrival cost and thrust cost, as shown in formula (2)

ftotal=λ1fs2fi3fc4fe+(λ5fv6fa7fw8fwa)+λ9ff。 (2)f total1 f s2 f i3 f c4 f e +(λ 5 f v6 f a7 f w8 f wa )+λ 9 f f . (2)

λ1、λ2、λ3、λ4、λ5、λ6、λ7、λ8、λ9为各个代价的权值;λ 1 , λ 2 , λ 3 , λ 4 , λ 5 , λ 6 , λ 7 , λ 8 , λ 9 are the weights of each cost;

其中,轨迹平滑度代价:由于无人机系统的特殊性,其多项式轨迹的三阶导数代表轨迹的无人机欧拉角的角速度,与无人机的轨迹平滑度相关,故依据B样条曲线的导数特性得到平滑度代价函数如下:Among them, the cost of trajectory smoothness: due to the particularity of the UAV system, the third derivative of its polynomial trajectory represents the angular velocity of the UAV Euler angle of the trajectory, which is related to the trajectory smoothness of the UAV. Therefore, according to the B-spline The derivative characteristic of the curve yields the smoothness cost function as follows:

Figure BDA0003145468600000081
Figure BDA0003145468600000081

终点到达度代价:End point arrival cost:

Figure BDA0003145468600000082
Figure BDA0003145468600000082

动力学可行性代价:公式(5)、(6)中的q仅计算前三维,对应的载荷三维位置,求解载荷的速度和加速度代价Dynamic feasibility cost: q in formulas (5) and (6) only calculates the front three-dimensional, the corresponding three-dimensional position of the load, and solves the speed and acceleration cost of the load

Figure BDA0003145468600000091
Figure BDA0003145468600000091

Figure BDA0003145468600000092
Figure BDA0003145468600000092

公式(7)、(8)中的q计算后六维,对应的每个无人机相对于载荷的角度偏移量,求解无人机相对运动的角速度和角加速度代价q in formulas (7) and (8) calculates the back six dimensions, the corresponding angular offset of each UAV relative to the load, and solves the angular velocity and angular acceleration cost of the relative motion of the UAV

Figure BDA0003145468600000093
Figure BDA0003145468600000093

Figure BDA0003145468600000094
Figure BDA0003145468600000094

飞行器间防碰撞代价:Cost of collision avoidance between aircraft:

Figure BDA0003145468600000095
Figure BDA0003145468600000095

飞行器阵型避障代价:由公式(1)可以得出每一架飞行器的位置以及载荷的位置,在每一架飞行器到载荷的绳上取8个点作为碰撞检测点,代价函数如下:Aircraft formation obstacle avoidance cost: From formula (1), the position of each aircraft and the position of the load can be obtained, and 8 points are taken as the collision detection points on the rope from each aircraft to the load. The cost function is as follows:

Figure BDA0003145468600000096
Figure BDA0003145468600000096

以及推力代价:and the thrust cost:

Figure BDA0003145468600000097
Figure BDA0003145468600000097

3.3)通过步骤3.1)、3.2)将整个路径规划问题转换为一个非线性优化问题,并确定了优化的一组良好的初始解、优化问题代价函数,对该优化问题进行非线性优化即可解出优化出每个维度的B样条曲线的最优控制点值,从而得到每个维度的多项式表达式,实时解出整个飞行器编队的飞行路径。3.3) Through steps 3.1) and 3.2), the entire path planning problem is converted into a nonlinear optimization problem, and a set of good initial solutions and optimization problem cost functions for optimization are determined, and the optimization problem can be solved by nonlinear optimization The optimal control point value of the B-spline curve of each dimension is optimized, and the polynomial expression of each dimension is obtained, and the flight path of the entire aircraft formation is solved in real time.

定义平滑度权重为10,终点到达度权重为0.01,动力学可行性权重为0.001,飞机间防碰撞权重为1,飞行器阵型避障权重为1,推力权重为0.0001。此时在CPU为i5-8250u的笔记本上运行规划算法,各个约束的cost值如下f smoothness:0.12772Define the smoothness weight as 10, the end point arrival weight as 0.01, the dynamic feasibility weight as 0.001, the anti-collision weight between aircraft as 1, the aircraft formation obstacle avoidance weight as 1, and the thrust weight as 0.0001. At this time, the planning algorithm is run on a notebook with a CPU of i5-8250u. The cost value of each constraint is as follows: f smoothness: 0.12772

f distance:0.000663906f distance: 0.000663906

f feasibility:9.63905e-05f feasibility: 9.63905e-05

f thrust:0f thrust: 0

f obs:2.95077e-06f obs: 2.95077e-06

f_combine:0.128925f_combine: 0.128925

代码运算时间为0.8765s,规划出的路径如图3所示,轨迹是平滑的并且避开了所有的障碍物,也没有相互碰撞,满足了所有的约束条件,其中4条蓝色(折线)的轨迹分别指无人机和载荷在前端使用RRT*算法寻找出的良好初始轨迹,4条红色(平滑曲线)的轨迹分别代表3架无人机和载荷的最终轨迹。The code operation time is 0.8765s. The planned path is shown in Figure 3. The trajectory is smooth and avoids all obstacles and does not collide with each other. All constraints are met, of which 4 are blue (polylines) The trajectories refer to the good initial trajectories found by the UAV and the payload using the RRT* algorithm at the front end, respectively, and the 4 red (smooth curves) trajectories represent the final trajectories of the 3 UAVs and the payload, respectively.

以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明公开的技术范围内,可轻易想到各种等效的修改或替换,这些修改或替换都应涵盖在本发明的保护范围之内。The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited to this. Any person skilled in the art can easily think of various equivalents within the technical scope disclosed by the present invention. Modifications or substitutions should be included within the protection scope of the present invention.

Claims (2)

1. A track generation method for a flexible rope system constraint multi-robot is characterized by comprising the following steps:
step 1: generating an initial path of an overall formation
1.1) obtaining map information: acquiring image information of an actual environment through computer vision, and then generating an ESDF global grid map which can provide the distance from each grid to a nearest obstacle and the gradient value far away from the obstacle;
1.2) defining RRT path search algorithm parameters: the whole matrix is regarded as a sphere which takes the load position as the center of sphere and takes the length of the rope as the radius; for each aircraft, defining a deflection angle in the vertical direction and the horizontal direction respectively; the position of each aircraft can be obtained through the load position and two deflection angles; the optimization parameters of the sphere only need to comprise the absolute position of the load and the angle value of the aircraft relative to the load, and the absolute positions of all the aircraft can be solved; setting a vector with 3+2n dimensions as a random expansion vector of RRT, wherein the 3+2n dimensions are the three-dimensional position of the load and two deflection angles of n aircrafts;
1.3) generating an initial flight track: setting a starting point and an end point, and performing path search to obtain a group of discrete vectors as an initial track;
step 2: solving track optimization initial solution parameters
2.1) carrying out interpolation on each dimension between two adjacent vectors respectively to obtain a group of approximately continuous values on each dimension;
2.2) carrying out inverse solution on the continuous data of each dimension to obtain a corresponding B spline curve control point parameter as an optimized initial parameter;
and step 3: optimizing a flight trajectory satisfying a constraint condition
3.1) selecting all B-spline curve control points as optimization variables, and inputting the parameters of the B-spline curve control points obtained in the step 2 as initial solutions;
3.2) defining the track optimization of the aircraft as a nonlinear optimization problem, wherein the optimization problem cost comprises track smoothness cost, end point constraint, dynamic feasibility constraint, inter-aircraft collision-prevention constraint, aircraft formation obstacle avoidance constraint and thrust constraint;
3.3) carrying out nonlinear optimization on the optimization problem in the step 3.2) to solve the optimal control point value of the B spline curve of each dimensionality in the optimization vector, thereby obtaining a polynomial expression of each dimensionality and solving the flight path of the whole aircraft formation in real time;
the optimization problem cost can be expressed as the following equation:
ftotal=λ1fs2fi3fc4fe+(λ5fv6fa7fw8fwa)+λ9ff
Figure FDA0003625809080000021
Figure FDA0003625809080000022
Figure FDA0003625809080000023
Figure FDA0003625809080000024
Figure FDA0003625809080000025
Figure FDA0003625809080000026
Figure FDA0003625809080000027
Figure FDA0003625809080000028
Figure FDA0003625809080000031
wherein q is a 9-dimensional vector, amaxIs the maximum acceleration, vmaxIs the maximum speed, wmaxAt maximum angular velocity, wamaxAt maximum angular acceleration, dminMinimum distance between aircraft, dmaxMaximum distance between aircraft, obsminMinimum distance allowed for obstacle avoidance, qendTo the flight end point qend,dnearTo obtain the distance from each point to the nearest obstacle from the ESDF map, λ1、λ2、λ3、λ4、λ5、λ6、λ7、λ8、λ9Is the weight of each cost.
2. The method for generating a trajectory of multiple robots constrained by flexible ropes according to claim 1, wherein n in step 1.2 is 2.
CN202110749405.5A 2021-07-02 2021-07-02 Flexible rope system constraint multi-robot track generation method Active CN113485418B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110749405.5A CN113485418B (en) 2021-07-02 2021-07-02 Flexible rope system constraint multi-robot track generation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110749405.5A CN113485418B (en) 2021-07-02 2021-07-02 Flexible rope system constraint multi-robot track generation method

Publications (2)

Publication Number Publication Date
CN113485418A CN113485418A (en) 2021-10-08
CN113485418B true CN113485418B (en) 2022-07-05

Family

ID=77939422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110749405.5A Active CN113485418B (en) 2021-07-02 2021-07-02 Flexible rope system constraint multi-robot track generation method

Country Status (1)

Country Link
CN (1) CN113485418B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114167732B (en) * 2021-12-13 2024-08-09 西北工业大学 Coupling constraint multi-agent distributed robust nonlinear model prediction control method
CN117466161B (en) * 2023-09-08 2024-06-11 兰州交通大学 An obstacle avoidance trajectory planning method for a multi-machine suspension system
CN118466584B (en) * 2024-04-30 2024-11-15 空中地标(浙江)科技有限公司 Formation flight control method suitable for unmanned aerial vehicle advertising
CN119579383B (en) * 2025-02-08 2025-04-15 北京理工大学 Multi-vehicle collaborative track planning method, system and equipment for heavy-load cargo transportation

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104192713A (en) * 2014-09-10 2014-12-10 南开大学 Time-optimal bridge crane track planning method based on differential flatness and B-spline
CN106842926A (en) * 2017-02-08 2017-06-13 北京航空航天大学 A kind of aerial vehicle trajectory optimization method based on positive real B-spline
CN108326852A (en) * 2018-01-16 2018-07-27 西北工业大学 A kind of space manipulator method for planning track of multiple-objection optimization
CN108563243A (en) * 2018-06-28 2018-09-21 西北工业大学 A kind of unmanned aerial vehicle flight path planing method based on improvement RRT algorithms
CN109542106A (en) * 2019-01-04 2019-03-29 电子科技大学 A kind of paths planning method under mobile robot multi-constraint condition
CN110926477A (en) * 2019-12-17 2020-03-27 湘潭大学 A UAV Route Planning and Obstacle Avoidance Method
KR20200042394A (en) * 2018-10-12 2020-04-23 오로라 플라이트 사이언시스 코퍼레이션 Trajectory planner for a vehicle
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112068588A (en) * 2020-08-12 2020-12-11 浙江大学 A method for generating trajectory of unmanned aerial vehicle based on flight corridor and Bezier curve
CN112596549A (en) * 2020-12-29 2021-04-02 中山大学 Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule
CN112965523A (en) * 2021-02-09 2021-06-15 西北工业大学 Offline track generation method for rope-connected multiple aircrafts

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104192713A (en) * 2014-09-10 2014-12-10 南开大学 Time-optimal bridge crane track planning method based on differential flatness and B-spline
CN106842926A (en) * 2017-02-08 2017-06-13 北京航空航天大学 A kind of aerial vehicle trajectory optimization method based on positive real B-spline
CN108326852A (en) * 2018-01-16 2018-07-27 西北工业大学 A kind of space manipulator method for planning track of multiple-objection optimization
CN108563243A (en) * 2018-06-28 2018-09-21 西北工业大学 A kind of unmanned aerial vehicle flight path planing method based on improvement RRT algorithms
KR20200042394A (en) * 2018-10-12 2020-04-23 오로라 플라이트 사이언시스 코퍼레이션 Trajectory planner for a vehicle
CN109542106A (en) * 2019-01-04 2019-03-29 电子科技大学 A kind of paths planning method under mobile robot multi-constraint condition
CN110926477A (en) * 2019-12-17 2020-03-27 湘潭大学 A UAV Route Planning and Obstacle Avoidance Method
CN111562797A (en) * 2020-07-06 2020-08-21 北京理工大学 Unmanned aerial vehicle flight time optimal real-time trajectory optimization method capable of ensuring convergence
CN112068588A (en) * 2020-08-12 2020-12-11 浙江大学 A method for generating trajectory of unmanned aerial vehicle based on flight corridor and Bezier curve
CN112596549A (en) * 2020-12-29 2021-04-02 中山大学 Multi-unmanned aerial vehicle formation control method, device and medium based on continuous convex rule
CN112965523A (en) * 2021-02-09 2021-06-15 西北工业大学 Offline track generation method for rope-connected multiple aircrafts

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Approach Trajectory Planning of Space Robot for Impact Minimization;Panfeng Huang等;《Proceedings of the 2006 IEEE International Conference on Information Acquisition》;20060823;全文 *
考虑时间约束的多飞行器轨迹优化方法研究;杨卓乔等;《航天控制》;20210415;第39卷(第2期);全文 *
视觉导引受限下空间绳系机器人最优逼近控制;胡永新;《宇航学报》;20190430;第40卷(第4期);全文 *

Also Published As

Publication number Publication date
CN113485418A (en) 2021-10-08

Similar Documents

Publication Publication Date Title
CN113485418B (en) Flexible rope system constraint multi-robot track generation method
Li et al. Trajectory planning of load transportation with multi-quadrotors based on reinforcement learning algorithm
Liang et al. Unmanned aerial transportation system with flexible connection between the quadrotor and the payload: modeling, controller design, and experimental validation
CN112965523B (en) Offline track generation method for rope-connected multiple aircrafts
CN111367318A (en) A dynamic obstacle environment navigation method and device based on visual semantic information
Tao et al. Swing-reducing flight control system for an underactuated indoor miniature autonomous blimp
CN110687917A (en) Distributed formation reconfiguration formation control method for multiple unmanned boats
CN110231821A (en) The adaptive kernel action amalgamation method of the improvement of multi-robot formation
CN113900449B (en) Multi-unmanned aerial vehicle track planning method and device, unmanned aerial vehicle and storage medium
Zhao et al. Online motion planning for deforming maneuvering and manipulation by multilinked aerial robot based on differential kinematics
CN112947572A (en) Terrain following-based four-rotor aircraft self-adaptive motion planning method
CN110162053A (en) The adaptive behavior fusion method that the more unmanned boats of isomery are formed into columns
Joukhadar et al. Control Systems and Applications
CN116719347A (en) Multi-rotor unmanned aerial vehicle online safety track generation method under multi-constraint
CN116476080A (en) An Aerial Automatic Grasping Operation Planning Method Based on Geometric Feasibility
Ping et al. Collaborative control based on payload-leading for the multi-quadrotor transportation systems
Su et al. Object gathering with a tethered robot duo
Xie et al. A data-driven obstacle avoidance scheme for redundant robots with unknown structures
CN118672296B (en) A method and device for real-time planning of high-speed obstacle avoidance trajectories for a multi-machine hoisting system
Kamath et al. Vision augmented 3 DoF quadrotor control using a non-singular fast-terminal sliding mode modified super-twisting controller
CN113821028B (en) Underactuated AUV formation track tracking control method based on distributed model predictive control
Silveira et al. Optimal visual servoed guidance of outdoor autonomous robotic airships
CN117742355A (en) Four-rotor unmanned aerial vehicle hanging transportation flight control method based on unknown environment
Bipin et al. Autonomous navigation of generic quadrocopter with minimum time trajectory planning and control
CN113031651B (en) Bilateral teleoperation control system and method of UAV hanging system based on value function approximation

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