一种基于轨迹点处理的无人机自主飞行控制方法

本发明涉及无人机，尤其是一种基于轨迹点处理的无人机自主飞行控制方法。

背景技术：

1、随着无人机技术的快速发展，无人机自主导航已经成为了多个领域不可或缺的技术力量。在军事侦察、地理勘测、灾害监测、物流运输等领域，无人机自主导航技术的应用不仅极大提高了作业效率，还拓展了无人机的作业范围，使其能够执行更为复杂和危险的任务，从而保障人类的安全和福祉。在这样的背景下，探索和发展有效的无人机导航与控制方法，对于提升无人机的自主性、效率和安全性具有重要意义。

2、但是，现有技术中，无人机自主导航通常采用直接航点控制或是基于规则的控制策略。这些方法在简单环境下效果显著，但在复杂或动态变化的环境中，面临着适应性不足、路径优化有限等问题。

3、1)直接航点控制：这种方法通过直接设定一系列航点来控制无人机飞行路径。无人机依次飞向这些预设的航点，从而完成导航。虽然简单直观，但在面对复杂环境或需精确控制飞行轨迹时，此方法的灵活性和精确度不足。

4、2)基于规则的控制：依赖特定的规则或算法(如避障算法)来调整无人机的飞行方向和速度。这种方法对动态环境有一定的适应性，但在高动态或复杂环境中，规则的制定和执行可能不够准确或高效。

5、3)经典pid控制：通过比例(p)、积分(i)、微分(d)控制器来调整无人机的飞行状态，以接近预定轨迹。pid控制在一定场景下稳定可靠，但对于复杂或预先未知的飞行路径，pid调参困难，难以实现最优飞行效率。

技术实现思路

1、针对现有技术的不足，本发明提供一种基于轨迹点处理的无人机自主飞行控制方法，通过对轨迹点进行多项式拟合处理、考虑运动控制的平稳性进行轨迹点优化，以及应用模型预测控制(mpc)算法来实现无人机的高效、平稳飞行。

2、本发明的技术方案为：一种基于轨迹点处理的无人机自主飞行控制方法，包括以下步骤：

3、s1)、根据任务需求和预测的环境信息由规划算法生成一系列轨迹点；

4、s2)、将规划出的轨迹点坐标由机体坐标系转换为世界坐标系；

5、s3)、对坐标转换后的轨迹点进行优化处理；

6、s4)、将优化处理后的轨迹点输入mpc模型，通过mpc模型预测出对应的无人机控制指令，最终实现轨迹控制。

7、作为优选的，步骤s2)中，所述的轨迹点的坐标转换包括位置、速度、加速度、jrek和snap的转换，以及姿态的更新。

8、作为优选的，步骤s2)中，所述的轨迹点位置的坐标转换表达式为：

9、pw＝rw_sgps+tw_s；

10、其中，pw为每个轨迹点转换为世界坐标系下的位置，rw_s、tw_s分别表示变换矩阵tw_s的旋转矩阵和平移向量，ps为每个轨迹点在机体坐标系下的位置。

11、作为优选的，步骤s2)中，所述每个轨迹点速度、加速度、jrek和snap的转换只涉及方向的改变，不涉及位置的变化；

12、因此，设定机体坐标系中的轨迹点的速度、加速度、jrek和snap的向量表示为vs，转换后的世界坐标系中的轨迹点速度、加速度、jrek和snap表示为vw，那么转换表达式为：

13、vw＝rw_sgvs；

14、式中，rw_s表示变换矩阵的旋转矩阵平移向量。

15、作为优选的，步骤s2)中，所述轨迹点姿态的更新涉及到将线加速度向量与重力向量之间的旋转转换为四元数表示的姿态；

16、首先，计算出线加速度向量在世界坐标系下的方向，然后使用从这个方向到重力方向的旋转来表示无人机的姿态；

17、假设重力方向向量为g＝(0,0,1)t，线加速度向量为aw，则轨迹点姿态的四元数qpitch_roll通过以下方式计算：

18、qpitch_roll＝quaterniond::fromtwovectors(g,aw)；

