基于路径规划的无人艇容错控制方法及其控制系统

1.本发明属于无人艇控制技术领域，更具体地，涉及一种基于路径规划的无人艇容错控制方法及其控制系统。


背景技术：

2.近几十年来，无人艇得到了巨大的发展，它是海洋工程不可缺少的工具。由于其高效率、灵活性和低运行成本，无人艇已广泛应用于水质监测、海洋调查、军事侦察等领域。在一些水上任务中，无人艇的工作负载(包括传感器、执行器、能源系统)通常很贵重。此外，无人艇的运动经常受到风、浪、潮汐、浪涌和水下障碍物(珊瑚礁、泥浆)等海洋环境干扰，这很容易导致无人艇执行器故障，存在执行器不能调零的状况，这意味着控制输入始终被限制在正区间或者是负区间。例如，如果双体船的一侧失去动力、单体船转向舵机卡住，导致推进力只能往左或者往右，因此无人艇不能沿直线航行，而只能进行向左或者向右前方运行。在这种情况下，完成航行轨迹跟踪任务十分艰巨，安全返港的可能性极低。
3.因此，设计一种无人艇存在航向角单调性约束情况下的容错控制方法具有重大意义。


技术实现要素：

4.针对现有技术的以上缺陷或改进需求，本发明提供了一种基于路径规划的无人艇容错控制方法及其控制系统，其目的在于解决具有单调航向约束故障无人艇的容错控制问题。
5.为实现上述目的，按照本发明的一个方面，提供了一种基于路径规划的无人艇容错控制方法，包括：
6.步骤s1：判断无人艇是否出现故障，所述故障为无人艇不能直线形式、只能朝一侧转向且只能前进不能后退，若是，执行步骤s2，若否，重复步骤s1；
7.步骤s2：根据无人艇的动力学模型和故障信息，规划故障起始位置至目标位置的目标轨迹集，所述目标轨迹集包括若干个交替的前向半圆轨迹和转向半圆轨迹，前向半圆轨迹的半径大于转向半圆轨迹的半径，以前向半圆轨迹与转向半圆轨迹的轨迹切换位置作为目标点且前向半圆轨迹与转向半圆轨迹在目标点处相切，所有目标点位于故障起始位置指向目标位置的直线上；
8.步骤s3：控制无人艇对所述目标轨迹集进行跟踪，当无人艇到达目标点时，将下一半圆轨迹作为新的目标轨迹进行追踪，直至完成对目标轨迹集中所有目标轨迹的跟踪。
9.在其中一个实施例中，所述无人艇的动力学模型为：
[0010][0011][0012]
[0013][0014][0015][0016]
其中，(x,y)表示无人艇的位置坐标，ψ表示无人艇的航向，w,v,r分别表示纵向速度、横荡速度和角速度，τu、τr分别表示推进器功率和转向舵角， d1,d2,d3分别为纵荡、横荡和艏摇时水动力阻尼系数，m1,m2,m3分别为无人艇在横向、纵向和航向上的附加质量效应的惯性质量。
[0017]
在其中一个实施例中，所述无人艇的故障信息表示为：
[0018][0019]
分别为追踪目标轨迹时的推进器功率和转向舵角，其中0≤τu≤τ
umax
，τ
umax
为推进器可达到的最大功率，当0《τ
rmin
≤τr≤τ
rmax
，表示无人艇只能朝右转向，(τ
rmin
,τ
rmax
)为朝右转向的转向角度区间，当τ’rmin
≤τr≤τ’rmax
《0，表示无人艇只能朝左转向，(τ’rmin
,τ’rmax
) 为朝左转向的转向角度区间。
[0020]
在其中一个实施例中，每一个目标轨迹以前一个目标点为起点并以后一个目标点为终点，当无人艇位于目标轨迹的起点时，当前目标轨迹的终点设置于无人艇的可达区域内，定义任一个目标轨迹sr(η0,η
t
)，η0＝(x0,y0,ψ0)为目标轨迹sr的起点坐标和航向，η
t
＝(x
t
,y
t
,ψ
t
)为目标轨迹sr的终点坐标和航向，当无人艇位于起点η0时，对应终点满足的条件为：
[0021][0022][0023]
其中为可达区域内的η
t
离η0的最远位置，为纵向速度的预定上界，为横荡速度的预定上界，r为角速度的预定下界。
[0024]
在其中一个实施例中，对于任意一个目标轨迹sr(η0,η
t
)，η0＝ (x0,y0,ψ0)为每个目标轨迹sr的起点，η
t
＝(x
t
,y
t
,ψ
t
)为每个目标轨迹sr的终点，每条目标轨迹sr(η0,η
t
)满足：
[0025][0026][0027]
ψr＝ψ0+rrt
[0028][0029][0030]
[0031][0032][0033][0034]
其中，(xr,yr,ψr)为目标轨迹sr上的任一点的目标值，上的任一点的目标值，为目标轨迹sr的轨迹半径，wr、vr、rr分别为无人艇在目标轨迹sr上的纵向速度、横荡速度和角速度的目标值,r为角速度的预定下界，为角速度的预定上界。
[0035]
在其中一个实施例中，所述目标轨迹集包括nr个目标点，目标点集合其中，η
ri
表示第i个目标点，所述目标点集合满足：
[0036]
p
ri
＝(x
ri
,y
ri
)
[0037][0038][0039][0040][0041]
其中，(x
ri
,y
ri
)表示第i个目标点的坐标，(xm,ym)表示故障起始位置， (xf,yf)表示目标位置，d
rf
为前向半圆轨迹的直径，d
rt
为转向半圆轨迹的直径，d
rf
》d
rt
，ψ
ri
为无人艇于第i个目标点的航向，wr、vr分别为无人艇在以第i个目标点为终点的目标轨迹sr上的纵向速度、横荡速度的目标值；
[0042]
且
[0043][0044]
其中，floor(.)为向下取整函数。
[0045]
在其中一个实施例中，所述目标轨迹集包括nr+1个目标轨迹，目标轨迹集且s
ri
＝sr(η
r(i-1)
,η
ri
)，表示第 i个目标轨迹的起点为η
r(i-1)
、终点为η
ri
，表示最后一个目标轨迹的起点为终点为目标位置。
[0046]
在其中一个实施例中，步骤s3包括，根据无人艇的动力学误差系统，利用无人艇的轨迹跟踪控制律控制无人艇对所述目标轨迹集进行跟踪，其中，
[0047]
所述无人艇的动力学误差系统表示为ξe＝[xe,ye,ψe,we,ve,re]，满足：
[0048][0049][0050][0051][0052][0053][0054]
其中，xe＝x
r-x，ye＝y
r-y，ψe＝ψ
r-ψ，we＝w
r-w，ve＝v
r-v， re＝r
r-r；
[0055][0056]
所述无人艇的轨迹跟踪控制律为：
[0057][0058][0059]
其中，a1、a2、a3、a4、a5、a6为控制律的控制参数，并满足
[0060]
a1》d
2-d1[0061][0062][0063]
a4》0
[0064]
a5》-d3[0065]
a6》0
[0066]
在其中一个实施例中，步骤s3中，无人艇的容错控制步骤包括：
[0067]
步骤s31：确定当前目标轨迹；
[0068]
步骤s32：控制无人艇跟踪当前目标轨迹，并在跟踪期间计算无人艇当前位置与目标轨迹终点的距离，判断距离是否小于预设误差值，若否，则表示当前目标轨迹的跟踪未完成，重复执行步骤s32；若是，则表示当前目标轨迹的跟踪已完成，执行步骤s33；
[0069]
步骤s33：判断目标轨迹集中的目标轨迹是否全部跟踪完，若否，则以下一目标轨迹作为进行跟踪的当前目标轨迹并跳转至步骤s32，若是，则表示到达目标位置，结束跟踪。
[0070]
按照本发明的另一方面，提供了一种基于路径规划的无人艇容错控制系统，包括：
[0071]
故障识别单元，用于判断无人艇是否出现故障，所述故障为无人艇不能直线形式、只能朝一侧转向且只能前进不能后退；
[0072]
轨迹规划单元，用于根据无人艇的动力学模型和故障信息，规划故障起始位置至目标位置的目标轨迹集，所述目标轨迹集包括若干个交替的前向半圆轨迹和转向半圆轨迹，前向半圆轨迹的半径大于转向半圆轨迹的半径，以前向半圆轨迹与转向半圆轨迹的轨迹切换位置作为目标点且前向半圆轨迹与转向半圆轨迹在目标点处相切，所有目标点位于故障起始位置指向目标位置的直线上；
[0073]
轨迹跟踪单元，用于控制无人艇对所述目标轨迹集进行跟踪，当无人艇到达目标点时，将下一半圆轨迹作为新的目标轨迹进行追踪，直至完成对目标轨迹集中所有目标轨迹的跟踪。
[0074]
本技术针对无人艇的控制输入始终被限制在正区间或者是负区间的故障，提出了相应的容错控制方法，通过规划交替的前向半圆轨迹和转向半圆轨迹，形成从起始位置到目标位置的目标轨迹集，控制无人艇对所述目标轨迹集进行切换和跟踪，提高了无人艇容错控制的灵活性和可行性，实现了在如果双体船的一侧失去动力或舵机卡住、航向角固定、无人艇不能沿直线航行而只能进行螺旋运动的场景下的无人艇装置的容错控制和轨迹跟踪，为无人艇在故障场景下的容错控制和轨迹跟踪提供了新的思路，提高了无人艇在执行任务时的可靠性，为无人艇的应用开拓了新的前景。
附图说明
[0075]
图1为一实施例的基于路径规划的无人艇容错控制方法的步骤流程图；
[0076]
图2为一实施例的规划出的目标轨迹集的示意图；
[0077]
图3为一实施例的无人艇运行期间的状态分解示意图。
具体实施方式
[0078]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0079]
如图1为一实施例中的基于路径规划的无人艇容错控制方法的步骤流程图，该控制方法包括：
[0080]
步骤s100：判断无人艇是否出现故障，所述故障为无人艇不能直线形式、只能朝一侧转向且只能前进不能后退。
[0081]
当无人艇出现故障时，执行步骤s2，当无人艇未出现故障时，重复步骤s100。
[0082]
其中，故障为无人艇不能直线形式、只能朝一侧转向且只能前进不能后退，即无人艇的控制输入始终被限制在正区间或者是负区间。
[0083]
步骤s200：根据无人艇的动力学模型和故障信息，规划故障起始位置至目标位置
的目标轨迹集，所述目标轨迹集包括若干个交替的前向半圆轨迹和转向半圆轨迹。
[0084]
其中，前向半圆轨迹的半径大于转向半圆轨迹的半径，以前向半圆轨迹与转向半圆轨迹的轨迹切换位置作为目标点且前向半圆轨迹与转向半圆轨迹在目标点处相切，所有目标点位于故障起始位置指向目标位置的直线上。
[0085]
如图2的实施例中规划的从故障起始位置η
r0
＝(xm,ym)至目标位置的目标轨迹集其包括nr+1条目标轨迹。其中，s
ri
表示第i条目标轨迹，第i条目标轨迹可以表示为s
ri
＝sr(η
r(i-1)
,η
ri
)，即第i条目标轨迹s
ri
为有目标点η
r(i-1)
至目标点η
ri
的半圆轨迹。目标轨迹的数量为奇数，其中，第奇数条目标轨迹为前向半圆轨迹(轨迹的终点位于靠近目标位置的一侧)，第偶数条目标轨迹为转向半圆轨迹(轨迹的终点位于背离目标位置的一侧)，由此构成前向半圆轨迹-转向半圆轨迹
……‑
前向半圆轨迹的nr+1条交替轨迹。其中，所有的目标点(包括故障起始位置和目标位置)位于一条直线上。前向半圆轨迹和转向半圆轨迹在目标点出相切。
[0086]
在无人艇的容错控制中，基于无人艇的动力学获得符合无人艇模型的目标轨迹、轨迹生成规则和轨迹跟踪控制律，如图3，在纵荡、横荡和艏摇三个自由度上考虑无人艇的三自由度动力学模型。
[0087]
在一实施例中，无人艇的动力学模型为：
[0088][0089][0090][0091][0092][0093][0094]
其中，(x,y)表示无人艇的位置坐标，ψ表示无人艇的航向，w,v,r分别表示纵向速度、横荡速度和角速度，τu、τr分别表示推进器功率和转向舵角， d1,d2,d3分别为纵荡、横荡和艏摇时水动力阻尼系数，m1,m2,m3分别为无人艇在横向、纵向和航向上的附加质量效应的惯性质量。
[0095]
具体的，可以根据实际情况对k1～k7赋值，在一具体的实施例中，赋值后的无人艇的动力学模型为：
[0096][0097][0098][0099][0100]
[0101][0102]
在一实施例中，所述无人艇的故障信息表示为：
[0103][0104]
分别为追踪目标轨迹时的推进器功率和转向舵角，其中 0≤τu≤τ
umax
，τ
umax
为推进器可达到的最大功率，当0《τ
rmin
≤τr≤τ
rmax
，表示无人艇只能朝右转向，(τ
rmin
,τ
rmax
)为朝右转向的转向角度区间，当τ’rmin
≤τr≤τ’rmax
《0，表示无人艇只能朝左转向，(τ’rmin
,τ’rmax
) 为朝左转向的转向角度区间。
[0105]
例如，无人艇的执行器故障饱和为：
[0106]
0≤τu≤4000,2≤τr≤20
[0107]
表示无人艇的推进器功率小于4000，且无人艇只能朝右转向且转向角度为2～20。
[0108]
在一实施例中，对于每一个目标轨迹以前一个目标点为起点并以后一个目标点为终点，当无人艇位于目标轨迹的起点时，当前目标轨迹的终点设置于无人艇的可达区域内，定义任一个目标轨迹sr(η0,η
t
)，η0＝ (x0,y0,ψ0)为目标轨迹sr的起点坐标和航向，η
t
＝(x
t
,y
t
,ψ
t
)为目标轨迹sr的终点坐标和航向，当无人艇位于起点η0时，对应终点满足的条件为：
[0109][0110][0111]
其中为可达区域内的η
t
离η0的最远位置，为纵向速度的预定上界，为横荡速度的预定上界，r为角速度的预定下界。
[0112]
在一实施例中，对于任意一个目标轨迹sr(η0,η
t
)，η0＝(x0,y0,ψ0)为每个目标轨迹sr的起点，η
t
＝(x
t
,y
t
,ψ
t
)为每个目标轨迹sr的终点，每条目标轨迹sr(η0,η
t
)满足：
[0113][0114][0115][0116][0117][0118][0119][0120][0121]
[0122]
其中，(xr,yr,ψr)为目标轨迹sr上的任一点的目标值，上的任一点的目标值，为目标轨迹sr的轨迹半径，wr、vr、rr分别为无人艇在目标轨迹sr上的纵向速度、横荡速度和角速度的目标值,r为角速度的预定下界，为角速度的预定上界。
[0123]
在一实施例中，根据故障起始位置η
r0
＝(xm,ym)至目标位置规划出一系列目标点目标点集合满足：
[0124]
p
ri
＝(x
ri
,y
ri
)
[0125][0126][0127][0128][0129]
其中，(x
ri
,y
ri
)表示第i个目标点的坐标，(xm,ym)表示故障起始位置， (xf,yf)表示目标位置，d
rf
为前向半圆轨迹的直径，d
rt
为转向半圆轨迹的直径，d
rf
》d
rt
，ψ
ri
为无人艇于第i个目标点的航向，wr、vr分别为无人艇在以第i个目标点为终点的目标轨迹sr上的纵向速度、横荡速度的目标值。
[0130]
其中，
[0131][0132]
其中，floor(.)为向下取整函数。
[0133]
基于上述规划出的目标点，规划出包括nr+1个目标轨迹的目标轨迹集且表示第i个目标轨迹的起点为η
r(i-1)
、终点为η
ri
，表示最后一个目标轨迹的起点为终点为目标位置。
[0134]
在规划出目标轨迹集后，执行
[0135]
步骤s300：控制无人艇对所述目标轨迹集进行跟踪，当无人艇到达目标点时，将下
一半圆轨迹作为新的目标轨迹进行追踪，直至完成对目标轨迹集中所有目标轨迹的跟踪。
[0136]
具体的，目标轨迹集包括nr+1个目标轨迹，无人艇依次对里面的目标轨迹进行追踪，当当无人艇到达目标点时，将下一半圆轨迹作为新的目标轨迹进行追踪。
[0137]
具体的，根据无人艇的动力学误差系统，利用无人艇的轨迹跟踪控制律控制无人艇对所述目标轨迹集进行跟踪。
[0138]
其中，所述无人艇的动力学误差系统表示为ξe＝[xe,ye,ψe,we,ve,re]，满足：
[0139][0140][0141][0142][0143][0144][0145]
其中，xe＝x
r-x，ye＝y
r-y，ψe＝ψ
r-ψ，we＝w
r-w，ve＝v
r-v， re＝r
r-r，其中，带下标r的参数表示轨迹规划的目标值，而不带下标r的表示无人艇的实际状态值。
[0146]
在一实施例中，所述无人艇的轨迹跟踪控制律为：
[0147][0148][0149]
其中，a1、a2、a3、a4、a5、a6为控制律的控制参数，并满足
[0150]
a1》d
2-d1[0151][0152][0153]
a4》0
[0154]
a5》-d3[0155]
a6》0
[0156]
在实际追踪期间，根据以下步骤进行追踪：
[0157]
步骤s310：确定当前目标轨迹；
[0158]
步骤s320：控制无人艇跟踪当前目标轨迹，并在跟踪期间计算无人艇当前位置与目标轨迹终点的距离，判断距离是否小于预设误差值，若否，则表示当前目标轨迹的跟踪未完成，重复执行步骤s320；若是，则表示当前目标轨迹的跟踪已完成，执行步骤s330；
[0159]
步骤s330：判断目标轨迹集中的目标轨迹是否全部跟踪完，若否，则以下一目标轨
迹作为进行跟踪的当前目标轨迹并跳转至步骤s320，若是，则表示到达目标位置，结束跟踪。
[0160]
其代码表示如下：
[0161]
步骤1：规划出一系列目标点并设置目标点为轨迹间的切点规划出轨迹集
[0162]
步骤2：for i＝0:nr+1；(按顺序跟踪目标轨迹)
[0163]
步骤3：
ꢀꢀꢀꢀꢀ
while true；
[0164]
步骤4：
ꢀꢀꢀꢀꢀꢀꢀꢀ
利用控制器τu和τr控制无人艇跟踪目标轨迹s
ri
；
[0165]
步骤5：
ꢀꢀꢀꢀꢀꢀꢀꢀ
if||η-η
ri
||《∈；(计算当前状态与目标点状态的差值)
[0166]
步骤6：
ꢀꢀꢀꢀꢀꢀꢀ
break；
[0167]
步骤7：
ꢀꢀꢀꢀꢀ
end；
[0168]
步骤8：
ꢀꢀꢀꢀꢀ
end；
[0169]
步骤9：end。
[0170]
为了到达指定目标，利用轨迹跟踪控制器τu和τr控制无人艇跟踪目标轨迹s
r1
，并不断判断当前状态η与第一个目标点η
r1
差的范数||η-η
r1
||。当误差范数小于一定值的时候，将目标轨迹切换为s
r2
，并判断当前状态与第二个目标点差的范数，以此不断切换目标轨迹和目标点，如图2。最终，无人艇通过设计的轨迹跟踪控制律实现对于若干前向轨迹和转向轨迹的跟踪，到达目标点，实现了返港任务，如图2。
[0171]
相应的，本技术还涉及一种无人艇容错控制系统，其包括：
[0172]
故障识别单元，用于判断无人艇是否出现故障，所述故障为无人艇不能直线形式、只能朝一侧转向且只能前进不能后退；
[0173]
轨迹规划单元，用于根据无人艇的动力学模型和故障信息，规划故障起始位置至目标位置的目标轨迹集，所述目标轨迹集包括若干个交替的前向半圆轨迹和转向半圆轨迹，前向半圆轨迹的半径大于转向半圆轨迹的半径，以前向半圆轨迹与转向半圆轨迹的轨迹切换位置作为目标点且前向半圆轨迹与转向半圆轨迹在目标点处相切，所有目标点位于故障起始位置指向目标位置的直线上；
[0174]
轨迹跟踪单元，用于控制无人艇对所述目标轨迹集进行跟踪，当无人艇到达目标点时，将下一半圆轨迹作为新的目标轨迹进行追踪，直至完成对目标轨迹集中所有目标轨迹的跟踪。
[0175]
该无人艇容错控制系统用于实现上述控制方法，具体细节参考上文介绍，在此不再赘述。
[0176]
本技术针对无人艇的控制输入始终被限制在正区间或者是负区间的故障，提出了相应的容错控制方法，通过规划交替的前向半圆轨迹和转向半圆轨迹，形成从起始位置到目标位置的目标轨迹集，控制无人艇对所述目标轨迹集进行切换和跟踪，提高了无人艇容错控制的灵活性和可行性，实现了在如果双体船的一侧失去动力或舵机卡住、航向角固定、无人艇不能沿直线航行而只能进行螺旋运动的场景下的无人艇装置的容错控制和轨迹跟踪，为无人艇在故障场景下的容错控制和轨迹跟踪提供了新的思路，提高了无人艇在执行任务时的可靠性，为无人艇的应用开拓了新的前景。
[0177]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。