一种无人系统待跟踪路径航向角的估算算法

1.本发明涉及无人系统运动控制技术领域，具体涉及一种无人系统待跟踪路径航向角的估算算法。


背景技术：

2.无人系统跟踪控制可以分为路径跟踪和轨迹跟踪两类问题，在进行跟踪控制时往往需要提前获得待跟踪路径参考位点处的航向角或轨迹点处的航向角。以无人车的路径跟踪问题为例，在跟踪一条曲线路径时需要使用参考点处的航向角计算航向误差；以无人艇轨迹跟踪为例，对生成的一系列不包含首向角信息的航迹点的跟踪，需要一种方法估算每个航迹点处的首向角；以无人艇路径跟踪为例，有些制导算法与控制律的设计则要求待跟踪路径参考点处航向角或曲率(或切向角)已知。
3.目前大多研究在处理路径跟踪或轨迹跟踪问题时，通常用两种办法处理航向角：
4.1、待跟踪路径或轨迹航程较短，或不涉及频繁转向，航向角始终在0到180
°
或0到-180
°
间，则航向角直接用切向角替代，此方法不适合于任意路径的跟踪；
5.2、在涉及到偏航角连续转动经过北轴的情况时，若该问题不复杂，则手动更新航向角，但是手动更新只能用于短航程问题的研究，对于无限航程和转向频繁的跟踪问题不适用。


技术实现要素：

