【技术实现步骤摘要】
本专利技术涉及运动控制，尤其涉及一种水下航行器路径跟踪控制方法及装置。
技术介绍
1、目前，水下环境路径跟踪控制问题已经过较为长期的研究，并对于海洋环境快速搜索与救援等任务场景具有重要的应用意义。
2、相关技术中，对于具有高机动高推进性能的水下航行器，例如机器鱼，当前研究关注重点为如何优化并进一步提升系统的游动性能，对于高机动水下环境路径跟踪控制仍较少，同时，机器鱼存在着欠驱动和输入耦合的特性，导致对机器鱼进行水下跟踪的效率低，影响水下作业。
技术实现思路
1、本专利技术提供一种水下航行器路径跟踪控制方法及装置，用以解决现有技术中水下航行器的缺陷存在着欠驱动和输入耦合的特性，导致水下跟踪水下航行器的效率低，提高了水下跟踪水下航行器的效率和准确率。
2、本专利技术提供一种水下航行器路径跟踪控制方法，包括：
3、采集水下航行器的位置数据和速度数据；
4、根据目标视线导航策略对参考路径和所述位置数据进行处理，得到目标航向角；根据所述目标航向角、所述位置数据和所述速度数据对具有切换能力的非线性模型预测控制器进行最优值求解，得到所述水下航行器的最优关节偏置参数和最优关节摆动频率；
5、其中，具有切换能力的非线性模型预测控制器的优化目标是基于所述水下航行器的多种运动模态实现平面高机动路径跟随控制任务，所述具有切换能力的非线性模型预测控制器的代价函数基于状态误差权重矩阵、控制输入权重矩阵和中断状态误差权重矩阵确定；
6、根据
7、根据本专利技术提供的一种水下航行器路径跟踪控制方法，所述根据目标视线导航策略对参考路径和所述位置数据进行处理，得到目标航向角包括：
8、根据所述参考路径和所述位置数据确定所述水下航行器的前视交点；
9、根据所述前视交点和所述位置数据确定所述水下航行器的跟踪误差，并采用下式计算所述跟踪误差对应的目标航向角：
10、
11、其中，ψd为所述目标航向角，(xe，ye)为所述跟踪误差对应的坐标；sgn为符号函数。
12、根据本专利技术提供的一种水下航行器路径跟踪控制方法，所述代价函数通过下式表示：
13、
14、所述具有切换能力的非线性模型预测控制器的约束条件表示为：
15、subjectto:xi+1＝f(xi,ui),i＝0,i,np-1；
16、ui＝unc,i＝nc,i,np；
17、xi∈x,ui∈u；
18、其中，xd,i表示为第i个预测周期时的目标状态；xi为第i个预测周期时的实际状态，xi+1为第i+1个预测周期时的实际状态，ui为第i个预测周期时的控制量输入，xd,np为第np个预测周期式时的实际状态，i表示第i个预测周期；f(xi,ui)为所述水下航行器的动力学模型；np和nc分别代表预测时域和控制时域；x为状态约束集,u为控制约束集，l为状态代价函数,lt终端代价函数。
19、根据本专利技术提供的一种水下航行器路径跟踪控制方法，所述采集水下航行器的位置数据和速度数据包括：
20、通过所述水下航行器的运动学模型获取所述位置数据；通过所述水下航行器的动力学模型获取所述速度数据；
21、其中，运动学模型通过下式表示：
22、
23、
24、
25、所述动力学模型通过下式表示：
26、
27、
28、
29、vbx为系统前向速度，vby为系统横向速度，ψ为航向角度，为系统横坐标位置导数，为系统纵坐标导数，为系统航向角导数；mx,my,λz分别表示鱼体质量和转动惯量，包含有原始质量和附加质量部分，kx,ky,kτ分别表示非线性阻尼项；kt＝1/2mtl2表示尾鳍推力系数，mt＝1/4ρd2为尾鳍单位长度的虚拟质量，l表示尾鳍长度，ρ表示水密度，d表示尾鳍截面高度，kforce和kmoment分别表示为力和力矩的尺度因子；kforce为一个常数，kmoment为偏置角度α0的线性函数，rbt表示质心到尾鳍受力中心的距离。
30、根据本专利技术提供的一种，所述根据所述目标航向角、所述最优关节偏置参数和所述最优关节摆动频率对所述水下航行器的运动模式进行切换包括：
31、根据所述目标航向角和所述水下航行器的实际航向角之间的差值得到航向角误差；
32、在所述航向角误差大于误差阈值的情况下，以所述最优关节摆动频率为所述水下航行器的摆动频率，以所述最优关节偏置参数为所述水下航行器的关节偏置参数，并采用位置模式控制所述水下航行器输出具有偏置参数的正弦规律，直至所述航向角误差低于所述误差阈值。
33、根据本专利技术提供的一种，在所述根据所述目标航向角和所述水下航行器的实际航向角之间的差值得到航向角误差之后，所述方法还包括：
34、在所述航向角误差小于误差阈值的情况下，采用速度模式控制所述水下航行器输出无偏置参数的正弦规律，并控制所述水下航行器执行直线游动模式，直至航向角误差高于所述误差阈值。
35、本专利技术还提供一种水下航行器路径跟踪控制装置，包括：
36、数据采集模块，用于采集水下航行器的位置数据和速度数据；
37、计算模块，根据目标视线导航策略对参考路径和所述位置数据进行处理，得到目标航向角；根据所述目标航向角、所述位置数据和所述速度数据对具有切换能力的非线性模型预测控制器进行最优值求解，得到所述水下航行器的最优关节偏置参数和最优关节摆动频率；其中，具有切换能力的非线性模型预测控制器的优化目标是基于所述水下航行器的多种运动模态实现平面高机动路径跟随控制任务，所述具有切换能力的非线性模型预测控制器的代价函数基于状态误差权重矩阵、控制输入权重矩阵和中断状态误差权重矩阵确定
38、运动控制模块，用于根据所述目标航向角、所述最优关节偏置参数和所述最优关节摆动频率对所述水下航行器的运动模式进行切换。
39、本专利技术还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述水下航行器路径跟踪控制方法。
40、本专利技术还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述水下航行器路径跟踪控制方法。
41、本专利技术还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述水下航行器路径跟踪控制方法。
42、本专利技术提供的水下航行器路径跟踪控制方法及装置，通过导航策略对参考路径和位置数据进行处理，得到目标航向角；根据目标航向角、位置数据和速度数据对具有切换能力的非线性模型预测控制器进行最优值求解，得到水下航行器的最优关节偏置参本文档来自技高网...
【技术保护点】
1.一种水下航行器路径跟踪控制方法，其特征在于，包括：
2.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述根据目标视线导航策略对参考路径和所述位置数据进行处理，得到目标航向角包括：
3.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述代价函数通过下式表示：
4.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述采集水下航行器的位置数据和速度数据包括：
5.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述根据所述目标航向角、所述最优关节偏置参数和所述最优关节摆动频率对所述水下航行器的运动模式进行切换包括：
6.根据权利要求5所述的水下航行器路径跟踪控制方法，其特征在于，在所述根据所述目标航向角和所述水下航行器的实际航向角之间的差值得到航向角误差之后，所述方法还包括：
7.一种水下航行器路径跟踪控制装置，其特征在于，包括：
8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执
9.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述水下航行器路径跟踪控制方法。
10.一种计算机程序产品，包括计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述水下航行器路径跟踪控制方法。
...【技术特征摘要】
1.一种水下航行器路径跟踪控制方法，其特征在于，包括：
2.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述根据目标视线导航策略对参考路径和所述位置数据进行处理，得到目标航向角包括：
3.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述代价函数通过下式表示：
4.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述采集水下航行器的位置数据和速度数据包括：
5.根据权利要求1所述的水下航行器路径跟踪控制方法，其特征在于，所述根据所述目标航向角、所述最优关节偏置参数和所述最优关节摆动频率对所述水下航行器的运动模式进行切换包括：
6.根据权利要求5所述的水下航行器路径跟踪控制方法，其特征在...
【专利技术属性】
技术研发人员：周超，王霄飞，王健，范俊峰，尹昭然，朱春晖，张灼亮，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。