一种基于复杂环境下无人机近障碍物的控制方法与流程

本发明属于无人机，具体涉及一种基于复杂环境下无人机近障碍物的控制方法。

背景技术：

1、无人机领域的研究早已不满足于简单的人为操控，而是朝着智能化方向不断前进；所谓智能化是指无人机能够获取自身运动和周围环境信息、处理信息、根据目标任务需求进行自主决策并独立执行的能力；无人机在室外区域飞行时，若处于gps信号遮蔽严重的区域，如信号塔、矿洞等环境，在这种复杂环境下，依靠gps定位近障碍物的方法就不再可靠甚至完全失效，故为了判断障碍物的距离为无人机的安全飞行和对障碍物近距离检测带来了很大的挑战；在识别障碍物上，目前已经有很多方法成功地应用基于机器视觉的障碍物识别中，如双目摄像头，激光测距、超声、毫米波雷达、多源融合以及基于计算机视觉的识别方法，这些基于视觉识别方法的优点体现在具有信息量丰富、探测范围较广等诸多优势；在接近障碍物的运动上，有传统的基于梯度的运动规划算法，该算法需要构建所需的欧几里德符号距离场地图，且构建欧几里德符号距离场地图的方式有两种，一种是全局增量式，另一种是批量本地计算；但这些都不是专门用于近障碍物运动而构建的；因此设计一种使用双目摄像头的近障碍物控制方法是具有关键的研究意义。

2、现有技术中，传统的基于梯度的运动规划算法需要构建所需的欧几里德符号距离场地图，然而构建地图花费了整个规划算法70％的时间，从而限制了在有限资源情况下的运动规划方法的使用；另外欧几里德符号距离场的构建方式有全局增量式和批量本地计算两种方式，若使用全局增量式来构建欧几里德符号距离场，尽管这方法在动态更新情况下非常有效，但生成的欧几里德符号距离场几乎总是包含可能根本不会在近障碍物控制过程中所使用的冗余信息；但是对于运动规划来说现有的两种方法构建出的欧几里德符号距离场地图是多余的、不必要的，而且简单地手动减小欧几里德符号距离场地图范围，缺乏理论依据，也包含不必要的计算；欧几里德符号距离场长期以来一直被用于从嘈杂的传感器数据构建对象，其中一种包络算法是将欧几里德符号距离场构造的时间复杂度降低到o(n)；其中n表示为体素数；该算法不适用于欧几里德符号距离场的增量构建，而四旋翼飞行过程中经常需要动态更新场；也使用了增量欧几里德符号距离场生成方法，尽管这些方法在动态更新情况下非常有效，但生成的欧几里德符号距离场几乎总是包含可能根本不会在近障碍物控制过程中所使用的距离场信息；导致欧几里德符号距离场需要大量时间构建，且利用极低；二是使用pid作为近障碍物的控制算法的话，若比例环节没调好产生震荡，则无法保证无人机的安全飞行；另外对于非线性的控制系统，控制精度也会下降，并且还会有稳态误差，无法保证无人机与障碍物的准确距离；pid算法一般不依赖于未来的误差；如果依赖未来的误差，也没有考虑无人机的模型，必然不准确的控制。

3、针对以上缺点，本发明提出一种基于复杂环境下近障碍物的无人机控制方法；使无人机能够平稳的在障碍物周围进行控制。

技术实现思路

1、本发明的目的是提供一种基于复杂环境下无人机近障碍物的控制方法，首先通过比较碰撞轨迹和无碰撞轨迹引导路径，得到惩罚函数中的碰撞项，再使用轨迹优化器提取当前轨迹撞到的障碍物的信息，并且判断轨迹的动力学可不可行，延长该轨迹分配的时间，最后通过异性曲线拟合算法在保持原有轨迹形状的情况下降低轨迹的阶数，由此得到可行轨迹的准确距离场；另外使用模型预测控制mpc算法来代替pid算法，处理获得的距离场信息及高度信息，并且添加约束，使无人机有期望的对近障碍物检测轨迹，同时又有加速和高度约束，防止无人机撞到障碍物，另外有类似前馈功能，以改善控制器的性能；使无人机能够平稳的在障碍物周围进行控制。

2、本发明采取的技术方案具体如下：

3、一种基于复杂环境下无人机近障碍物的控制方法，包括以下步骤：

4、步骤1：使用双目摄像头得到关于障碍物间的曲线；

5、步骤2：根据曲线来估计障碍物的碰撞力，得到{p,v}对；

6、步骤3：设计基于梯度的轨迹优化器提取当前轨迹撞到的障碍物的信息；

7、步骤4：然后进行数值优化；

8、步骤5：将得到的{p,v}对设计mpc控制器，并基于mpc控制器输出无人机近障碍物的最优控制轨迹。

9、优选地，所述步骤1中，通过获取障碍物体在两个确定位置关系的摄像机的左右图像中的坐标，输出两摄像机公共视场内障碍物体的三维尺寸及空间物体特征点的三维坐标，估计目标点离相机的深度距离；然后计算障碍物体上的多个空间点p，则将这些空间点p连接起来，则得到一条关于障碍物曲线。