6.根据现有技术的不足，本发明的目的是提供一种无人系统待跟踪路径航向角的估算算法，能够解决任意路径跟踪时的航向角估算问题，且精度较高。
7.为了解决上述技术问题，本发明采用的技术方案为：
8.一种无人系统待跟踪路径航向角的估算算法，其特征在于，包括：
9.步骤1、获取无人系统运动过程中的密集离散位点集合；
10.步骤2、在ned坐标系下，令在某一位点处航行器绕竖直轴旋转圈数参数为m，假设初始位点航行器首向与正北方向一致时航向角为0
°
，初始位点处航行器绕竖直轴旋转圈数参数m0为0；
11.步骤3、令航行器在某一位点处绕竖直轴转动方向参数为flag1和切向角符号为flag2，其中，flag1＝1表示假设航行器对该路径或轨迹准确无误地跟踪时，估计在该位点处邻域范围内正在作顺时针转动，flag1＝0表示假设航行器对该路径或轨迹准确无误地跟踪时，估计在该位点处邻域范围内正在作逆时针转动，flag2＝1表示当前位点切向角在范围[0
°
,180
°
]内，flag2＝0表示当前位点切向角在范围(-180
°
,0
°
)内，获取航行器在初始位点p0(x0,y0)处绕竖直轴转动方向参数flag10和切向角符号flag20；
[0012]
步骤4、定义循环变量j用来遍历密集离散位点集合d，j初始赋值为1，获取循环变量j，读取位点pj(xj,yj)和p
j 1
(x
j 1
,y
j 1
)，并得到位点pj(xj,yj)的切向角φj；
[0013]
步骤5、获取参数mj、flag1j、flag2j；
[0014]
步骤6、计算当前位点pj(xj,yj)的航向角ψj，并令j＝j 1，返回步骤4，获取循环变量j，直到得到除集合d中初始位点p0(x0,y0)和最后一个位点外所有位点的航向角。
[0015]
进一步地，所述步骤1中，密集离散位点集合d能够由轨迹跟踪问题中的密集轨迹点组成，也能够在路径跟踪问题中对路径采样获得。
[0016]
进一步地，所述步骤3中，读取密集离散位点集合d的前三个点{p0(x0,y0),p1(x1,y1),p2(x2,y2)}，初始点航行器绕竖直轴转动方向参数flag10和切向角符号flag20的计算公式为：
[0017][0018]
进一步地，所述步骤4中，位点pj(xj,yj)处切向方向用其相邻两位点构成的向量近似表示，位点pj(xj,yj)处切向角计算公式为：
[0019]
进一步地，所述步骤5包括：
[0020]
步骤5.1、根据m
j-1
、flag1
j-1
、flag2
j-1
和位点pj(xj,yj)的切向角φj获取位点pj(xj,yj)处的航行器绕竖直轴旋转圈数参数；
[0021]
若φj≥0，flag1j＝1且flag2j＝0时判定航行器偏航角累计满360
°
，遂进入下一圆周转动的累计，更新mj＝m
j-1
1；
[0022]
若φj《0，flag1j＝0且flag2j＝1时判定航行器偏航角返回上一圆周转动的累计，更新mj＝m
j-1-1；
[0023]
否则，mj＝m
j-1
；
[0024]
步骤5.2、根据m
j-1
、flag1
j-1
、flag2
j-1
和位点pj(xj,yj)的切向角φj获取位点pj(xj,yj)处的绕竖直轴转动方向参数和切向角符号；
[0025]
若φj≥0，当同时满足flag1
j-1
＝0、y
j 1-y
j-1
≥0和flag2
j-1
＝1或同时满足flag1
j-1
＝0和y
j 1-y
j-1
《0时更新flag1j＝0，flag2j＝1，否则更新flag1j＝1，flag2j＝1；
[0026]
若φj《0，当同时满足flag1
j-1
＝1、y
j 1-y
j-1
≥0和flag2
j-1
＝0或同时满足flag1
j-1
＝1、y
j 1-y
j-1
《0时更新flag1j＝1，flag2j＝0，否则更新flag1j＝0，flag2j＝0。
[0027]
进一步地，所述步骤6中，由参数mj、flag1j、flag2j和位点pj(xj,yj)的切向角φj计算当前位点pj(xj,yj)的航向角ψj，若flag1j＝flag2j，则ψj＝φj mj*360
°
；若flag1j＝0、flag2j＝1，则ψj＝φj (m
j-1)*360
°
；若flag1j＝1、flag2j＝0，则ψj＝φj (mj 1)*360
°
。
[0028]
进一步地，对于不存在横向运动的无人系统，无人系统待跟踪路径航向角能够直接通过步骤1～步骤6获取，对于有横向速度的无人系统，引入一个小角度修正量ε；
[0029]
则位点pj处首向角ψj′
的计算公式为：
[0030]
ψj′
＝ψj ε
[0031][0032]
其中：u,v分别是无人系统的纵向速度、横向速度。
[0033]
与现有技术相比，本发明具有以下优点和有益效果：
[0034]
1.本发明所述的一种无人系统待跟踪路径航向角的估算算法，算法结构简单，易于编程实现，程序运行速度快，该算法通过更新航行器绕竖直轴旋转圈数参数、航行器绕竖直轴转动方向参数和切向角符号参数实现“假设航行器对该路径或轨迹准确无误地跟踪时，该航行器在位点处的状态”的估计，进而可以自动估算、输出待跟踪路径上各位点处的航向角，避免了在偏航角连续转动经过北轴的情况下人工修正参考航向角，并且适用于无限航程和转向频繁的跟踪问题。
[0035]
2.本发明所述的一种无人系统待跟踪路径航向角的估算算法，根据任意一条路径的离散位点集合，使用某位点的相邻两位点构成的向量与反余弦函数估算该点处的切向角和航向角，避免了函数求导运算以及在某些位点处切向角、航向角不存在的情况，对于路径上每个位点的跟踪都能估算出一个参考航向角，适用于对任意路径下航向角的估算。
[0036]
3.本发明所述的一种无人系统待跟踪路径航向角的估算算法，通过定义在某位点处的航行器绕竖直轴转动方向参数和切向角符号参数使问题得到了简化，方便对下个位点处的航行器绕竖直轴旋转圈数参数进行更新。
附图说明
[0037]
图1为本发明待跟踪路径航向角的估算流程图。
[0038]
图2为本发明步骤5和步骤6中参数m,flag1,flag2更新流程图。
[0039]
图3为本发明实施例在一条任给的待跟踪路径下的测试示意图。
[0040]
图4为本发明有横向速度的无人艇示意图。
具体实施方式
[0041]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
[0042]
在本发明中，切向角定义为：待跟踪路径或轨迹上某一位点处切向量方向与正北方向间的夹角，取值区间为(-180
°
，180
°
]，用φ表示。
[0043]
航向角或偏航角定义为：航行器准确无误地跟踪至路径上某位点处时，初始点切向角与初始时刻到当前时刻关于偏航角速度的积分的加和，用ψ表示。
[0044]
一种无人系统待跟踪路径航向角的估算算法，包括以下步骤，如图1和图2所示：
[0045]
步骤1、获取无人系统运动过程中的密集离散位点集合，令所述密集离散位点集合为d＝{pi(xi,yi),i＝0,1,2,3
…
,n}；
[0046]
步骤2、在ned坐标系下，令在某一位点处航行器绕竖直轴旋转圈数参数为m，假设初始位点航行器首向与正北方向一致时航向角为0
°
，初始位点处航行器绕竖直轴旋转圈数参数m0为0；
[0047]
步骤3、令航行器在某一位点处绕竖直轴转动方向参数为flag1和切向角符号为flag2，其中，flag1＝1表示假设航行器对该路径或轨迹准确无误地跟踪时，估计在该位点处邻域范围内正在作顺时针转动，flag1＝0表示假设航行器对该路径或轨迹准确无误地跟踪时，估计在该位点处邻域范围内正在作逆时针转动，flag2＝1表示当前位点切向角在范围[0
°
,180
°
]内，flag2＝0表示当前位点切向角在范围(-180
°
,0
°
)内，获取航行器在初始位点p0(x0,y0)处绕竖直轴转动方向参数flag10和切向角符号flag20；
[0048]
步骤4、定义循环变量为j用来遍历密集离散位点集合d，j初始赋值为1，获取循环变量j，读取位点pj(xj,yj)和p
j 1
(x
j 1
,y
j 1
)，并得到位点pj(xj,yj)的切向角φj；
[0049]
步骤5、获取参数mj、flag1j、flag2j；
[0050]
步骤6、计算当前位点pj(xj,yj)的航向角ψj，并令j＝j 1，返回步骤4，获取循环变量j，直到得到除集合d中初始位点p0(x0,y0)和最后一个位点外所有位点的航向角。
[0051]
其中，所述步骤1中，密集离散位点集合d可以由轨迹跟踪问题中的密集轨迹点组成，也可以在路径跟踪问题中对路径采样获得。
[0052]
在步骤1中，通过获取无人系统运动过程中的密集离散位点集合，使得本估算算法能够解决任意路径跟踪时的航向角估算问题。
[0053]
所述步骤2中，竖直轴也指的是地轴。
[0054]
在步骤3中，读取密集离散位点集合d的前三个点{p0(x0,y0),p1(x1,y1),p2(x2,y2)}，初始点航行器绕竖直轴转动方向参数flag10和切向角符号flag20的计算公式为：
[0055][0056]
在步骤4中，位点pj(xj,yj)处切向方向用其相邻两位点构成的向量近似表示，位点pj(xj,yj)处切向角计算公式为：
[0057]
所述步骤5包括，如图2所示：
[0058]
步骤5.1、根据m
j-1
、flag1
j-1
、flag2
j-1
和位点pj(xj,yj)的切向角φj获取位点pj(xj,yj)处的航行器绕竖直轴旋转圈数参数；
[0059]
若φj≥0，flag1j＝1且flag2j＝0时判定航行器偏航角累计满360
°
，遂进入下一圆周转动的累计，更新mj＝m
j-1
1；
[0060]
若φj《0，flag1j＝0且flag2j＝1时判定航行器偏航角返回上一圆周转动的累计，更新mj＝m
j-1-1；
[0061]
否则，mj＝m
j-1
；
[0062]
步骤5.2、根据m
j-1
、flag1
j-1
、flag2
j-1
和位点pj(xj,yj)的切向角φj获取位点pj(xj,yj)处的绕竖直轴转动方向参数和切向角符号；
[0063]
若φj≥0，当同时满足flag1
j-1
＝0、y
j 1-y
j-1
≥0和flag2
j-1
＝1或同时满足flag1
j-1
＝0和y
j 1-y
j-1
《0时更新flag1j＝0，flag2j＝1，否则更新flag1j＝1，flag2j＝1；
[0064]
若φj《0，当同时满足flag1
j-1
＝1、y
j 1-y
j-1
≥0和flag2
j-1
＝0或同时满足flag1
j-1
＝1、y
j 1-y
j-1
《0时更新flag1j＝1，flag2j＝0，否则更新flag1j＝0，flag2j＝0。
[0065]
在这个过程中，由于参数flag2含义是表示当前位点pj(xj,yj)的切向角符号，即当前位点切向向量在北轴左侧或右侧，因此当前位点切向角φj≥0时，flag2j＝1，而φj《0时，flag2j＝0。
[0066]
而由于参数flag1表示当前位点pj(xj,yj)处的绕竖直轴转动方向参数，即当前位点pj(xj,yj)邻域内的转动方向，若φj≥0，当同时满足flag1
j-1
＝0、y
j 1-y
j-1
≥0和flag2
j-1
＝1或同时满足flag1
j-1
＝0和y
j 1-y
j-1
《0时认为航行器对该路径或轨迹准确无误地跟踪时应正在逆时针转动，否则保持顺时针转动；而φj《0，当同时满足flag1
j-1
＝1、y
j 1-y
j-1
≥0和
flag2
j-1
＝0或同时满足flag1
j-1
＝1、y
j 1-y
j-1
《0时认为航行器对该路径或轨迹准确无误地跟踪时应正在顺时针转动，否则保持逆时针转动。
[0067]
在步骤6中，由参数mj、flag1j、flag2j和位点pj(xj,yj)的切向角φj，若flag1j＝flag2j，则ψj＝φj mj*360
°
；若flag1j＝0、flag2j＝1，则ψj＝ψj (m
j-1)*360
°
；若flag1j＝1、flag2j＝0，则ψj＝φj (mj 1)*360
°
。
[0068]
在本发明一个实施例中，如图3所示，该路径由一条线段和3个半圆所组成：线段长度为50，半圆的半径分别为50、100、150，半圆的圆心分别为(0,0)、(50,0)、(0,0)；离散位点由对该路径采样得到，线段上采样间隔为0.5，在半圆上的采样间距为2rad；初始点坐标为(0,0)处，若航行器从此处出发，偏航角速度方向全程会保持为顺时针，本发明算法得到的航向角呈不断增加趋势，该估算结果与实际沿该路径运动的航行器结果一致。图3中标记的a～g处坐标点的估算结果及其误差如表1所示。
[0069]
在步骤2中，初始点(0,0)处m0为0；
[0070]
读取集合前三个点的坐标分别为(0,0)、(0.5,0)、(1,0)，通过步骤3得flag10＝flag20＝1；
[0071]
从初始点(0,0)到序号a处位点(50,0)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、1，由步骤4和步骤5更新参数mj,flag1j,flag2j为0、1、1。
[0072]
对于序号a处位点(50,0)，其相邻两点坐标分别为(49.5,0)和(49.97,-1.74)，用步骤4中切向角计算公式得位点1处切向角约为164.939
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、1，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为0、1、1，则根据步骤6，该点航向角为164.939
°
。
[0073]
对于序号a处位点(50,0)的下一个位点(49.97,-1.74)，位点(49.97,-1.74)的相邻两点坐标分别为(50,0)和(49.88,-3.49)，用步骤4中切向角计算公式得位点2处切向角约为-178.031
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、1，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为0、1、0，则根据步骤6，该点航向角为181.969
°
。
[0074]
从位点(49.97,-1.74)到序号b处位点(0,-50)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、0，由步骤4和步骤5更新参数mj,flag1j,flag2j为0、1、0。
[0075]
对于序号b处位点(0,-50)，其相邻两点坐标分别为(0.87,-49.99)和(-0.87,-49.99)，用步骤4中切向角计算公式得位点2处切向角约为-90
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、0，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为0、1、0，则根据步骤6，该点航向角为270
°
。
[0076]
从序号b处位点(0,-50)到序号c处位点(-50,0)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、0，由步骤4和步骤5更新参数mj,flag1j,flag2j为0、1、0。
[0077]
对于序号c处位点(-50,0)，其相邻两点坐标分别为(-49.99,-0.87)和(-49.98,1.75)，用步骤4中切向角计算公式得位点3处切向角约为0.167
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为0、1、0，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为1、1、1，则根据步骤6，该点航向角为360.167
°
。
[0078]
从序号c处位点(-50,0)到序号d处位点(50,100)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、1，由步骤4和步骤5更新参数mj,flag1j,flag2j为1、1、
1。
[0079]
对于序号d处位点(50,100)，其相邻两点坐标分别为(48.25,99.98)和(51.75,99.98)，用步骤4中切向角计算公式得位点4处切向角约为90
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、1，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为1、1、1，则根据步骤6，该点航向角为450
°
。
[0080]
从序号d处位点(50,100)到序号e处位点(150,0)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、1，由步骤4和步骤5更新参数mj,flag1j,flag2j为1、1、1。
[0081]
对于序号e处位点(150,0)，其相邻两点坐标分别为(149.98,1.75)和(149.97,-2.62)，用步骤4中切向角计算公式得位点5处切向角约为-179.90
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、1，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为1、1、0，则根据步骤6，该点航向角为540.100
°
。
[0082]
从序号e处位点(150,0)到序号f处位点(0,-150)，两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、0，由步骤4和步骤5更新参数mj,flag1j,flag2j为1、1、0。
[0083]
对于序号f处位点(0,-150)，其相邻两点坐标分别为(2.62,-149.98)和(-2.62,-149.98)，用步骤4中切向角计算公式得位点6处切向角约为-90
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、0，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为1、1、0，则根据步骤6，该点航向角为630
°
。
[0084]
从序号f处位点(0,-150)到序号g处位点两点之间的位点pj(xj,yj)均满足：m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、0，由步骤4和步骤5更新参数mj,flag1j,flag2j为1、1、0。
[0085]
对于序号g处位点其相邻两点坐标分别为(-104.20,-107.90)和(-107.90,-104.20)，用步骤4中切向角计算公式得位点7处切向角约为-45.00
°
；当前参数m
j-1
,flag1
j-1
,flag2
j-1
分别为1、1、0，由步骤5更新参数，更新后的参数mj,flag1j,flag2j分别为1、1、0，则根据步骤6，该点航向角为675
°
[0086]
表1 航行器在图3中路径位点a～g的航向角估算
[0087]
[0088]
由表1可见，本发明算法可以对无人系统待跟踪路径的位点航向角做出较为准确的估算。序号1是线段与半圆的交点，在该点处路径不可导，故航向角真实值为空。表1中给出的结果可以用作跟踪控制的参考值，对于不可导点也可给出参考航向角，且该点处航向角方向指向下一条切换路径，表示航行器在拐角处期望转向，可以作为实际航行的参考。
[0089]
由于本发明估算航向角的基础是相邻两位点的坐标，而任意路径都能表达为离散位点的集合，因此本发明算法对于任意路径都适用，且离散位点集合越密集估算结果越趋近于真实值(期望值)。实施例中步骤4给出的切向角估算公式，用反余弦函数替代反正切函数，避免了90
°
无法求解切向角的情况。
[0090]
对于不存在横向运动的无人系统，例如无人车、轮式机器人等，其在跟踪路径或轨迹时的参考航向角信息可以直接由本发明估算得出。对于有横向速度的无人系统，例如无人艇，如图4所示，其首向与实际运动方向存在夹角。无人艇的横向速度相对于纵向速度较小，可以引入一个小角度修正量ε，则位点pj处首向角ψj′
的估算为：ψj′
＝ψj ε。其中，u,v分别是无人艇的纵向速度、横向速度。由于无人艇速度变化缓慢，因此将本发明应用于无人艇轨迹跟踪中，在线对未来一段时间参考航向角进行估算时具有较好性能。
[0091]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。