一种基于Bezier曲线的无人艇靠泊路径规划方法

一种基于bezier曲线的无人艇靠泊路径规划方法
技术领域
1.本发明涉及航线规划的技术领域，具体而言，涉及一种基于bezier曲线的无人艇靠泊路径规划方法。


背景技术：

2.自主靠泊是实现自主航行的“最后一公里”，对实现从泊位到泊位的远距离自主航行具有重要意义。现有基于bezier曲线的轨迹规划方法多用于无人车领域，而这类方法仅考虑满足车辆的动力学约束和拟合出的bezier曲线的严格曲率连续性，且不考虑大惯性作用。无人艇靠泊工况难点在于：(1)约束条件多，泊位信息复杂多变，而车辆泊位一般具有较为规范的大小形状等；(2)靠泊最终期望航速为0kn，此时速度低舵效差，控制难度大，相比之下车辆可以在低速情况仍具有较好的控制能力；(3)依赖于惯导或北斗等设备定位导航，其自身存在误差；(4)泊位狭小，对控制精度要求高。现有靠泊路径规划方法存在曲率不连续等问题，会导致角速度不连续。无人艇角速度不能跳变，因此易产生跟踪误差，进而使得无人艇偏离期望航线。研究表明bezier曲线具有曲率连续及便于考虑约束的特性，故研究用于无人艇的基于bezier曲线靠泊路径规划方法以降低无人艇靠泊的控制难度。


技术实现要素：