10、优选地，所述步骤2中，定义b样条曲线的控制点q，生成一条满足终端约束但不考虑障碍物的b样条曲线φ；接着，对于每段被检测到的碰撞的线段，用优化程序生成一个无碰撞的路径γ；对于发生碰撞的线段的每个控制点都生成一个在障碍物表面的定位点，对应一个排斥方向向量，其中i为控制点的索引，j为{p,v}对的索引；每个{p,v}对对应一个特定的控制点；qi到第j个障碍物的距离为：dij＝(qi-pij)vij；单位向量v在第一次生成后就不会再次发生改变，进而dij的值是分正负的；迭代过程中反复生成{p,v}对，判断是否为新障碍物的标准是：若控制点qi处于障碍物中时，并且对于当前得到的所有障碍物j满足dij>0，则该障碍物为新发现的障碍物；从而计算影响轨迹的障碍物信息。

11、优选地，所述步骤3中，使用均匀b样条曲线φ来表示轨迹，根据无人机的微分平坦特性降低要规划的变量；优化问题被定义为：

12、

13、其中js为光滑项，jc为碰撞项，jd为动力可行项，λs，λc，λd为惩罚项的权值；

14、最小化高阶导数能够使得整段轨迹光滑；所以光滑项由加速度和加加速度的平方和构成；由于b样条曲线的凸包性质，则js为：

15、

16、碰撞项能够通过惩罚dij<sf安全界限的控制点，将控制点推离障碍物；并构造了一个二阶连续的惩罚函数jc，随着dij的减小jc的斜率就越小；构造的二阶连续的惩罚函数jc为：

17、

18、其中cij＝sf-dij；

19、对所有控制点的惩罚求和得到总的碰撞项为：

20、

21、并直接闭式计算jc对qi的导数：

22、

23、其中np为控制点qi的{p,v}j的个数；

24、以f()为每个维度的高阶导数构造惩罚函数为：

25、

26、

27、其中，ωv,ωa,ωj为每一项的权重，cr∈c∈{vi,ai,ji},a1,b1,c1,a2,b2,c2用来满足函数二阶连续性，cm为导数限制，cj为二次和三次函数的交界，λ<1-ε(ε＜＜1)为弹性系数，使得最终的结果满足约束。

28、优选地，所述步骤4中，首先构建目标函数：

29、

30、首先计算超过限制的比例：

31、

32、其中i∈{1,...,nc-1},j∈{1,...,nc-2},k∈{1,...,nc-3},r∈{x,y,z}；re表明相对于φs，φf需要多分配多少时间；vi，aj和jk分别与δt的一次、二次和三次成反比；则φf的新时间间隔为：

33、δt'＝reδt。

34、优选地，所述步骤4中，通过求解一个闭式的最小二乘问题，在约束下初始生成时间跨度为δt'的轨迹φf，同时保持与φs相同的形状和控制点数；然后重新计算光滑项和可行项得到新的目标函数：

35、

36、其中λf是拟合项的权重(φs和φf的拟合程度)；

