CN109839956B - 一种无人机的路径规划方法及装置 - Google Patents
一种无人机的路径规划方法及装置 Download PDFInfo
- Publication number
- CN109839956B CN109839956B CN201910160307.0A CN201910160307A CN109839956B CN 109839956 B CN109839956 B CN 109839956B CN 201910160307 A CN201910160307 A CN 201910160307A CN 109839956 B CN109839956 B CN 109839956B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- intention
- force field
- distance
- 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
Links
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明实施例提供的一种无人机的路径规划方法及装置,方法包括:通过获取t时刻无人机的信息,确定t时刻靠近目标点意图的意图力场;确定t时刻减少径向抵近第i个障碍物意图的意图力场;确定t时刻绕开前方障碍物意图的意图力场;将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;在t+1时刻,基于不为0的所述混合意图力场的方向,进行飞行。因此,本发明实施例避免对无人机的路径规划的方法容易陷入局部最小值的情况。
Description
技术领域
本发明涉及人工智能技术领域,特别是涉及一种无人机的路径规划方法及装置。
背景技术
随着人工智能技术的高速发展,无人机已经在地图测绘、灾害监测、农药喷洒、物流运输等领域获得广泛应用。鉴于无人机的周围可能存在地形、建筑、其他障碍物,这些障碍物对无人机的飞行安全造成严重威胁,为保障无人机的作业安全,对无人机行驶路径进行规划显得尤为重要。
现有技术,无人机行驶过程中利用自身传感器,感知探测周边一定范围内的障碍物信息,采用人工势场法对无人机的路径进行规划。下面详细介绍使用人工视场法对无人机的路径进行规划的过程。
人工势场法中通过将目标点产生对无人机的引力和障碍物对无人机产生的斥力进行求和,将无人机的行驶方向设置为引力和斥力的合力方向,以此来控制无人机的运行,并生成路径规划曲线。
如果障碍物对无人机的斥力与目标点对无人机的引力方向相反,大小相等,此时斥力与引力的和为0,导致无人机不知向那个方向行驶,因此无人机可能暂停不动,这种情况下对无人机的路径规划陷入局部最小值。因此,现有技术对无人机的路径规划的方法容易陷入局部最小值。
发明内容
本发明实施例的目的在于提供一种无人机的路径规划方法及装置,通过调整无人机的飞行角度,避免无人机的路径规划陷入局部最小值。具体技术方案如下:
第一方面,本发明实施例提供的一种无人机的路径规划方法包括:
获取t时刻无人机的信息,所述无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
确定t时刻靠近目标点意图的意图力场,靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将目标点的位置与无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在第一距离大于第一门限参数的情况下,将第一比值、第一门限参数与第一尺度因子相乘得到的;第一比值为:目标点的位置与无人机在二维平面的位置的距离,与第一距离的比值;第一距离为:目标点的位置与无人机在二维平面的位置的欧几里得距离;
确定t时刻减少径向抵近第i个障碍物意图的意图力场;减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在第二距离不小于第二门限参数的情况下,将减少径向抵近i个障碍物意图的意图力场作为0得到的;第二比值为:无人机在二维平面的位置与第i个障碍物的位置之间的差值,与第二距离的比值;第二距离为:无人机在二维平面的位置与第i个障碍物的位置的欧几里得距离;距离差值为:第二距离的倒数与第二门限参数的倒数相减的结果,与第二距离的二次方的倒数相乘的结果;
确定t时刻绕开前方障碍物意图的意图力场;绕开前方障碍物意图的意图力场是通过在第三距离小于第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在第三距离不小于第二门限参数的情况下,将绕开前方障碍物意图的意图力场作为0得到的,第三距离为:无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离;
将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。
可选的,将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场,包括:
使用矢量求和公式,对t时刻靠近目标点意图的意图力场、减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为无人机的混合意图力场;
其中,F1(p0(t))是t时刻靠近目标点意图的意图力场,是t时刻减少径向抵近i个障碍物意图的意图力场,是t时刻绕开前方障碍物意图的意图力场,p0(t)是t时刻无人机在二维平面的位置,i是障碍物的序号,N(t)是t时刻障碍物的总数;m是t时刻无人机飞行方向上最近的障碍物的序号。
可选的,在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0,包括:
在t时刻无人机的混合意图力场为0的情况下,通过如下角度公式调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
角度公式为:其中,θm1(t)为:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的左侧切线的夹角,θm2(t)是:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的右侧切线的夹角,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值;θm(t)是时刻无人机至无人机飞行方向上第m个最近的障碍物中心的连线、与无人机至无人机飞行方向上第m个最近的障碍物方向的切线的夹角,角度旋转矩阵是:p0(t)是t时刻无人机在二维平面的位置,m是t时刻无人机飞行方向上最近的障碍物的序号。
本发明第一方面实施例提供一种无人机的路径规划方法还包括:
在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值不高于角度阈值的情况下,通过公式:p0(t+T)=p0(t+T-1)+δl[cos(θ(F(p0(t+T-1)))),sin(θ(F(p0(t+T-1))))]T计算无人机t+T时刻的位置;其中,将δl是预设的迭代步长,p0(t+T-1)是t+T-1时刻无人机在二维平面的位置,θ(F(p0(t+T-1))是t+T-1时刻混合意图力场的方向;p0(t+T)是t+T时刻无人机在二维平面的位置;
在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值高于角度阈值的情况下,通过公式:θ0(t+T)=θ0(t+T-1)+sgn(θ(F(p0(t+T-1)))-θ0(t+T-1))×δθ,p0(t+T)=p0(t+T-1)+δl[cos(θ0(t+T-1)),sin(θ0(t+T-1))]T,计算无人机t+T时刻的位置;
其中,sgn(·)为符号函数,θ0(t+T)是t+T时刻无人机的飞行方向,θ0(t+T-1)是t+T-1时刻无人机的飞行方向,t+T时刻无人机在二维平面的位置与目标点的位置之间的距离小于无人机的最小转弯半径,T为不小于1的正整数。
可选的,在计算无人机t+T时刻的位置的步骤之后,本发明第一方面实施例提供一种无人机的路径规划方法还包括:
初始化航线点;航线点包括:航线点水平位置及航线点垂直位置,初始化航线点是令第t个航线点位置等于t时刻无人机在二维平面的位置,且s=1;
判断t+j个航线点的位置是否是最后一个航线点位置,如果是,则令s=s+1,Q(s)=q0(t+j);
如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率;
基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离;
如果t+j个航线点处的切线距离小于切线距离阈值,则重复如果t+j个航线点的位置不是最后一个航线点位置,则j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离的过程;
如果t+j个航线点处的切线距离不小于切线距离阈值,则令航路点序号s=s+1,Q(s)等于q0(t+j-1),输出Q(s);
其中,s是航路点序号,Q(s)是航路点序列;q0(t)是第t个航线点;j取正整数,斜率计算公式是:kq是第t个航线点处的斜率,yq0(t+j)是t+j个航线点垂直位置,yq0(t)是t个航线点垂直位置,xq0(t+j)是t+j个航线点水平位置,xq0(t)是t个航线点水平位置,第t个航线点处的切线方程是:yq0(t)=kqxq0(t)+bq,bq=yq0(t)-kqxq0(t),dq(t+j)是t+j个航线点处的切线距离,角标q代表航线点参数,角标q0代表航线点坐标位置参数。
可选的,切线距离阈值可以通过以下步骤得到:
将第t个航线点与第t+j个航线点之间距离的绝对值与sin(δθ)的乘积,确定为切线距离阈值,其中,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值。
第二方面,本发明实施例提供的一种无人机的路径规划装置,包括:
信息获取模块,用于获取t时刻无人机的信息,无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
第一确定模块,用于确定t时刻靠近目标点意图的意图力场;靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将目标点的位置与无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在第一距离大于第一门限参数的情况下,将第一比值、第一门限参数与第一尺度因子相乘得到的;第一比值为:目标点的位置与无人机在二维平面的位置的距离,与第一距离的比值;第一距离为:目标点的位置与无人机在二维平面的位置的欧几里得距离;
第二确定模块,用于确定t时刻减少径向抵近第i个障碍物意图的意图力场;减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在第二距离不小于第二门限参数的情况下,将减少径向抵近i个障碍物意图的意图力场作为0得到的;第二比值为:无人机在二维平面的位置与第i个障碍物的位置之间的差值,与第二距离的比值;第二距离为:无人机在二维平面的位置与第i个障碍物的位置的欧几里得距离;距离差值为:第二距离的倒数与第二门限参数的倒数相减的结果,与第二距离的二次方的倒数相乘的结果;
第三确定模块,用于确定t时刻绕开前方障碍物意图的意图力场;绕开前方障碍物意图的意图力场是通过在第三距离小于第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在第三距离不小于第二门限参数的情况下,将绕开前方障碍物意图的意图力场作为0得到的,第三距离为:无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离;
力场求和模块,用于将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
角度调节模块,用于在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
飞行确定模块,用于在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。
可选的,力场求和模块具体用于:
使用矢量求和公式,对t时刻靠近目标点意图的意图力场、减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为无人机的混合意图力场;
其中,F1(p0(t))是t时刻靠近目标点意图的意图力场,是t时刻减少径向抵近i个障碍物意图的意图力场,是t时刻绕开前方障碍物意图的意图力场,p0(t)是t时刻无人机在二维平面的位置,i是障碍物的序号,N(t)是t时刻障碍物的总数;m是t时刻无人机飞行方向上最近的障碍物的序号。
可选的,角度调整模块,具体用于:
在t时刻无人机的混合意图力场为0的情况下,通过如下角度公式调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
角度公式为:其中,θm1(t)为:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的左侧切线的夹角,θm2(t)是:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的右侧切线的夹角,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值;θm(t)是时刻无人机至无人机飞行方向上第m个最近的障碍物中心的连线、与无人机至无人机飞行方向上第m个最近的障碍物方向的切线的夹角,角度旋转矩阵是:p0(t)是t时刻无人机在二维平面的位置,m是t时刻无人机飞行方向上最近的障碍物的序号,角标m1中的1和角标m2中的2是为了加以区分无人机至无人机飞行方向上最近的障碍物方向的左侧和右侧切线的夹角。
可选的,本发明第二方面实施例提供的一种无人机的路径规划的装置还包括:
方向调整模块,用于在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值不高于角度阈值的情况下,通过公式:p0(t+T)=p0(t+T-1)+δl[cos(θ(F(p0(t+T-1)))),sin(θ(F(p0(t+T-1))))]T计算无人机t+T时刻的位置;其中,将δl是预设的迭代步长,p0(t+T-1)是t+T-1时刻无人机在二维平面的位置,θ(F(p0(t+T-1))是t+T-1时刻混合意图力场的方向;p0(t+T)是t+T时刻无人机在二维平面的位置;
在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值高于角度阈值的情况下,通过公式:θ0(t+T)=θ0(t+T-1)+sgn(θ(F(p0(t+T-1)))-θ0(t+T-1))×δθ,p0(t+T)=p0(t+T-1)+δl[cos(θ0(t+T-1)),sin(θ0(t+T-1))]T,计算无人机t+T时刻的位置;
其中,sgn(·)为符号函数,θ0(t+T)是t+T时刻无人机的飞行方向,θ0(t+T-1)是t+T-1时刻无人机的飞行方向,t+T时刻无人机在二维平面的位置与目标点的位置之间的距离小于无人机的最小转弯半径,T为不小于1的正整数。
可选的,本发明第二方面实施例提供的一种无人机的路径规划的装置还包括:
航线规划模块,用于初始化航线点;航线点包括:航线点水平位置及航线点垂直位置,初始化航线点是令第t个航线点位置等于t时刻无人机在二维平面的位置,且s=1;
判断t+j个航线点的位置是否是最后一个航线点位置,如果是,则令s=s+1,Q(s)=q0(t+j);
如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率;
基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离;
如果t+j个航线点处的切线距离小于切线距离阈值,则重复如果t+j个航线点的位置不是最后一个航线点位置,则j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离的过程;
如果t+j个航线点处的切线距离不小于切线距离阈值,则令航路点序号s=s+1,Q(s)等于q0(t+j-1),输出Q(s);
其中,s是航路点序号,Q(s)是航路点序列;q0(t)是第t个航线点;j取正整数,斜率计算公式是:kq是第t个航线点处的斜率,yq0(t+j)是t+j个航线点垂直位置,yq0(t)是t个航线点垂直位置,xq0(t+j)是t+j个航线点水平位置,xq0(t)是t个航线点水平位置,第t个航线点处的切线方程是:yq0(t)=kqxq0(t)+bq,bq=yq0(t)-kqxq0(t),dq(t+j)是t+j个航线点处的切线距离,角标q代表航线点参数,角标q0代表航线点坐标位置参数。
可选的,本发明第二方面实施例提供的一种无人机的路径规划的装置还包括:
阈值确定模块,用于:将第t个航线点与第t+j个航线点之间距离的绝对值与sin(δθ)的乘积,确定为切线距离阈值,其中,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值。
本发明实施例提供的一种无人机的路径规划方法及装置,通过获取t时刻无人机的信息,确定t时刻靠近目标点意图的意图力场;确定t时刻减少径向抵近第i个障碍物意图的意图力场;确定t时刻绕开前方障碍物意图的意图力场;将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。避免障碍物对无人机的斥力与目标点对无人机的引力方向相反,大小相等,此时斥力与引力的和为0,对无人机的路径规划的方法容易陷入局部最小值的情况。当然,实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1为本发明实施例提供的一种无人机的路径规划方法的流程图;
图2为本发明实施例提供的无人机的飞行示意图;
图3为本发明实施例提供的无人机调整飞行方向的示意图;
图4为本发明实施例提供的无人机自主规划航线的示意图;
图5为本发明实施例提供的一种无人机的路径规划的装置结构图;
图6为本发明实施例提供的一种电子设备的结构图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整的描述,显然,所描述的实施例仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
首先,对本发明实施例提供的一种无人机的路径规划方法作介绍。
如图1所示,本发明实施例提供的一种无人机的路径规划方法,包括:
S101,获取t时刻无人机的信息;
其中,无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
S102,确定t时刻靠近目标点意图的意图力场;
其中,靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将目标点的位置与无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在第一距离大于第一门限参数的情况下,将第一比值、第一门限参数与第一尺度因子相乘得到的;第一比值为:目标点的位置与无人机在二维平面的位置的距离,与第一距离的比值;第一距离为:目标点的位置与无人机在二维平面的位置的欧几里得距离。意图力场指无人机根据航行的飞行意图信息而产生的引导无人机航行的虚拟力场,飞行意图信息包括:避开障碍物保障飞行安全、向目标点航行完成飞行任务等等。
其中,F1(p0(t))是靠近目标点意图的意图力场,k1是第一尺度因子,k1∈R+,d1是第一门限参数,d1∈R+,||·||表示欧几里得距离,第一距离是||pr-p0(t)||,第一比值是pr是目标点位置,pr=[xr,yr]T,xr是目标点水平位置,yr是目标点垂直位置,角标r表示目标点。p0(t)是t时刻无人机在二维平面的位置,p0(t)=[x0(t),y0(t)]T,x0(t)是t时刻无人机的横坐标位置,y0(t)是t时刻无人机的纵坐标位置,p0(t)中的角标0代表无人机,F1(p0(t))中F的角标1是为了与其他的意图力场区分。
参考图2,由上述实施方式可知,当无人机与目标点的距离不大于d1时,靠近目标点意图I1的意图力场大小为k1||pr-p0(t)||,方向由无人机指向目标点;当无人机与目标点的距离大于d1时,靠近目标点意图I1的力场大小为k1d1,方向由无人机指向目标点。由此可见,当无人机距离目标点越远,靠近目标点的意图力场越大。在本实施方式中,k1可以为但是不限于设置为1,d1可以设置为但是不限于设置为100。
S102,确定t时刻减少径向抵近第i个障碍物意图的意图力场;
其中,减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在第二距离不小于第二门限参数的情况下,将减少径向抵近i个障碍物意图的意图力场作为0得到的;第二比值为:无人机在二维平面的位置与第i个障碍物的位置之间的差值,与第二距离的比值;第二距离为:无人机在二维平面的位置与第i个障碍物的位置的欧几里得距离;距离差值为:第二距离的倒数与第二门限参数的倒数相减的结果,与第二距离的二次方的倒数相乘的结果。
其中,是抵近第i个障碍物意图的意图力场,k2是第二尺度因子,Li(t)是第i个障碍物占据的水平面积,Li(t)∈R+,i∈[1,N(t)],N(t)是t时刻障碍物的个数,di0是无人机与第i个障碍物之间的距离,di0=||p0(t)-pi(t)||,d2是第二门限参数,d2∈R+,di0的角标i0表示无人机到第i个障碍物,距离差值是:第二比值是:i是障碍物序号。
参考图2,当无人机与障碍物的距离不小于d2时,减少径向抵近障碍物意图I2的意图力场大小为0,当无人机与障碍物的距离小于d2时,减少径向抵近障碍物意图I2的意图力场大小和di0成反相关,其方向由障碍物指向无人机。在本实施方式中,忽略无人机飞行过程中远距离障碍物对无人机的影响,k2可以为但是不限于设置为1,d2可以设置为但是不限于设置为1000,远距离障碍物可以是距离无人机1000米以外的障碍物。
S104,确定t时刻绕开前方障碍物意图的意图力场;
其中,绕开前方障碍物意图的意图力场是通过在第三距离小于第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在第三距离不小于第二门限参数的情况下,将绕开前方障碍物意图的意图力场作为0得到的,第三距离为:无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离;
其中,是绕开前方障碍物意图的意图力场,k3是第三尺度因子,Lm(t)是无人机飞行方向上第m个最近的障碍物占据的水平面积,Lm(t)∈R+,m∈[1,N(t)],C(p0(t),θm(t))是角度旋转矩阵,pm(t)是无人机飞行方向上第m个最近的障碍物的位置,pm(t)=[xm(t),ym(t)]T,xm(t)是t时刻无人机飞行方向上第m个最近的障碍物的横坐标位置,ym(t)是t时刻无人机飞行方向上第m个最近的障碍物的纵坐标位置,dm0是无人机与无人机飞行方向上第m个最近的障碍物之间的距离,是第三比值,dm0的角标m0表示无人机到无人机飞行方向上第m个最近的障碍物,m是无人机飞行方向上最近的障碍物序号,θm(t)是无人机到无人机飞行方向上第m个最近的障碍物连线方向,和无人机与无人机飞行方向上第m个最近的障碍物相切方向的夹角。
参考图3,若|θm1(t)|大于|θm2(t)|,θm(t)=θm2(t);若|θm1(t)|不大于|θm2(t)|,θm(t)=θm1(t),θm1(t)为:t时刻无人机至无人机飞行方向上第m个最近的障碍物中心的连线、与无人机至无人机飞行方向上第m个最近的障碍物方向的左侧切线的夹角,θm2(t)是:t时刻无人机至无人机飞行方向上第m个最近的障碍物中心的连线、与无人机至无人机飞行方向上第m个最近的障碍物方向的右侧切线的夹角。
当靠近目标点意图I1的意图力场和减少径向抵近障碍物意图I2的意图力场相互平衡时,可能存在局部最小值问题。从获得绕开前方障碍物意图的意图力场I3可以看出,当意图I1的意图力场和意图I2的意图力场趋于平衡时,k3的值趋于1,此时无人机以绕开前方障碍物的意图I3进行飞行为主。通过获得的公式可以求解出从而避免现有技术中路径规划陷入局部极小值问题,且当障碍物越大、距离障碍物距离越近时,绕开前方障碍物意图的意图力场越大。
S105,将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
为了避免对无人机的路径规划的方法容易陷入局部最小值,上述S105可以使用矢量求和公式,对t时刻靠近目标点意图的意图力场、减少径向抵近第i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为无人机的混合意图力场。
其中,F1(p0(t))是t时刻靠近目标点意图的意图力场,是t时刻减少径向抵近第i个障碍物意图的意图力场,是t时刻绕开前方障碍物意图的意图力场,p0(t)是t时刻无人机在二维平面的位置,i是障碍物的序号,N(t)是t时刻障碍物的总数;m是t时刻无人机飞行方向上最近的障碍物的序号。
S106,在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
为了避免对无人机的路径规划的方法容易陷入局部最小值,上述S106在t时刻无人机的混合意图力场为0的情况下,可以通过如下角度公式调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0。
参考图3,如果|θm1(t)|≤|θm2(t)|,则将角度旋转矩阵中的θm(t)确定θm1(t),θm(t)的大小变更为θm(t)+δθ。如果,|θm1(t)|>|θm2(t)|,则将角度旋转矩阵中的θm(t)确定θm2(t),θm(t)的大小变更为θm(t)-δθ,以便无人机可以绕开飞行方向上第m个最近的障碍物,朝着目标点前进,避免无人机原地不动,从而无人机的路径规划陷入局部最小值。
S107,在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。
本发明实施例通过获取t时刻无人机的信息,确定t时刻靠近目标点意图的意图力场;确定t时刻减少径向抵近第i个障碍物意图的意图力场;确定t时刻绕开前方障碍物意图的意图力场;将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近第i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。避免障碍物对无人机的斥力与目标点对无人机的引力方向相反,大小相等,此时斥力与引力的和为0,对无人机的路径规划的方法容易陷入局部最小值的情况。
在上述S107的步骤之后,本发明实施例提供的一种无人机的路径规划方法还包括:
步骤一:在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值不高于角度阈值的情况下,通过公式:p0(t+T)=p0(t+T-1)+δl[cos(θ(F(p0(t+T-1)))),sin(θ(F(p0(t+T-1))))]T计算无人机t+T时刻的位置;
其中,将δl是预设的迭代步长,p0(t+T-1)是t+T-1时刻无人机在二维平面的位置,θ(F(p0(t+T-1))是t+T-1时刻混合意图力场的方向;p0(t+T)是t+T时刻无人机在二维平面的位置;
步骤二:在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值高于角度阈值的情况下,通过公式:θ0(t+T)=θ0(t+T-1)+sgn(θ(F(p0(t+T-1)))-θ0(t+T-1))×δθ,p0(t+T)=p0(t+T-1)+δl[cos(θ0(t+T-1)),sin(θ0(t+T-1))]T,计算无人机t+T时刻的位置。
其中,sgn(·)为符号函数,θ0(t+T)是t+T时刻无人机的飞行方向,θ0(t+T-1)是t+T-1时刻无人机的飞行方向,t+T时刻无人机在二维平面的位置与目标点的位置之间的距离小于无人机的最小转弯半径,T为不小于1的正整数。
本发明实施例中T从1至T+1依次取值,通过上述步骤一及步骤二可以获得无人机t至t+T时刻的二维水平位置p0(t),p0(t+1),…,p0(t+T)。
在上述获得无人机t至t+T时刻的二维水平位置p0(t),p0(t+1),…,p0(t+T)的步骤之后,本发明实施例提供的一种无人机的路径规划方法还包括:
步骤一:初始化航线点;航线点包括:航线点水平位置及航线点垂直位置,初始化航线点是令第t个航线点位置等于t时刻无人机的水平位置无人机在二维平面的位置,且s=1;
步骤二:判断t+j个航线点的位置是否是最后一个航线点位置,如果是,则令s=s+1,Q(s)=q0(t+j);
步骤三:如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1;利用斜率计算公式计算在第t个航线点处航线切线的斜率;
步骤四:基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离;
步骤五:如果t+j个航线点处的切线距离小于切线距离阈值,则重复如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离的过程;
在一种可能的实施方式中,利用切线距离公式,将第t个航线点与第t+j个航线点之间距离的绝对值与sin(δθ)的乘积,确定为切线距离阈值。
其中,切线距离公式是sin(δθ)||q0(t+j)-q0(t)||,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值。
步骤六:如果t+j个航线点处的切线距离不小于切线距离阈值,则令航路点序号s=s+1,Q(s)等于q0(t+j-1),输出Q(s);
其中,s是航路点序号,Q(s)是航路点序列;q0(t)是第t个航线点;j取正整数,斜率计算公式是:kq是第t个航线点处的斜率,yq0(t+j)是t+j个航线点垂直位置,yq0(t)是t个航线点垂直位置,xq0(t+j)是t+j个航线点水平位置,xq0(t)是t个航线点水平位置,第t个航线点处的切线方程是:yq0(t)=kqxq0(t)+bq,bq=yq0(t)-kqxq0(t),最后一个航线点的位置是p0(t+T),dq(t+j)是t+j个航线点处的切线距离,角标q代表航线点,角标q0代表无人机的航线点。
参考图4,初始化航线点,令q0(t)=p0(t)。假设j取值为1,判断q0(t+1)位置是不是最后一个航线点的位置p0(t+T),如果是,则将Q(s)输出为q0(t+1),s=s+1。如果q0(t+1)的位置不是最后一个航线点位置,则将q0(t+1)及q0(t)输入斜率计算公式及切线方程,计算q0(t)处的斜率kq,计算q0(t+1)处的切线距离dq(t+j),如果q0(t+1)处的切线距离dq(t+j)不小于切线距离阈值sin(δθ)||q0(t+j)-q0(t)||,则s=s+1,Q(s)等于q0(t+j-1),输出Q(s),如果q0(t+1)处的切线距离dq(t+j)小于切线距离阈值sin(δθ)||q0(t+j)-q0(t)||,则重复如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离的过程,直至所有航线点判断完成。
本实施方式通过计算输出航路点序列Q(s),无人机按照航路点序列经过的航线点进行飞行,无人机可以有效地进行自主路径规划。
下面继续对本发明实施例提供的一种无人机的路径规划装置作介绍。
如图5所示,本发明实施例提供的一种无人机的路径规划装置,包括:
信息获取模块501,用于获取t时刻无人机的信息,无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
第一确定模块502,用于确定t时刻靠近目标点意图的意图力场;靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将目标点的位置与无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在第一距离大于第一门限参数的情况下,将第一比值、第一门限参数与第一尺度因子相乘得到的;第一比值为:目标点的位置与无人机在二维平面的位置的距离,与第一距离的比值;第一距离为:目标点的位置与无人机在二维平面的位置的欧几里得距离;
第二确定模块503,用于确定t时刻减少径向抵近第i个障碍物意图的意图力场;减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在第二距离不小于第二门限参数的情况下,将减少径向抵近i个障碍物意图的意图力场作为0得到的;第二比值为:无人机在二维平面的位置与第i个障碍物的位置之间的差值,与第二距离的比值;第二距离为:无人机在二维平面的位置与第i个障碍物的位置的欧几里得距离;距离差值为:第二距离的倒数与第二门限参数的倒数相减的结果,与第二距离的二次方的倒数相乘的结果;
第三确定模块504,用于确定t时刻绕开前方障碍物意图的意图力场;绕开前方障碍物意图的意图力场是通过在第三距离小于第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在第三距离不小于第二门限参数的情况下,将绕开前方障碍物意图的意图力场作为0得到的,第三距离为:无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离;
力场求和模块505,用于将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
角度调节模块506,用于在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
飞行确定模块507,用于在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。
可选的,力场求和模块具体用于:
使用矢量求和公式,对t时刻靠近目标点意图的意图力场、减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为无人机的混合意图力场;
其中,F1(p0(t))是t时刻靠近目标点意图的意图力场,是t时刻减少径向抵近i个障碍物意图的意图力场,是t时刻绕开前方障碍物意图的意图力场,p0(t)是t时刻无人机在二维平面的位置,i是障碍物的序号,N(t)是t时刻障碍物的总数;m是t时刻无人机飞行方向上最近的障碍物的序号。
可选的,角度调整模块,具体用于:
在t时刻无人机的混合意图力场为0的情况下,通过如下角度公式调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
角度公式为:其中,θm1(t)为:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的左侧切线的夹角,θm2(t)是:t时刻无人机至无人机飞行方向上最近的障碍物中心的连线、与无人机至无人机飞行方向上最近的障碍物方向的右侧切线的夹角,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值;θm(t)是时刻无人机至无人机飞行方向上第m个最近的障碍物中心的连线、与无人机至无人机飞行方向上第m个最近的障碍物方向的切线的夹角,角度旋转矩阵是:p0(t)是t时刻无人机在二维平面的位置,m是t时刻无人机飞行方向上最近的障碍物的序号,角标m1中的1和角标m2中的2是为了加以区分无人机至无人机飞行方向上最近的障碍物方向的左侧和右侧切线的夹角。
可选的,本发明实施例提供的一种无人机的路径规划的装置还包括:
方向调整模块,用于在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值不高于角度阈值的情况下,通过公式:p0(t+T)=p0(t+T-1)+δl[cos(θ(F(p0(t+T-1)))),sin(θ(F(p0(t+T-1))))]T计算无人机t+T时刻的位置;其中,将δl是预设的迭代步长,p0(t+T-1)是t+T-1时刻无人机在二维平面的位置,θ(F(p0(t+T-1))是t+T-1时刻混合意图力场的方向;p0(t+T)是t+T时刻无人机在二维平面的位置;
在t+T-1时刻混合意图力场的方向与无人机的飞行方向的角度差值高于角度阈值的情况下,通过公式:θ0(t+T)=θ0(t+T-1)+sgn(θ(F(p0(t+T-1)))-θ0(t+T-1))×δθ,p0(t+T)=p0(t+T-1)+δl[cos(θ0(t+T-1)),sin(θ0(t+T-1))]T,计算无人机t+T时刻的位置;
其中,sgn(·)为符号函数,θ0(t+T)是t+T时刻无人机的飞行方向,θ0(t+T-1)是t+T-1时刻无人机的飞行方向,t+T时刻无人机在二维平面的位置与目标点的位置之间的距离小于无人机的最小转弯半径,T为不小于1的正整数。
可选的,本发明实施例提供的一种无人机的路径规划的装置还包括:
航线规划模块,用于初始化航线点;航线点包括:航线点水平位置及航线点垂直位置,初始化航线点是令第t个航线点位置等于t时刻无人机在二维平面的位置,且s=1;
判断t+j个航线点的位置是否是最后一个航线点位置,如果是,则令s=s+1,Q(s)=q0(t+j);
如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率;
基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离;
如果t+j个航线点处的切线距离小于切线距离阈值,则重复如果t+j个航线点的位置不是最后一个航线点位置,则j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于第t个航线点处航线切线的斜率及在第t个航线点处的切线方程,计算t+j个航线点处的切线距离的过程;
如果t+j个航线点处的切线距离不小于切线距离阈值,则令航路点序号s=s+1,Q(s)等于q0(t+j-1),输出Q(s);
其中,s是航路点序号,Q(s)是航路点序列;q0(t)是第t个航线点;j取正整数,斜率计算公式是:kq是第t个航线点处的斜率,yq0(t+j)是t+j个航线点垂直位置,yq0(t)是t个航线点垂直位置,xq0(t+j)是t+j个航线点水平位置,xq0(t)是t个航线点水平位置,第t个航线点处的切线方程是:yq0(t)=kqxq0(t)+bq,bq=yq0(t)-kqxq0(t),dq(t+j)是t+j个航线点处的切线距离,角标q代表航线点参数,角标q0代表航线点坐标位置参数。
可选的,本发明实施例提供的一种无人机的路径规划的装置还包括:
阈值确定模块,用于:将第t个航线点与第t+j个航线点之间距离的绝对值与sin(δθ)的乘积,确定为切线距离阈值,其中,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值。
本发明实施例还提供了一种电子设备,如图6所示,包括处理器601、存储器602、通信接口603和通信总线604,其中,处理器601,存储器602,通信接口603通过通信总线604完成相互间的通信,
存储器602,用于存放计算机程序;
处理器601,用于执行存储器602上所存放的程序时,实现如下步骤:
获取t时刻无人机的信息,无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
确定t时刻靠近目标点意图的意图力场;
确定t时刻减少径向抵近第i个障碍物意图的意图力场;
确定t时刻绕开前方障碍物意图的意图力场;
将t时刻靠近目标点意图的意图力场、t时刻减少径向抵近i个障碍物意图的意图力场及t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
在t时刻无人机的混合意图力场为0的情况下,调节角度旋转矩阵直至t时刻无人机的混合意图力场不为0;
在t+1时刻,基于不为0的混合意图力场的方向,进行飞行。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect,简称PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture,简称EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示,图中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory,简称RAM),也可以包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,简称CPU)、网络处理器(Network Processor,简称NP)等;还可以是数字信号处理器(Digital Signal Processing,简称DSP)、专用集成电路(Application SpecificIntegrated Circuit,简称ASIC)、现场可编程门阵列(Field-Programmable Gate Array,简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中,还提供了一种计算机可读存储介质,该计算机可读存储介质中存储有指令,当其在计算机上运行时,使得计算机执行上述实施例中任一所述的一种无人机的路径规划方法。
在本发明提供的又一实施例中,还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述实施例中任一所述的一种无人机的路径规划方法。
在上述实施例中,可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时,可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时,全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中,或者从一个计算机可读存储介质向另一个计算机可读存储介质传输,例如,所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质,(例如,软盘、硬盘、磁带)、光介质(例如,DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述,各个实施例之间相同相似的部分互相参见即可,每个实施例重点说明的都是与其他实施例的不同之处。尤其,对于装置/电子设备/计算机可读存储介质/计算机程序产品实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例而已,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等,均包含在本发明的保护范围内。
Claims (10)
1.一种无人机的路径规划方法,其特征在于,所述方法包括:
获取t时刻无人机的信息,所述无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
确定t时刻靠近目标点意图的意图力场,所述靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将所述目标点的位置与所述无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在所述第一距离大于所述第一门限参数的情况下,将第一比值、所述第一门限参数与所述第一尺度因子相乘得到的;所述第一比值为:所述目标点的位置与所述无人机在二维平面的位置的距离,与所述第一距离的比值;所述第一距离为:目标点的位置与所述无人机在二维平面的位置的欧几里得距离;
确定t时刻减少径向抵近第i个障碍物意图的意图力场;所述减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、所述第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在所述第二距离不小于所述第二门限参数的情况下,将所述减少径向抵近i个障碍物意图的意图力场作为0得到的;所述第二比值为:所述无人机在二维平面的位置与第i个障碍物的位置之间的差值,与所述第二距离的比值;所述第二距离为:所述无人机在二维平面的位置与所述第i个障碍物的位置的欧几里得距离;所述距离差值为:所述第二距离的倒数与所述第二门限参数的倒数相减的结果,与所述第二距离的二次方的倒数相乘的结果;
确定t时刻绕开前方障碍物意图的意图力场;所述绕开前方障碍物意图的意图力场是通过在第三距离小于所述第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在所述第三距离不小于所述第二门限参数的情况下,将所述绕开前方障碍物意图的意图力场作为0得到的,所述第三距离为:所述无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离,所述第三比值为:t时刻无人机飞行方向上第m个最近的障碍物的位置与t时刻无人机在二维平面的位置之间的差值与,t时刻无人机至无人机飞行方向上第m个最近的障碍物之间的距离二次方的比值;
将所述t时刻靠近目标点意图的意图力场、所述t时刻减少径向抵近i个障碍物意图的意图力场及所述t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
在所述t时刻无人机的混合意图力场为0的情况下,调节所述角度旋转矩阵直至所述t时刻无人机的混合意图力场不为0;
在t+1时刻,基于不为0的所述混合意图力场的方向,进行飞行。
2.根据权利要求1所述的方法,其特征在于,所述将所述t时刻靠近目标点意图的意图力场、所述t时刻减少径向抵近i个障碍物意图的意图力场及所述t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场,包括:
使用矢量求和公式,对t时刻靠近目标点意图的意图力场、减少径向抵近i个障碍物意图的意图力场及所述t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为无人机的混合意图力场;
3.根据权利要求1所述的方法,其特征在于,所述在所述t时刻无人机的混合意图力场为0的情况下,调节所述角度旋转矩阵直至所述t时刻无人机的混合意图力场不为0,包括:
在所述t时刻无人机的混合意图力场为0的情况下,通过如下角度公式调节所述角度旋转矩阵直至所述t时刻无人机的混合意图力场不为0;
所述角度公式为:其中,θm1(t)为:t时刻无人机至所述无人机飞行方向上最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上最近的障碍物方向的左侧切线的夹角,θm2(t)是:t时刻无人机至所述无人机飞行方向上最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上最近的障碍物方向的右侧切线的夹角,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值;θm(t)是时刻无人机至所述无人机飞行方向上第m个最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上第m个最近的障碍物方向的切线的夹角,角度旋转矩阵是:p0(t)是t时刻无人机在二维平面的位置,m是t时刻无人机飞行方向上最近的障碍物的序号。
4.根据权利要求1所述的方法,其特征在于,所述方法还包括:
在t+T-1时刻所述混合意图力场的方向与无人机的飞行方向的角度差值不高于角度阈值的情况下,通过公式:p0(t+T)=p0(t+T-1)+δl[cos(θ(F(p0(t+T-1)))),sin(θ(F(p0(t+T-1))))]T计算无人机t+T时刻的位置;其中,将δl是预设的迭代步长,p0(t+T-1)是t+T-1时刻无人机在二维平面的位置,θ(F(p0(t+T-1))是t+T-1时刻混合意图力场的方向;p0(t+T)是t+T时刻无人机在二维平面的位置;
在t+T-1时刻所述混合意图力场的方向与无人机的飞行方向的角度差值高于角度阈值的情况下,通过公式:θ0(t+T)=θ0(t+T-1)+sgn(θ(F(p0(t+T-1)))-θ0(t+T-1))×δθ,p0(t+T)=p0(t+T-1)+δl[cos(θ0(t+T-1)),sin(θ0(t+T-1))]T,计算无人机t+T时刻的位置;
其中,sgn(·)为符号函数,θ0(t+T)是t+T时刻无人机的飞行方向,θ0(t+T-1)是t+T-1时刻无人机的飞行方向,所述t+T时刻无人机在二维平面的位置与目标点的位置之间的距离小于无人机的最小转弯半径,所述T为不小于1的正整数。
5.根据权利要求4所述的方法,其特征在于,在所述计算无人机t+T时刻的位置的步骤之后,所述方法还包括:
初始化航线点;所述航线点包括:航线点水平位置及航线点垂直位置,所述初始化航线点是令第t个航线点位置等于t时刻无人机在二维平面的位置,且s=1;
判断t+j个航线点的位置是否是最后一个航线点位置,如果是,则令s=s+1,Q(s)=q0(t+j);
如果t+j个航线点的位置不是最后一个航线点位置,则令j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率;
基于所述第t个航线点处航线切线的斜率及在所述第t个航线点处的切线方程,计算所述t+j个航线点处的切线距离;
如果t+j个航线点处的切线距离小于切线距离阈值,则重复如果t+j个航线点的位置不是最后一个航线点位置,则j=j+1,利用斜率计算公式计算在第t个航线点处航线切线的斜率,基于所述第t个航线点处航线切线的斜率及在所述第t个航线点处的切线方程,计算所述t+j个航线点处的切线距离的过程;
如果t+j个航线点处的切线距离不小于切线距离阈值,则令航路点序号s=s+1,Q(s)等于q0(t+j-1),输出所述Q(s);
6.根据权利要求5所述的方法,其特征在于,所述切线距离阈值可以通过以下步骤得到:
将所述第t个航线点与所述t+j个航线点之间距离的绝对值与sin(δθ)的乘积,确定为所述切线距离阈值,其中,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值。
7.一种无人机的路径规划装置,其特征在于,所述装置包括:
信息获取模块,用于获取t时刻无人机的信息,所述无人机的信息包括:无人机在二维平面的位置、无人机的飞行方向、目标点的位置、第i个障碍物的位置、第i个障碍物占据的水平面积及障碍物的总数;
第一确定模块,用于确定t时刻靠近目标点意图的意图力场;所述靠近目标点意图的意图力场是通过在第一距离不大于第一门限参数的情况下,将所述目标点的位置与所述无人机在二维平面的位置之间距离,与第一尺度因子相乘得到的;或者,在所述第一距离大于所述第一门限参数的情况下,将第一比值、所述第一门限参数与所述第一尺度因子相乘得到的;所述第一比值为:所述目标点的位置与所述无人机在二维平面的位置的距离,与所述第一距离的比值;所述第一距离为:目标点的位置与所述无人机在二维平面的位置的欧几里得距离;
第二确定模块,用于确定t时刻减少径向抵近第i个障碍物意图的意图力场;所述减少径向抵近i个障碍物意图的意图力场是通过在第二距离小于第二门限参数的情况下,将第二尺度因子、所述第i个障碍物占据的水平面积、第二比值与距离差值相乘得到的,或者在所述第二距离不小于所述第二门限参数的情况下,将所述减少径向抵近i个障碍物意图的意图力场作为0得到的;所述第二比值为:所述无人机在二维平面的位置与第i个障碍物的位置之间的差值,与所述第二距离的比值;所述第二距离为:所述无人机在二维平面的位置与所述第i个障碍物的位置的欧几里得距离;所述距离差值为:所述第二距离的倒数与所述第二门限参数的倒数相减的结果,与所述第二距离的二次方的倒数相乘的结果;
第三确定模块,用于确定t时刻绕开前方障碍物意图的意图力场;所述绕开前方障碍物意图的意图力场是通过在第三距离小于所述第二门限参数的情况下,将第三尺度因子、t时刻无人机飞行方向上最近的障碍物占据的水平面积、角度旋转矩阵及第三比值相乘得到的;或者,在所述第三距离不小于所述第二门限参数的情况下,将所述绕开前方障碍物意图的意图力场作为0得到的,所述第三距离为:所述无人机在二维平面的位置与t时刻无人机飞行方向上最近的障碍物的位置的欧几里得距离,所述第三比值为:t时刻无人机飞行方向上第m个最近的障碍物的位置与t时刻无人机在二维平面的位置之间的差值与,t时刻无人机至无人机飞行方向上第m个最近的障碍物之间的距离二次方的比值;
力场求和模块,用于将所述t时刻靠近目标点意图的意图力场、所述t时刻减少径向抵近i个障碍物意图的意图力场及所述t时刻绕开前方障碍物意图的意图力场进行矢量求和,将求和结果确定为t时刻无人机的混合意图力场;
角度调节模块,用于在所述t时刻无人机的混合意图力场为0的情况下,调节所述角度旋转矩阵直至所述t时刻无人机的混合意图力场不为0;
飞行确定模块,用于在t+1时刻,基于不为0的所述混合意图力场的方向,进行飞行。
9.根据权利要求7所述的装置,其特征在于,所述角度调整模块,具体用于:
在所述t时刻无人机的混合意图力场为0的情况下,通过如下角度公式调节所述角度旋转矩阵直至所述t时刻无人机的混合意图力场不为0;
所述角度公式为:其中,θm1(t)为:t时刻无人机至所述无人机飞行方向上最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上最近的障碍物方向的左侧切线的夹角,θm2(t)是t时刻无人机至所述无人机飞行方向上最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上最近的障碍物方向的右侧切线的夹角,δθ是无人机在t时刻至t+1时刻内可调整的最大角度值;θm(t)是时刻无人机至所述无人机飞行方向上第m个最近的障碍物中心的连线、与所述无人机至所述无人机飞行方向上第m个最近的障碍物方向的切线的夹角,角度旋转矩阵是:p0(t)是t时刻无人机在二维平面的位置,m是t时刻无人机飞行方向上最近的障碍物的序号。
10.一种电子设备,其特征在于,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现权利要求1-6任一所述的方法步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910160307.0A CN109839956B (zh) | 2019-03-04 | 2019-03-04 | 一种无人机的路径规划方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910160307.0A CN109839956B (zh) | 2019-03-04 | 2019-03-04 | 一种无人机的路径规划方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109839956A CN109839956A (zh) | 2019-06-04 |
CN109839956B true CN109839956B (zh) | 2020-08-07 |
Family
ID=66885296
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910160307.0A Active CN109839956B (zh) | 2019-03-04 | 2019-03-04 | 一种无人机的路径规划方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109839956B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111506104B (zh) * | 2020-04-03 | 2021-10-01 | 北京邮电大学 | 一种规划无人机位置的方法及装置 |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104406588A (zh) * | 2014-11-13 | 2015-03-11 | 沈阳航空航天大学 | 一种威胁环境下基于导引速度场的航路规划方法 |
US9436187B2 (en) * | 2015-01-15 | 2016-09-06 | The United States Of America As Represented By The Secretary Of The Navy | System and method for decentralized, multi-agent unmanned vehicle navigation and formation control |
CN105867365B (zh) * | 2016-03-11 | 2019-02-05 | 中国矿业大学(北京) | 基于改进人工势场法的路径规划导航系统及方法 |
CN108459612B (zh) * | 2017-02-21 | 2021-03-23 | 北京航空航天大学 | 基于人工势场法的无人机编队控制方法及装置 |
CN107219857B (zh) * | 2017-03-23 | 2020-12-01 | 南京航空航天大学 | 一种基于三维全局人工势函数的无人机编队路径规划算法 |
CN108326849B (zh) * | 2018-01-04 | 2019-08-30 | 浙江大学 | 一种基于改进人工势场法的多自由度机械臂动态避障路径规划方法 |
CN108469828A (zh) * | 2018-03-23 | 2018-08-31 | 哈尔滨工程大学 | 一种改进人工势场优化算法的auv航路规划方法 |
CN108415461A (zh) * | 2018-05-28 | 2018-08-17 | 济南大学 | 一种无人飞行器的航迹规划 |
CN109062948B (zh) * | 2018-06-22 | 2020-11-13 | 广州杰赛科技股份有限公司 | 目标点确定、目标路径确定方法和系统 |
-
2019
- 2019-03-04 CN CN201910160307.0A patent/CN109839956B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109839956A (zh) | 2019-06-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Rathinam et al. | Vision-based monitoring of locally linear structures using an unmanned aerial vehicle | |
McGee et al. | Obstacle detection for small autonomous aircraft using sky segmentation | |
US9523986B1 (en) | System and method for secure, privacy-aware and contextualised package delivery using autonomous vehicles | |
Bacik et al. | Autonomous flying with quadrocopter using fuzzy control and ArUco markers | |
CN112740268B (zh) | 目标检测方法和装置 | |
Weon et al. | Object Recognition based interpolation with 3d lidar and vision for autonomous driving of an intelligent vehicle | |
Escobar‐Alvarez et al. | R‐ADVANCE: rapid adaptive prediction for vision‐based autonomous navigation, control, and evasion | |
Yasin et al. | Low-cost ultrasonic based object detection and collision avoidance method for autonomous robots | |
CN108391429A (zh) | 用于自主车辆速度跟随的方法和系统 | |
Eynard et al. | Real time UAV altitude, attitude and motion estimation from hybrid stereovision | |
US20180165974A1 (en) | Vehicle collision prevention | |
Zhang et al. | Monocular depth estimation for UAV obstacle avoidance | |
Han et al. | Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map | |
CN109839956B (zh) | 一种无人机的路径规划方法及装置 | |
Lioulemes et al. | Safety challenges in using AR. Drone to collaborate with humans in indoor environments | |
Ivanovas et al. | Block matching based obstacle avoidance for unmanned aerial vehicle | |
Flögel et al. | Infrastructure-aided localization and state estimation for autonomous mobile robots | |
Hara et al. | Vehicle localization based on the detection of line segments from multi-camera images | |
Jurado et al. | Vision‐based trajectory tracking system for an emulated quadrotor UAV | |
Singh et al. | Map making in social indoor environment through robot navigation using active SLAM | |
CN206096937U (zh) | 无人配送车 | |
Lwowski et al. | A reactive bearing angle only obstacle avoidance technique for unmanned ground vehicles | |
Abegaz et al. | Sensors based Lane Keeping and Cruise Control of Self Driving Vehicles | |
Bauer et al. | Vision only collision detection with omnidirectional multi-camera system | |
Huang et al. | Autonomous multi-view navigation via deep reinforcement learning |
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 |