3.本发明的目的在于：保证了无人艇在靠泊时能够安全有效的进行路径规划，给出一个适宜控制的路径。
4.本发明的技术方案是：提供了一种基于bezier曲线的无人艇靠泊路径规划方法，该方法包括以下步骤：
5.s1、根据泊位信息规划第一段bezier曲线，将泊位中的期望停船位置设置为控制点p4，控制点p4的位置坐标为(x
p4
，y
p4
)，以控制点p4为起点垂直泊位向外取一段距离的位置设置为控制点p3，控制点p3的位置坐标为(xg，yg)，控制点p3和控制点p4形成一段一阶bezier曲线，用曲线参数t表达为
6.s2、将无人艇的初始位置设置为控制点p0，控制点p0的位置坐标为(x0，y0)；规划第二段bezier曲线，计算需要的剩余控制点p1和p2，其中控制点p1的位置坐标表示为(x
p1
，y
p1
)，控制点p2的位置坐标表示为(x
p2
，y
p2
)，将以控制点p0至p3唯一确定的三阶bezier曲线表示为：
[0007][0008]
计算满足无人艇运动学约束条件ρ(t)≥ρ
min
的控制点p1和控制点p2的位置坐标，其中ρ(t)是三阶bezier曲线任意一处的曲率半径，ρ
min
是无人艇在对应位置的最小回转半径；
[0009]
s3、判断控制点p1和控制点p2是否计算完毕，若未完成则返回步骤s2，若完成则以
控制点p0至p3形成第二段三阶bezier曲线，至此路径规划完毕，步骤s1与步骤s3得到的bezier曲线组合后即为规划得到的路径。
[0010]
上述任一项技术方案中，进一步地，步骤s1中控制点p4与控制点p3之间的距离为一倍船长。
[0011]
上述任一项技术方案中，进一步地，步骤s2中计算控制点p1和控制点p2的位置坐标具体包括：
[0012]
s21、求出控制点p1、p2位置坐标与控制点p0、p3位置坐标的关系式，对三阶bezier曲线表达式求二阶导数得：
[0013][0014]
无人艇位于控制点p0时还未开始打舵，位于控制点p3时舵角复原为零度，两点的漂角均为0，速度方向与艏向一致，故三阶bezier曲线起始点p0与终点p3受到非完整约束：
[0015][0016]
满足上述约束条件的控制点p1和控制点p2表示为：
[0017][0018]
其中，ψ0为开始靠泊时无人艇的艏向，ψg为无人艇停泊在泊位时的艏向，d1为控制点p0与控制点p1之间的距离，d2为控制点p2与控制点p3之间的距离；
[0019]
s22、计算d1和d2，将三阶bezier曲线任意一处的曲率半径表示为：
[0020][0021]
将d1与d2离散化，取定初值和离散间隔，同时确定曲线参数变量t的离散间隔和迭代次数，根据ρ(t)计算式依次计算每个d1，d2和t对应的ρ(t)，判断此时ρ(t)是否满足条件ρ(t)≥ρ
min
，若满足条件，则内层循环进入下一次迭代，迭代次数增加1次，对t增加对应的离散间隔，若不能满足条件则不计此次的迭代次数，在内层循环计算达到迭代次数上限后进入中层循环，中层循环中d2按照离散间隔逐步增加至上限，对每一个d2进行相应内层循环的计算，在中层循环d2计算完毕后进入最外层循环进行d1的迭代计算，最外层循环中d1按照离散间隔逐步增加至上限，并对每一个d1进行相应中层和内层循环的计算，
[0022]
在内层、中层和最外层循环都结束后，得到具有足够精度的d1和d2，根据控制点p0、d1、控制点p3和d2即可确定控制点p1和控制点p2的位置坐标。
[0023]
上述任一项技术方案中，进一步地，步骤s1得到的一阶bezier曲线和步骤s3得到的三阶bezier曲线在控制点p3处相连。
[0024]
上述任一项技术方案中，进一步地，步骤s2中控制点p1、p2的选取不能超过港池水域范围。
[0025]
上述任一项技术方案中，进一步地，在步骤s1之前还包括：
[0026]
s0、确认无人艇的初始位置，通过激光雷达感知靠泊区域的他船航行状况，判断靠泊区域内无障碍物时，则继续进行，否则中止规划继续待机；基于激光雷达确定泊位信息，对激光雷达返回的点云信息进行聚类，从而得到泊位的位置信息。
[0027]
本发明的有益效果是：
[0028]
1)在无人艇靠泊路径规划中采用bezier曲线，使规划的靠泊路径更为光顺，在计算控制点的过程中，处于水中的无人艇无法如陆上车辆一样通过轮胎与地面的摩擦力使车身相对于后轮转动而快速改变前轮指向，从而使拟合的泊车曲线更容易满足汽车泊车条件。相对于无人艇，本发明的方法提出需满足水中运动学约束条件ρ(t)≥ρ
min
，即生成的三阶bezier曲线任意一处的曲率半径需大于或等于无人艇在对应位置的最小回转半径，这样有效降低无人艇靠泊工况的控制难度，提高靠泊成功率和效率；
[0029]
2)本发明的方法还结合了具体泊位信息进行控制点计算，将路径分为两部分，其中一段使用期望停船位置控制点p4和由控制点p4计算出的另一个控制点p3形成一阶bezier曲线，避免对bezier曲线的循迹造成的频繁打舵和低速控制难问题，同时考虑了停船需要的缓冲距离。
附图说明
[0030]
本发明的上述和附加方面的优点在结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0031]
图1是根据本发明的一个实施例的基于bezier曲线的无人艇靠泊路径规划方法的路径规划示意图；
[0032]
图2是根据本发明的一个实施例的基于bezier曲线的无人艇靠泊路径规划方法的流程示意图；
[0033]
图3是根据本发明的一个实施例的基于bezier曲线的无人艇靠泊路径规划方法的举例计算示意图。
具体实施方式
[0034]
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互结合。
[0035]
在下面的描述中，阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
[0036]
如图1和图2所示，本实施例提供了一种基于bezier曲线的无人艇靠泊路径规划方法，该方法包括：
[0037]
s0、确认无人艇的初始位置，通过激光雷达感知靠泊区域的他船航行状况，判断靠泊区域内无障碍物时，则继续进行，否则中止规划继续待机。
[0038]
其中，考虑到无人艇进港位置不确定性，目前港池内避碰难度大，需要停船观察待安全后再靠泊，将停船观察的位置即无人艇的初始位置；在初始位置处，无人艇使用激光雷达扫描靠泊区域，当雷达感知到其他船只或障碍物在靠泊区域航行时，无人艇在初始位置
处继续待机，直至可以安全停泊时，再接着规划路径。
[0039]
基于激光雷达确定泊位信息，对激光雷达返回的点云信息进行聚类，从而得到泊位的位置信息。
[0040]
规划bezier曲线需要确定曲线的控制点，其中路径的开始和结尾各是一个控制点，所以需要得到泊位的具体位置信息。
[0041]
s1、根据泊位信息规划第一段bezier曲线，如图1所示，将泊位中的期望停船位置设置为控制点p4，控制点p4的位置坐标为(x
p4
，y
p4
)，以控制点p4为起点垂直泊位向外取一倍船长距离的位置设置为控制点p3，控制点p3的位置坐标为(xg，yg)，控制点p3和控制点p4形成一段一阶bezier曲线，用曲线参数t表达为
[0042]
具体地，根据经验所得，无人艇在到达泊位时的末端航速为1kn，此时的舵效几乎为零，为避免对bezier曲线的循迹造成的频繁打舵和低速控制难问题，同时考虑到停船需要的缓冲距离，故以控制点p4为起点垂直泊位形成一段一阶bezier曲线，曲线的另一端即为控制点p3。这样对控制点p3和控制点p4形成的一阶bezier曲线，也就是直线p3p4进行循迹控制就不需要对艏向进行频繁调整，速度调整也较为容易，控制难度较低。
[0043]
s2、将无人艇的初始位置设置为控制点p0，控制点p0的位置坐标为(x0，y0)。规划第二段bezier曲线，计算需要的控制点p1和控制点p2，其中控制点p1的位置坐标表示为(x
p1
，y
p1
)，控制点p2的位置坐标表示为(x
p2
，y
p2
)，具体的计算步骤包括：
[0044]
s21、将以控制点p0至p3唯一确定的三阶bezier曲线表示为：
[0045][0046]
对上式求二阶导数得：
[0047][0048]
无人艇位于控制点p0时还未开始打舵，位于控制点p3时舵角复原为零度，两点的漂角均为0，速度方向与艏向一致，故三阶bezier曲线起始点p0与终点p3受到非完整约束：
[0049][0050]
满足上述约束条件的控制点p1和控制点p2表示为：
[0051][0052]
其中，ψ0为开始靠泊时无人艇的艏向，ψg为无人艇停泊在泊位时的艏向，即泊位开口的朝向的反方向，d1为控制点p0与控制点p1之间的距离，d2为控制点p2与控制点p3之间的距离。s22、计算第二段三阶bezier曲线任意一处的曲率半径：
[0053][0054]
考虑无人艇的运动特性，ρ(t)应满足ρ(t)≥ρ
min
，其中ρ
min
是无人艇的最小回转半径。
[0055]
具体地，考虑到曲线具有凸包性，该bezier曲线始终会在包含了所有控制点的最小凸多边形内部，以此可以得到相应的d1和d2的限制范围。
[0056]
将d1与d2离散化，取定合适的初值和离散间隔，同时确定曲线参数变量t的离散间隔和迭代次数，根据ρ(t)计算式依次计算每个d1，d2和t对应的ρ(t)，判断此时ρ(t)是否满足条件ρ(t)≥ρ
min
，若满足条件，则内层循环进入下一次迭代，迭代次数增加1次，对t增加对应的离散间隔，若不能满足条件则不计此次的迭代次数。
[0057]
在内层循环计算达到迭代次数上限后进入中层循环，中层循环中d2按照离散间隔逐步增加至上限，对每一个d2进行相应内层循环的计算，在中层循环d2计算完毕后进入最外层循环进行d1的迭代计算，最外层循环中d1按照离散间隔逐步增加至上限，并对每一个d1进行相应中层和内层循环的计算，在内层、中层和最外层循环都结束后，得到具有足够精度的d1和d2，根据控制点p0、d1、控制点p3和d2即可确定控制点p1和控制点p2。
[0058]
由三阶bezier曲线的定义及ρ(t)的计算式可知控制点对其附近的曲线段曲率影响较大，考虑到不应该把艏向调整的任务放在靠近目标泊位时完成，因此曲线在靠近末端，也就是控制点p0和控制点p3处的曲率不宜过大，故d1、d2应适当取小。因此d1和d2的取值应该从小到大变化。
[0059]
s3、判断控制点p1和控制点p2是否计算完毕，若未完成则返回步骤s2，若完成则以控制点p0至p3形成第二段三阶bezier曲线，至此路径规划完毕，步骤s1与步骤s3得到的bezier曲线组合后即为规划得到的路径。
[0060]
在本发明的另一个实施例中，在某码头的一个固定泊位对上述方法进行实际测试。
[0061]
如图3所示，获取到的泊位信息如下：该固定泊位为10m
×
10m一面开口的矩形泊位，四个顶点的位置用经纬度坐标描述，从开口一端开始逆时针编号0、1、2、3号点，如下表1所示。
[0062]
表1泊位顶点坐标
[0063]
顶点编号经度纬度0119.55429235.4022211119.55427335.4023102119.55416535.4022943119.55418335.402205
[0064]
此外，无人艇长15m、宽4.8m，取0号顶点和3号顶点连线的中点作为控制点p4，即可保证无人艇最终停泊在泊位正中央，且与左右各保持2.6m间隙的同时，与前方保留2.5m间隙，接着以控制点p4为起点垂直泊位向外取一倍船长距离，也就是30m距离的位置设置为控制点p3。
[0065]
在本次停泊过程中，无人艇在直线路段的期望航速为1kn，靠泊工况的初始状态为
(119.555999e，35.400849n，312.17
°
，0kn)，控制点p0就在此初始状态的坐标上，目标状态为(119.554236e，35.402212n，350.13
°
，1kn)，控制点p3就在此目标状态的坐标上，无人艇在曲线路段的期望航速为3kn，最大舵角取为15
°
，代入由参数辨识得到的操纵响应模型得到最小回转直径为24.76m，即最小曲率半径ρ
min
＝12.38m。接着应用迭代算法求d1和d2，考虑到bezier曲线的控制点将曲线限制在凸多边形内部，因此控制点的选取不能超过港池水域范围，港池水域纵向最长边为227m，因此0＜d1≤227m、0＜d2≤227m；取d1离散化间隔为5m，即d1取值为5、10、15
…
225m，为降低循环次数，取d2离散化间隔为10m，即d2取值为10、20、30
…
220m。
[0066]
将t取值离散化为1000份，即t取值为0.001、0.002、0.003...1，之后根据ρ(t)计算式依次计算每个d1、d2和t对应的ρ(t)，判断此时ρ(t)是否满足无人艇运动学约束条件ρ(t)≥ρ
min
，若满足条件，则内层循环进入下一次迭代，迭代次数增加1次，t增加0.001，若不能满足条件则不计此次的迭代次数。
[0067]
输出最终得出的d1和d2，写出控制点p1和控制点p2的坐标，使用所求得的控制点p0至p3代入三阶bezier曲线、控制点p3和p4代入一阶bezier曲线，将两段曲线组合，得到所需的靠泊路径。
[0068]
以上结合附图详细说明了本发明的技术方案，本发明提出了一种基于bezier曲线的无人艇靠泊路径规划方法，确定控制点p4后求出控制点p3，再通过控制点p0和控制点p3计算控制点p1和控制点p2。
[0069]
通过两控制点p3、p4规划了一段一阶bezier曲线，通过四控制点p0至p3规划了一段三阶bezier曲线，将两段曲线组合，即为靠泊路径。
[0070]
本发明中的步骤可根据实际需求进行顺序调整、合并和删减。
[0071]
尽管参考附图详地公开了本发明，但应理解的是，以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围。本发明的保护范围由附加权利要求限定，并可包括在不脱离本发明保护范围和精神的情况下针对发明所作的各种变型、改型及等效方案。