37、js被定义为从φf(αt')到φs(αt)各向异性位移的积分，其中t和t'为轨迹φs和φf的时长，a∈[0,1]；由于拟合的曲线φs已经无碰撞，对于两条曲线，用带有低权重的轴向位移来放宽光滑调整限制，用高权重的径向位移来防止碰撞。

38、优选地，所述步骤4中，由于φf(αt')的椭圆体是一个以φs(αt)为中心的椭圆，绕它的主

39、轴或切线旋转一定角度得到的；则轴向位移da和径向位移dr为：

40、

41、则拟合项为：

42、

43、其中a和b分别为椭圆的半长径和半短径，径向位移对应的半短径b使径向位移的惩罚权重增大以防止碰撞；最终完成数据优化得到了可行防碰撞轨迹及其距离场{p,v}对。

44、优选地，所述步骤5中，设定无人机状态量x＝[{p,v},h,v]t，控制量其中β,α是相互耦合的关系；

45、整个系统表示为f(x,u)，采用下标r代表轨迹参考量，则参考系统为f(xr,ur)；将f(x,u)在参考点进行泰勒展开，忽略高阶项，得：

46、

47、减去xr，得：

48、

49、其中为无人机实际状态与参考系统状态的偏差；xr＝[{p,v}r,hr,vr]t；为无人机实际控制量与参考系统输入控制量的偏差；a，b均为状态矩阵；

50、使用近似离散化方法对上式进行离散化，得：

51、

52、式中：ak和bk均为离散化后的线性时变矩阵；i为单位矩阵；t为离散时间步长控制周期；k为任意次采样次数；分别为第k和第k+1次采样时无人机实际位置和期望位置的差值；为第k次采样时无人机实际的俯仰角、横滚角和偏航角与期望的俯仰角、横滚角和偏航角的差值；ck为单位矩阵；为离散系统输出，即其中y(k)为实际输出轨迹，yr(k)为参考轨迹。

53、优选地，所述步骤5中，采用增量式状态空间模型，亦即将作为控制输入的增量，以消除因外部干扰以及模型不精确带来的稳态误差；为了便于添加约束，定义新的系统状态变量(因第k次采样时采用了上一时刻的控制量计算结果而状态量由于使用了传感器采集数据，因此其为实时状态)，故新的状态空间模型为：

54、

55、其中：

56、

57、假设在预测时域内，风速和风向不发生变化，为简化运算，假定如下：

58、

59、则预测输出为：

60、p(k)＝ψkξ(k)+θkh(k)

61、其中：

62、

63、式中：p(k)为第k次采样时在预测时域np内的预测输出；h(k)为第k次采样时在控制时域nc内的预测输入增量，通常情况下nc≦np；ψk和θk均为系统矩阵；为第k次采样时预测第k+np次采样时的系统输出值；δu((k+nc-1)|k)为第k次采样时预测第k+nc-1次采样时的输入增量值，其他参数同理；

64、优选地，所述步骤5中，以无人帆船状态量偏差及其控制量增量来构建目标函数j(k)：

65、

66、式中：q和r为权重矩阵，根据控制需求予以更改；体现了无人机在mpc控制器作用下对近障碍物的跟踪效果，体现了无人机运动是否平稳，即在满足跟踪任务的条件下尽量保证相对较小的控制增量整理并去掉与h(k)无关的项，即可得到标准二次型目标函数j(h(k))：

67、

68、式中：mk＝2(θktqeθk+re)，gk＝2θktqe[ψkξ(k)]，其中且权重矩阵

69、在第k次采样时的预测时域np内，约束条件可以表示为：

70、δumin≦h(k)≦δumax

71、umin≦wh(k)+u(k-1)≦umax

72、其中

73、

74、式中：δumin，δumax均表示输入增量约束；umin，umax均为输入约束；w为下三角矩阵；u(k-1)为上一时刻的俯仰角、横滚角和偏航角的控制量；最终基于mpc的无人机近障碍物的控制轨迹可以描述为最优值问题：

75、

76、δumin≦h(k)≦δumax

77、umin≦wh(k)+u(k-1)≦umax

78、在每一个控制周期内对上式进行二次规划(qp)求解，即可得到控制时域nc内的输入增量，将作为第k+1次采样时无人机的俯仰角、横滚角和偏航角输入，进入下一个控制周期后，循环上述优化过程，直至完成近障碍物轨迹跟踪过程。

79、本发明取得的技术效果为：

80、1、本发明中首先通过比较碰撞轨迹和无碰撞轨迹引导路径，得到惩罚函数中的碰撞项，再使用轨迹优化器提取当前轨迹撞到的障碍物的信息，并且判断轨迹的动力学可不可行，延长该轨迹分配的时间，最后通过异性曲线拟合算法在保持原有轨迹形状的情况下降低轨迹的阶数，由此得到可行轨迹的准确距离场；另外使用模型预测控制mpc算法来代替pid算法，处理获得的距离场信息及高度信息，并且添加约束，使无人机有期望的对近障碍物检测轨迹，同时又有加速和高度约束，防止无人机撞到障碍物，另外有类似前馈功能，以改善控制器的性能；使无人机能够平稳的在障碍物周围进行控制。

81、2、本发明不需要欧几里德符号距离场地图，将双目摄像头获得的碰撞轨迹和无碰撞轨迹引导路径进行比较，得到惩罚函数中的碰撞项，再使用轨迹优化器提取当前轨迹撞到的障碍物的信息，并且判断轨迹的动力学可不可行，延长该轨迹分配的时间，最后通过异性曲线拟合算法在保持原有轨迹形状的情况下降低轨迹的阶数，由此得到可行轨迹的准确距离场；该方法使获得的距离场是来自可行轨迹，并且数值准确，将所需要的距离场信息更高效的获得；

82、3、本发明采用mpc算法来对获取到距离场信息、高度信息、速度信息以及加速度信息来预测无人机在未来某一时间段内的表现来进行优化控制，并且对信息进行约束，保证无人机可以安全飞行并对障碍物近距离检测。

83、4、本发明将从可行轨迹中获得的距离场信息与mpc算法结合，使其成为一种基于复杂环境下无人机近障碍物的控制方法，所设计的控制方法与普遍使用的基于pid的控制方法具有更小的超调、更小的稳态误差、轨迹预测及局部最优。

84、5、本发明提出无需欧几里德符号距离场地图获得可行轨迹距离场的方法；通过碰撞轨迹的比较，来获得具有惩罚函数的碰撞项，减少了对根本不会用到在近障碍物控制过程中所使用的冗余信息的计算；通过异性曲线拟合算法在保持原有轨迹形状的情况下降低轨迹的阶数，降低了计算的复杂度，由此得到可行轨迹的准确距离场。

85、6、本发明提出将获得到的距离场信息、高度等信息来预测无人机在未来某一时间段内的表现来进行优化控制，并且对距离场、高度以及加速度进行约束，保证无人机在近障碍物的控制中可以安全飞行、很小的超调、很小的稳态误差以及无人机的轨迹预测。