仿生滑翔机器海豚平面路径跟踪方法及系统
技术领域
本发明涉及水下机器人控制技术领域,特别涉及一种仿生滑翔机器海豚平面路径跟踪方法及系统。
背景技术
自然界,海洋生物经过漫长的自然进化,发展出卓越的游动能力。随着机器人技术的蓬勃发展,各类水下仿生平台相继出现,为仿生推进机理研究和工程技术开发提供了新的手段和途径。通过模仿鲸豚类生物,机器海豚凭借其高效的游动方式,展现出非凡的机动性。
近年来,为了提高机器海豚的续航能力,科研人员引入了浮力调节机制,发明了仿生滑翔机器海豚,使其兼具高机动和强续航的优势,拓展了实际应用的范围。
其中,路径跟踪问题一直以来都是水下机器人的研究热点,其目标是根据规划的路径生成一条期望曲线,使机器人能够从任一点出发,设计跟踪控制器,逐渐收敛于该曲线。路径跟踪技术是导航控制系统的重要组成部分,对于海洋作业的顺利完成有着重要的意义。
针对所述技术,已有许多方法广泛应用于水下机器人研究中。Fossen等人设计了视线导航法以最小化交叉跟踪误差,然后基于反步法(Backstepping,BP)完成了控制器设计,并通过实验验证了算法有效性。Wang等人提供了一种基于BP的跟踪控制器,用于仿生水下航行器圆周和直线跟踪,并通过模糊逻辑模型获得实际波动鳍的参数。Sun等人提出了一种比例-积分(Proportional-integral,PI)滑模控制(Sliding mode control,SMC)算法,增强了欠驱动水下机器人系统的鲁棒性,提高了抗扰能力。Jia等人针对带有规定性能的欠驱动水下机器人轨迹跟踪问题,提出了一种自适应输出反馈控制器,并通过仿真验证了所提方法的有效性。但是,上述跟踪控制方法或存在视线导航的不平滑问题,或存在偏航角奇异问题,从而导致路径跟踪不准确。
发明内容
为了解决现有技术中的上述问题,即为了实现准确的路径跟踪,本发明的目的在于提供一种仿生滑翔机器海豚平面路径跟踪方法及系统。
为解决上述技术问题,本发明提供了如下方案:
一种仿生滑翔机器海豚平面路径跟踪方法,包括:
基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
可选地,所述基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点,具体包括:
针对任意相邻的两个路径点pk、pk+1:
基于当前目标跟踪点,以仿生滑翔机器海豚在水下的当前位置为圆心、半径为γ设置运动虚拟圆,以路径点pk为圆心、半径为Rr设置目标虚拟圆:
当所述仿生滑翔机器海豚进入到路径点pk的目标虚拟圆内,更新当前目标跟踪点为运动虚拟圆与直线段pkpk+1的交点,直至当仿生滑翔机器海豚进入下一个路径点pk+1的目标虚拟圆内。
可选地,所述基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩,具体包括:
根据所述当前位置坐标及目标跟踪点,确定跟踪误差,所述跟踪误差变量包括目标位置误差及目标偏航角误差;
基于李雅普诺夫函数,根据所述目标偏航角误差确定偏航力矩;
基于李雅普诺夫函数,根据所述目标位置误差确定前进推力。
可选地,根据以下公式确定偏航力矩τr:
re=r-α1,
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;
可选地,根据以下公式确定前进推力τu:
ue=u-α2,
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;
α2可选地,所述基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩,还包括:
利用跟踪微分器,用于平滑目标跟踪点的导数和镇定函数α2,以修正所述仿生滑翔机器海豚的前进推力。
可选地,所述根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,具体包括;
基于仿生滑翔机器海豚在水下的当前位置坐标,确定仿生滑翔机器海豚在水下的动力学模型
其中,(x,y)为仿生滑翔机器海豚在水下的当前位置坐标,ψ表示偏航角,u表示平面沿x轴的线速度,v表示平面沿y轴的线速度,r表示平面z轴角速度,
基于所述动力学模型,并忽略俯仰和横滚运动,得到仿生滑翔机器海豚在水下的平面动力学模型
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;diag(·)表示对角矩阵;τu表示前进推力,τr表示偏航力矩;
根据所述前进推力和偏航力矩控制调整所述平面动力学模型,更新所述仿生滑翔机器海豚在水下的当前位置。
为解决上述技术问题,本发明还提供了如下方案:
一种仿生滑翔机器海豚平面路径跟踪系统,包括:
第一确定单元,用于基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
第二确定单元,基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
调整单元,用于根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
为解决上述技术问题,本发明还提供了如下方案:
一种仿生滑翔机器海豚平面路径跟踪系统,包括:
处理器;以及
被安排成存储计算机可执行指令的存储器,所述可执行指令在被执行时使所述处理器执行以下操作:
基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
为解决上述技术问题,本发明还提供了如下方案:
一种计算机可读存储介质,所述计算机可读存储介质存储一个或多个程序,所述一个或多个程序当被包括多个应用程序的电子设备执行时,使得所述电子设备执行以下操作:
基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
根据本发明的实施例,本发明公开了以下技术效果:
本发明实时定位仿生滑翔机器海豚在水下的当前位置,并通过预先规划的路径点,确定所述仿生滑翔机器海豚的目标跟踪点,以及仿生滑翔机器海豚的前进推力和偏航力矩,进而根据前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,以调整所述仿生滑翔机器海豚在水下的运动轨迹,最终使得该运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
附图说明
图1是本发明仿生滑翔机器海豚平面路径跟踪方法的流程图;
图2是目标跟踪点的确定示意图;
图3是本发明仿生滑翔机器海豚平面路径跟踪系统的模块结构示意图。
符号说明:
第一确定单元—1,第二确定单元—2,调整单元—3。
具体实施方式
下面参照附图来描述本发明的优选实施方式。本领域技术人员应当理解的是,这些实施方式仅仅用于解释本发明的技术原理,并非旨在限制本发明的保护范围。
本发明的目的是提供一种仿生滑翔机器海豚平面路径跟踪方法,实时定位仿生滑翔机器海豚在水下的当前位置,并通过预先规划的路径点,确定所述仿生滑翔机器海豚的目标跟踪点,以及仿生滑翔机器海豚的前进推力和偏航力矩,进而根据前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,以调整所述仿生滑翔机器海豚在水下的运动轨迹,最终使得该运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
为使本发明的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1所示,本发明仿生滑翔机器海豚平面路径跟踪方法包括:
步骤100:基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点。
步骤200:基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩。
步骤300:根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
其中,在步骤100中,采用改进的视线导航法,是针对切换阶段的不平滑性,改变视线点的选取方式。
具体地,如图2所示,所述基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点,包括:
针对任意相邻的两个路径点pk、pk+1:
基于当前目标跟踪点,以仿生滑翔机器海豚在水下的当前位置为圆心、半径为γ设置运动虚拟圆,以路径点pk为圆心、半径为Rr设置目标虚拟圆:
当所述仿生滑翔机器海豚进入到路径点pk的目标虚拟圆内,更新当前目标跟踪点为运动虚拟圆与直线段pkpk+1的交点,直至当仿生滑翔机器海豚进入下一个路径点pk+1的目标虚拟圆内。
其中,k=1,2,...,N-1,N表示路径点的总数量。
优选地,在步骤200中,所述基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩,具体包括:
步骤210:根据所述当前位置坐标及目标跟踪点,确定跟踪误差,所述跟踪误差变量包括目标位置误差及目标偏航角误差;
步骤220:基于李雅普诺夫函数,根据所述目标偏航角误差确定偏航力矩;
步骤230:基于李雅普诺夫函数,根据所述目标位置误差确定前进推力。
根据以下公式确定偏航力矩τr:
re=r-α1,
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;
根据以下公式确定前进推力τu:
ue=u-α2,
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;
进一步地,在步骤200中,所述基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩,还包括:
利用跟踪微分器,用于平滑目标跟踪点的导数和镇定函数α2,以修正所述仿生滑翔机器海豚的前进推力。
具体地,通过跟踪微分器平滑目标跟踪点的导数和镇定函数α2,通过平滑了这两点,可得到稳定的前进推力。
在步骤300中,所述根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,具体包括;
步骤310:基于仿生滑翔机器海豚在水下的当前位置坐标,确定仿生滑翔机器海豚在水下的动力学模型
其中,(x,y)为仿生滑翔机器海豚在水下的当前位置坐标,ψ表示偏航角,v表示平面沿y轴的线速度,r表示平面z轴角速度,
步骤320:基于所述动力学模型,并忽略俯仰和横滚运动,得到仿生滑翔机器海豚在水下的平面动力学模型
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;diag(·)表示对角矩阵;τu表示前进推力,τr表示偏航力矩;
步骤330:根据所述前进推力和偏航力矩控制调整所述平面动力学模型,更新所述仿生滑翔机器海豚在水下的当前位置。
下面以一具体实施例,详细介绍本发明仿生滑翔机器海豚平面路径跟踪方法:
其中,仿生滑翔机器海豚主要由腰尾装置和胸鳍装置构成,腰尾关节均由电机驱动,两侧胸鳍由舵机驱动。在本发明中,腰尾装置的身体、尾鳍模式(body-caudal fin,BCF)主要用于提供推力,胸鳍装置的中间鳍、对鳍模式(median paired fin,MPF)则用来产生转向力矩。
本发明仿生滑翔机器海豚平面路径跟踪方法包括以下步骤:
步骤1,通过忽略俯仰和横滚运动,得到平面动力学模型。
针对平面路径跟踪问题,首先,定义机器人的实时平面坐标为(x,y),偏航角为ψ,平面线速度和偏航角速度向量为(u,v,r),可以推导出水下机器人的运动学模型如下:
然后,忽略俯仰和横滚运动,得到平面动力学模型
其中,M=diag(m11,m22,m33)表示质量参数矩阵;D=diag(d11,d22,d33)表示阻尼参数矩阵;diag(·)表示对角矩阵;τu和τr分别表示前进推力和偏航力矩。
步骤2,通过改进视线导航法,获取平滑的目标跟踪点和目标偏航角。
步骤3,通过推导自适应反步控制律,设计偏航控制器和速度控制器。
在此过程中,主要以设计李雅普诺夫函数为准则,通过推导反步控制律,同时保证系统的收敛性,具体设计原则遵循以下步骤:
步骤3.1,计算跟踪误差变量如下:
其中,(xd,yd)表示目标跟踪点;ψd表示目标偏航角:
其中,sign(·)表示符号函数;
步骤3.2,根据跟踪误差变量,选取偏航角误差作为状态变量z1:
控制器的设计目标是使z1→0,因此对其定义Barrier Lyapunov函数V1:
其中,ψL是一个正常数,用于限制状态变量的取值范围。因此,当|z1|<ψL时,所述V1是正定的。然后,定义re=r-α1,且镇定函数α1:
其中,k1是人工设定的正常数。将所述镇定函数α1代入
步骤3.3,为了使
然后,通过代入平面动力学模型中,得到
将所述
由于模型阻尼参数难以精确获取,本发明提出一种自适应控制算法以估计该参数,估计值为
确定偏航力矩τr:
其中,k2是人工设定的正常数。将τr代入
因此,通过定义参数估计值为:
进而可得到
然后,根据以下约束条件:
(1)V>0;
(2)
(3)如果
那么
则g(t)=k1z12+k2re2,通过
步骤3.4,根据视线导航法以及跟踪误差,可以得知exy≤σ,其中σ是一个设定的正常数(例如,σ为一个小的正常数)。因此,为了使exy→σ,定义李雅普诺夫函数V4如下:
然后,定义ue=u-α2,且镇定函数α2:
其中,k3是人工设定的正常数。根据约束条件,通过设定初始的偏航角误差,即ψe(0)<π/2,能够保证ψe(t)<π/2,t>0始终成立,因此避免了cos-1(ψe)的奇异现象。然后,通过将镇定函数α2代入
为了使
确定前进推力τu:
其中:
k4是人工设定的正常数。得到:
通过
步骤3.5,针对
其中,h表示采样时间;ξ1表示参考项ξr(k)的跟踪信号;ξ2表示ξ1的导数;fhan(·)表示构造的非线性函数;δ0和h0分别表示跟踪和滤波系数。
利用跟踪微分器平滑目标跟踪点的导数和镇定函数α2,通过平滑了这两点,然后会得到稳定的前进推力。
本发明仿生滑翔机器海豚的平面路径跟踪方法首先通过忽略俯仰和横滚运动,得到滑翔海豚的平面动力学模型。然后,利用改进的视线导航法,将期望路径转化为目标跟踪点和目标偏航角,并且对目标跟踪点应用了跟踪微分器,使其变化更加平滑。最后,运用反步法,推导出前进推力和偏航力矩。其中,通过设计障碍李雅普诺夫(Barrier Lyapunov)函数,解决了速度控制律中偏航角奇异问题,并且针对未知模型参数应用自适应控制,进一步提高了控制鲁棒性。
此外,本发明还提供了一种仿生滑翔机器海豚平面路径跟踪系统,可实现准确的路径跟踪。
如图3所示,本发明仿生滑翔机器海豚平面路径跟踪系统包括第一确定单元1、第二确定单元2及调整单元3。
具体地,所述第一确定单元1用于基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
所述第二确定单元2基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
所述调整单元3用于根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
此外,本发明还提供了一种仿生滑翔机器海豚平面路径跟踪系统,包括:
处理器;以及
被安排成存储计算机可执行指令的存储器,所述可执行指令在被执行时使所述处理器执行以下操作:
基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
优选地,本发明还提供了一种计算机可读存储介质,所述计算机可读存储介质存储一个或多个程序,所述一个或多个程序当被包括多个应用程序的电子设备执行时,使得所述电子设备执行以下操作:
基于预先规划的路径点以及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的目标跟踪点;
基于所述目标跟踪点及仿生滑翔机器海豚在水下的当前位置,确定所述仿生滑翔机器海豚的前进推力和偏航力矩;
根据所述前进推力和偏航力矩控制所述仿生滑翔机器海豚在水下运动,更新所述仿生滑翔机器海豚在水下的当前位置,最终使得所述仿生滑翔机器海豚在水下的运动轨迹逐渐收敛于预先规划的路径点形成的期望曲线。
相对于现有技术,本发明仿生滑翔机器海豚平面路径跟踪系统、计算机可读存储介质与上述仿生滑翔机器海豚平面路径跟踪方法的有益效果相同,在此不再赘述。
至此,已经结合附图所示的优选实施方式描述了本发明的技术方案,但是,本领域技术人员容易理解的是,本发明的保护范围显然不局限于这些具体实施方式。在不偏离本发明的原理的前提下,本领域技术人员可以对相关技术特征作出等同的更改或替换,这些更改或替换之后的技术方案都将落入本发明的保护范围之内。