本发明属于履带车控制,更具体地,涉及一种海底探测履带车航向角纠偏控制方法、装置及系统。
背景技术:
1、在过去的几十年里海洋技术已经取得了长远的发展,在各种海洋技术中,作为极为重要的工具和得力助手,水下机器人在海洋探索中担任着重要的角色,海底探测履带车就是水下机器人中的一员。
2、海底探测履带车执行任务时,不论是海底清淤、采矿和探测等任务,都需要对其进行航向角控制。海底探测履带车通常采用双马达驱动,控制履带车前进、后退、转弯。海底探测履带车在海底行走时,即便对左右履带驱动马达的控制输入完全相同,但由于其结构的不对称性(张紧力、制造公差等),外部负载的不对称性,不可避免的无法按照给定轨迹行走,无法精准控制其航向角。现有的航向角控制方法是基于水声信标或履带轮的编码器,使用水声信标可获取海底探测履带车的坐标信息,进而控制海底探测履带车行走轨迹;使用编码器可以进行海底探测履带车左右履带轮的速度控制,从而控制海底履带车前进、后退、转弯。使用水声信标可完成大范围粗略的轨迹控制和航向控制,但当执行任务的深度增加时,水声信标的误差会增大,无法完成局部精确的轨迹控制和航向角控制,且成本较高;使用编码器进行航向角控制时,由于海底探测履带车行走时存在打滑,编码器采集到的履带轮速度并不是履带的真实速度,该方法无法精确控制海底探测履带车的航向角。
技术实现思路
1、针对现有技术的以上缺陷或改进需求,本发明提供了一种海底探测履带车航向角纠偏控制方法、装置及系统,其目的在于,实现海底探测履带车航向角的实时精确控制。
2、为实现上述目的,按照本发明的第一方面,提出了一种海底探测履带车航向角纠偏控制方法,包括如下步骤:
3、海底探测履带车向目标航向角移动过程中,实时获取航向角偏差量θ0;
4、基于航向角偏差量θ0计算比例阀期望开度:
5、ωl=ω0+kl·f(θ0)
6、ωr=ω0-kr·f(θ0)
7、其中,ωl、ωr分别为左、右履带轮的比例阀期望开度;f(·)为模糊控制算法函数,kl、kr分别是左、右履带轮比例阀控制增益;ω0为比例阀基础开度;
8、根据比例阀期望开度,实时对履带轮比例阀开度进行调整,实现海底探测履带车航向角纠偏。
9、作为进一步优选的,根据比例阀的期望开度,实时对履带轮比例阀开度进行调整,包括如下步骤:
10、分别判断比例阀期望开度ωl、ωr是否在预设区间[ωmin,ωmax]内:
11、若比例阀期望开度在所述预设区间内,则不对其进行调节;若比例阀期望开度小于比例阀开度下限ωmin,则令其等于ωmin;若比例阀期望开度大于比例阀开度上限ωmax,则令其等于ωmax;
12、以调节后比例阀期望开度对履带轮比例阀开度进行调整。
13、作为进一步优选的,左、右履带轮比例阀控制增益kl、kr的确定方法为:预先进行航向角控制实验,获取不同kl和kr条件下海底探测履带车航向角控制评分,取控制评分最高的一组kl和kr作为最终值。
14、作为进一步优选的,海底探测履带车航向角控制评分的确定方法为:
15、
16、
17、其中,score是海底探测履带车航向角控制评分,error是中间变量;θt是目标航向角,θi是采集的第i个海底探测履带车实际航向角,n为航向角控制实验中采集数据的总次数。
18、作为进一步优选的,预先设定海底探测履带车目标航向角θt,通过罗经实时获取海底探测履带车实际航向角θ,计算获取航向角偏差量θ0=θt-θ。
19、按照本发明的第二方面,提供了一种用于实现上述海底探测履带车航向角纠偏控制方法的装置,包括罗经、上位机、控制器和液压马达驱动器,其中:
20、所述罗经安装在履带车正前方,用于实时获取履带车当前航向角,并将当前航向角通过控制器传递给上位机;
21、所述上位机用于基于当前航向角计算航向角偏差量,并确定比例阀的期望开度,然后将比例阀期望开度传递给控制器;
22、所述控制器用于通过所述液压马达驱动器实时对履带轮比例阀开度进行调整,实现海底探测履带车航向角纠偏。
23、作为进一步优选的,所述控制器和所述上位机之间通过光交换机连接。
24、按照本发明的第三方面,提供了一种海底探测履带车航向角纠偏控制系统,其包括处理器,所述处理器用于执行上述海底探测履带车航向角纠偏控制方法。
25、按照本发明的第四方面,提供了一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时实现上述海底探测履带车航向角纠偏控制方法。
26、总体而言,通过本发明所构思的以上技术方案与现有技术相比,主要具备以下的技术优点:
27、1.本发明通过获取航向角偏差量,并设计模糊控制方法计算履带轮比例阀期望开度,进而通过调整比例阀开度实现对航向角的精确控制,可将海底探测履带车直线行驶过程中的航向角偏差值控制在±1°范围内。将本发明应用于海底探测可以自动调整其航向角,确保航向角不出现偏差,针对直行的狭窄巷道,可安全通过,不发生碰撞事故。
28、2.在海底探测履带车内部存在左右履带驱动马达,由于其存在结构上的不对称性(张紧力、制造公差等),且左右履带受到的负载特性并不完全对称,所以当控制左右履带驱动马达的输入完全相同时,但左右驱动马达的输出并不相同。因此,本发明在模糊控制时增设了履带轮比例阀控制增益系数,以消除结构不对称的影响,进一步提高控制精度。
1.一种海底探测履带车航向角纠偏控制方法,其特征在于,包括如下步骤:
2.如权利要求1所述的海底探测履带车航向角纠偏控制方法,其特征在于,根据比例阀的期望开度,实时对履带轮比例阀开度进行调整,包括如下步骤:
3.如权利要求1所述的海底探测履带车航向角纠偏控制方法,其特征在于,左、右履带轮比例阀控制增益kl、kr的确定方法为:预先进行航向角控制实验,获取不同kl和kr条件下海底探测履带车航向角控制评分,取控制评分最高的一组kl和kr作为最终值。
4.如权利要求3所述的海底探测履带车航向角纠偏控制方法,其特征在于,海底探测履带车航向角控制评分的确定方法为:
5.如权利要求1-4任一项所述的海底探测履带车航向角纠偏控制方法,其特征在于,预先设定海底探测履带车目标航向角θt,通过罗经实时获取海底探测履带车实际航向角θ,计算获取航向角偏差量θ0=θt-θ。
6.一种用于实现如权利要求1-5任一项所述的海底探测履带车航向角纠偏控制方法的装置,其特征在于,包括罗经、上位机、控制器和液压马达驱动器,其中:
7.如权利要求6所述的装置,其特征在于,所述控制器和所述上位机之间通过光交换机连接。
8.一种海底探测履带车航向角纠偏控制系统,其特征在于,包括处理器,所述处理器用于执行如权利要求1-5任一项所述的海底探测履带车航向角纠偏控制方法。
9.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现如权利要求1-5任一项所述的海底探测履带车航向角纠偏控制方法。