19、同时考虑偏航角(heading)，并进一步结合偏转角qheading的旋转：

20、qpitch_roll＝quaterniond(angleaxisd(heading,vector3d::unitz()))；

21、最终轨迹点的姿态为：qatt＝qpitch_roll·qheading。

22、作为优选的，步骤s2)中，根据线加速度向量aw和重力加速度g＝(0,0,1)t，计算集合推力collective_thrust：

23、collective_thrust＝||aw+g||；

24、通过在一小段时间内推力向量的变化，估算无人机的旋转速率；

25、假设推力在时间dt内的变化由jerkj决定，则推力向量的变化表示为：

26、

27、

28、式中，thrust表示上述计算得到的当前时刻的集合推力collective_thrust，thrust1和thrust2是假设推力向量在这段时间内因加速度的导数j的作用而改变，从而通过j估算出来的，经dt时间变化前后的两个推力向量thrust1、thrust2；

29、利用这两个推力向量thrust1、thrust2，计算出角速度angular_rates：

30、

31、式中，δt＝时间间隔dt。

32、作为优选的，步骤s2)中，通过姿态传感器realsense d435和t265自带imu获取无人机在机体坐标系下当前的位置和姿态信息。

33、作为优选的，步骤s3)中，所述的轨迹优化处理包括如下步骤：

34、s31)、对预设或动态生成的轨迹点进行多项式拟合，通过多项式拟合生成一条平滑的飞行路径，优化轨迹点的连续性；

35、s32)、根据多项式拟合的结果进行轨迹重新采样，生成等间隔或满足特定条件的轨迹点；

36、s33)、通过设置恒定的弧长速度，调整轨迹点的分布，以适应不同飞行阶段的速度需求，实现无人机飞行速度的平稳过渡和调整。

37、作为优选的，步骤s31)中，对轨迹点进行多项式拟合具体如下：

38、s311)、基于轨迹复杂度和计算资源选择合适的多项式阶数n，连续性阶数j，同时清空历史多项式系数，并检查轨迹的第一个点是否从时间零开始，然后定义缩放因子数组scaling来调整位置、速度、加速度和jerk的系数，确保多项式在不同物理量级上的适当比例和连续性；

39、s312)、假设拟合的轨迹点p(t)关于时间t可以用n阶多项式表示：

40、

41、其中，ai是多项式系数，p(t)为轨迹点，t为时间，ti指的是序列中的第i个时间点；

42、s313)、构建系数矩阵，对于每个轨迹点i,(i＞0)、时间为ti，在给定连续性阶数j的约束下，高阶系数aj的系数矩阵ai,j和常数项bi被构建为：

43、

44、

45、其中，paxis(ti)是在轨迹点i处对应的位置，为时间点ti的j次方、ak为多项式的系数，j为连续性阶数；

46、s314)、通过最小二乘法求解线性方程组计算出多项式的多项式系数ak，并通过缩放因子数组scaling进行缩放和连续性调整。

47、作为优选的，步骤s32)中，所述的轨迹重新采样具体如下：

48、s321)、根据多项式系数ak重新采样轨迹点；

49、s322)、更新每个轨迹点的位置、速度、加速度、急动，即：

50、位置：

51、速度：

52、加速度：

53、jerk：

54、式中，aj为高阶系数；tj为时间点t的j次方，tj-1为时间点t的j-1次方；tj-2为时间点t的j-2次方，tj-3为时间点t的j-3次方；

55、s323)、计算并更新集合推力和姿态

56、考虑无人机要克服的重力以维持当前加速度，将加速度向量与重力加速度向量相加，得到集合推力向量：

57、

58、式中，为z轴方向上的单位向量；

59、通过eigen::quaterniond::fromtwovectors方法，计算从惯性坐标系的z轴到推力向量所需的旋转，得到表示俯仰和滚转的四元数；

60、如果启用了偏航控制，根据速度向量的方向计算偏航角，并将其转换为四元数表示；将俯仰滚转四元数qpitch_roll与偏航四元数qheading相乘，得到最终的姿态四元数qatt：

61、qatt＝qpitch_roll·qheading；

62、s324)、计算角速度，推力向量取模长即为集合推力的值，进而对角速度近似计算，利用两个时刻推力向量的变化，来估计出未来的姿态变化率，即角速度angular_rates：

63、

64、式中，time_step为时间间隔；ε＝1×10-9，以避免除以零的情况。

65、作为优选的，步骤s33)中，通过设置恒定的弧长速度，在保持常速弧长条件下重新采样多项式轨迹，调整轨迹点的分布，保持物体沿着路径的速度恒定，以避免不自然的加速或减速，以适应不同飞行阶段的速度需求，实现无人机飞行速度的平稳过渡和调整，具体为：

66、s331)、保留多项式拟合轨迹的第一个点，计算总时间end_time，并将时间间隔均匀分为steps步；

67、s332)、轨迹从新采样

68、通过逐步增加时间tcurr，在每一步计算当前位置poscurr，然后再计算从上一个点位置到当前点位置posprev之间的弧长δl并累加；

69、δl＝||poscurr-posprev||；

70、当累加的弧长达到预定的间隔speed*traj_dt时，将当前点添加到新的点集newpoints中，其中，speed为预保持恒定的飞行速度，traj_dt为设置的时间步长；

71、如果新的点集newpoints的数量不足，则表示无法基于给定的速度和时间间隔生成足够的点；如果新的点集少于原始点集，使用常速度外推法增加额外的点；

72、然后使用新的点集newpoints重新拟合多项式系数，重新采样点，以符合新拟合的多项式轨迹。

73、作为优选的，步骤s4)中，具体包括以下步骤：

74、s41)、将步骤s3)中优化处理后的轨迹作为参考轨迹，并将参考轨迹转换为mpc算法可以使用的格式；

75、其中，参考轨迹表示为一系列的参考状态xref,k和参考控制输入uref,k向量，对应于每个预测时间步长k；参考状态xref,k包括位置、速度、加速度和姿态，参考控制输入uref,k包括推力和三个轴的角速度；

76、s42)、对于每个轨迹采样点计算期望的偏航姿态，基于轨迹点的偏航角θ，使用欧拉角到四元数的转换来计算,偏航姿态的四元数qheading表示为：

77、

78、s43)、将偏航姿态的四元数qheading与轨迹点给出的原始姿态四元数qprev结合，以计算最终的参考姿态四元数qfinal，最终设置参考状态和输入，根据时间步长和轨迹信息进行采样和存储，其中，参考姿态四元数qfinal表示为：

79、qfinal＝qprev·qheading；

80、s44)、利用mpc算法根据当前的状态估计和参考轨迹计算一系列的优化控制输入，以最小化轨迹跟踪误差，采用成本函数j表示，通过所述的成本函数j充分考虑状态误差和控制输入成本；最终得到控制序列uk，然后通过控制序列uk控制无人机沿预定轨迹飞行，即：

81、

82、给定无人机动力学模型为：

83、xk+1＝f(xk,uk)；

84、其中，xref,k是参考轨迹在时间步k的参考状态；xk+1是在时间步k+1的预测状态，xk是在时间步k的预测状态，uk是在时间步k的控制输入，q和r是权重矩阵，用于量化状态偏差和控制代价的重要性，在mpc中由设计者根据系统性能需求和优化目标设定；n是预测范围，即在未来考虑多少个时间步；xn表示在预测范围结束时的预测状态，xref,n表示在预测范围结束时的参考状态；t表示预测范围的总时间长度。

85、本发明的有益效果为：

86、1、本发明通过对轨迹点的优化处理，本发明能够在复杂环境下实现更平滑的飞行路径，有效避免了因环境变化大而导致的飞行不稳定问题；

87、2、本发明优化的轨迹点结合mpc算法，能够预测并适应即将到来的环境变化，减少不必要的飞行调整，降低能耗，同时提升飞行的安全性；

88、3、本发明与传统的直接航点控制相比，本发明能够动态优化飞行轨迹，不仅提高了飞行路径的效率，也减少了对环境的依赖，提升了无人机自主导航的独立性和可靠性。