CN114408098B - 一种收放点位和模拟角度联合的摆臂控制方法 - Google Patents

Abstract

本发明公开了一种收放点位和模拟角度联合的摆臂控制方法，在摆臂的三个定位位置分别安装三个接近开关，并在摆臂上固连金属棒；带有金属棒的摆臂分别进行放过程和收过程的运动，三个接近开关均产生电平变化；计算伸缩臂的平均伸出速度；当摆臂从行程的起点到达中间点时，计算此时摆臂的摆角；并将摆臂的摆角与摆臂处于中间点时的预设角度进行比较校正；根据校正后的伸缩臂平均伸出速度、摆臂运动的计时时间即可计算得到摆臂在中间点和行程的终点之间的任意位置的摆角；根据该摆角可对摆臂在布放到位点和回收到位点之间的任意位置进行调节，实现对摆臂的控制；本发明能够实现摆臂摆角连续定位，克服了原有收放系统摆臂控制使用三点式定位。

Description

一种收放点位和模拟角度联合的摆臂控制方法

技术领域

本发明属于海上遥控艇收放系统技术领域，具体涉及一种收放点位和模拟角度联合的摆臂控制方法。

背景技术

武器装备的无人化、自动化和智能化是当前武器装备的显著特征，特别是无人化作业过程中，将整个作业过程数据化、可视化是实现自动化和无人化的前提条件，数据的质量往往就决定了自动化和无人化的水平，连续可靠的数据才能够显著提高任务执行的高效率和可靠性。

目前，海上遥控艇收放系统上安装有摆臂，通过摆臂的摆动对布放设备进行收放，由于现有遥控艇结构受限，在初始设计时，对摆臂的三个定位位置进行定位，三个定位位置分别为布放到位点、中间点和回收到位点；因此摆臂能够在这三个定位位置准确定位，除这三个定位位置之外的区域就不能准确定位；收放系统进行拖曳作业时，摆臂需要处于一个设定的摆角来配合，这个摆角根据不同拖曳速度会存在一个调整范围，而目前摆臂只能提供上述三个定位位置的摆角，若摆臂不在上述三个定位位置，就不能对摆臂的摆角有一个定量的描述，无法精确调节摆臂到设定摆角，使用中会造成一定困难；目前可通过计时方法对不在上述三个定位位置的摆臂摆角进行计算，但是面对多套设备、使用环境等带来的一致性问题，仅用计时方法不能保证摆角计算的准确度和可重复性，使得摆臂摆角的数值不够准确，最终导致对摆臂的控制精度较低。

发明内容

有鉴于此，本发明提供了一种收放点位和模拟角度联合的摆臂控制方法，能够实现摆臂摆角连续定位，克服了原有收放系统摆臂控制使用三点式定位，对除三点外的随机位置缺乏定量描述手段，当远程遥控作业过程中主控端对摆臂工况不能唯一确定的问题。

本发明是通过下述技术方案实现的：

一种收放点位和模拟角度联合的摆臂控制方法，所述摆臂一端与遥控艇销接，伸缩臂的一端与遥控艇铰接，另一端与摆臂的中部铰接，所述摆臂与遥控艇的销接点为A点，所述伸缩臂与遥控艇的铰接点为C点，伸缩臂与摆臂的铰接点为B点，A点、B点及C点组成三角形△ABC；摆臂的三个定位位置分别为布放到位点、中间点和回收到位点；

所述摆臂控制方法的具体步骤如下：

步骤一，在摆臂的三个定位位置分别安装三个接近开关，并在所述摆臂上固连金属棒；

步骤二，带有金属棒的摆臂分别进行放过程和收过程的运动，三个接近开关均产生电平变化；

步骤三，计算伸缩臂的平均伸出速度S_default；当摆臂从行程的起点到达中间点时，计算此时摆臂的摆角

并将摆臂的摆角/>

与摆臂处于中间点时的预设角度/>

进行比较；

若差值小于0.5％，则无需对伸缩臂平均伸出速度S_default进行校正，根据S_default、摆臂运动的计时时间即可计算得到摆臂在中间点和行程的终点之间的任意位置的摆角

若差值大于或等于0.5％，则需对伸缩臂平均伸出速度S_default进行校正；校正后的S_default记为S_avr，根据S_avr、摆臂运动的计时时间即可计算得到摆臂在中间点和行程的终点之间的任意位置的摆角

步骤四，根据步骤三计算出中间点和行程的终点之间的摆角

可对摆臂在布放到位点和回收到位点之间的任意位置进行调节，实现对摆臂的控制。

进一步的，在步骤三中，根据三角形△ABC的余弦定理计算摆臂的摆角

计算公式如下：

式中，l_ab为AB边的长度，l_ac为AC边的长度，均为固定值，l_bc为BC边的长度，即伸缩臂长度。

进一步的，在步骤三中，当摆臂的行程的起点为回收到位点时，l_bc的计算公式如下；

l_bc＝l₁+t_x×S_default公式(2)

式中，l₁为摆臂在回收到位点时伸缩臂的长度，为固定值，t_x是摆臂运动的计时时间。进一步的，在步骤三中，当摆臂的行程的起点为布放到位点时，l_bc的计算公式如下；

l_bc＝l₂+t_x×S_default公式(3)

式中，l₂为摆臂在回收到位点时伸缩臂的长度，为固定值，t_x是摆臂运动的计时时间。

进一步的，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

将此时摆臂的摆角

修正为/>

即令/>

将/>

和t₁代入公式(1)和公式(2)中，推导出修正后的伸缩臂平均伸出速度S_default，令修正后的伸缩臂平均伸出速度记为S_avr，将S_avr代入公式(1)和公式(2)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和布放到位点之间的任意位置的摆角/>

进一步的，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

对伸缩臂平均伸出速度S_default进行修正，修正后的伸缩臂平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂处于中间点时，伸缩臂的长度，为已知量；

将S_avr代入公式(1)和公式(2)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和布放到位点之间的任意位置的摆角

进一步的，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

将此时摆臂的摆角

修正为/>

即令/>

将/>

和t₂代入公式(1)和公式(3)中，推导出修正后的伸缩臂平均伸出速度S_default，令修正后的伸缩臂平均伸出速度记为S_avr，将S_avr代入公式(1)和公式(3)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和回收到位点之间的任意位置的摆角/>

进一步的，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

对伸缩臂平均伸出速度S_default进行修正，修正后的伸缩臂平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂处于中间点时，伸缩臂的长度，为已知量；

将S_avr代入公式(1)和公式(3)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和回收到位点之间的任意位置的摆角

进一步的，在步骤三中，计算伸缩臂的平均伸出速度S_default的方法如下：

实测伸缩臂伸出长度的最大值为L_total和伸出总用时为T_total，得出伸缩臂平均伸出速度S_default，S_default＝_total/T_total。

有益效果：

(1)本发明在不改变当前遥控艇收放系统机械和电气结构，不增添新设备的前提下，将仅能够实现摆臂摆角三点定位升级为连续角度定位，即把描述摆臂工况的信息由点位置升级为摆臂摆角，与每个位置一一对应，消除摆臂摆角定位盲区问题，有效解决拖体收放过程中摆臂若处于定位盲区需先找基准点的问题，减少了非必要运动时间，提高了系统作业效率。

(2)本发明利用原有点位置的准确性对生成的摆角进行校正，以消除误差累积、环境因素等可能导致的漂移，在一定程度上继承和保留原来点位置定位的优点，又使上级系统和主控端得的是一个摆臂的准确位置，而不是存在两种可能性的定位区域，从而让控制和决策更精准高效。

附图说明

图1为摆臂的连接关系图；

图2为摆臂运动过程的三个接近开关产生信号的顺序图；

其中，1-摆臂，2-伸缩臂，3-转轴，4-金属棒。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本实施例提供了一种收放点位和模拟角度联合的摆臂控制方法，所述摆臂1安装在收放系统的遥控艇上，并通过转轴3与遥控艇后甲板的基座销接，令转轴3的轴线为A点，所述摆臂1可绕所述A点转动，收放系统的伸缩臂2安装在遥控艇上，其一端与遥控艇上的连接基座铰接，另一端与摆臂1的中部铰接，如图1所述，所述伸缩臂2与连接基座的铰接点为C点，伸缩臂与摆臂的铰接点为B点；

由于现有遥控艇结构受限，在初始设计时，对摆臂1的三个定位位置进行定位，三个定位位置分别为布放到位点、中间点和回收到位点；因此摆臂1能够在这三个定位位置准确定位，除这三个定位位置之外的区域就不能准确定位；收放系统进行拖曳作业时，摆臂1需要处于一个设定的摆角来配合，这个摆角根据不同拖曳速度会存在一个调整范围，而目前摆臂1只能提供上述三个定位位置的摆角，若摆臂1不在上述三个定位位置，就不能对摆臂1的摆角有一个定量的描述，无法精确调节摆臂1到设定摆角，使用中会造成一定困难；目前可通过计时方法对不在上述三个定位位置的摆臂1的摆角进行计算，但是面对多套设备、使用环境等带来的一致性问题，且由于伸缩臂2的单向运动匀速，摆臂1的摆角变化不是匀速的，因此，仅用计时方法不能保证摆角计算的准确度和可重复性；同时，由于收放系统对布放设备的放流程和收流程中的工作负荷是不同的，所以摆臂1的行程总用时和摆角的变化速率也是不同的，因此，不能使用单纯的计时方法来计算摆臂1的摆角；为准确生成摆臂1的摆角，本实施例利用原有的三个定位位置来修正摆臂1的摆角的角度变化，同时把摆臂1的收过程和放过程分别处理，提高摆臂1的摆角的准确度，具体步骤如下：

步骤一，在摆臂1的三个定位位置分别安装三个接近开关，即采用三个接近开关来标示布放到位点、中间点和回收到位点；令回收到位点的接近开关为接近开关a，中间点的接近开关为接近开关b，布放到位点的接近开关为接近开关c；三个接近开关的直径均为D；

并在所述摆臂1上固连小直径的金属棒4，当金属棒4开始与所述接近开关接触时(即金属棒4位于接近开关的直径D的一侧)，触发接近开关，接近开关发生电平变化，当金属棒4越过接近开关，并离开所述接近开关(即金属棒4位于接近开关的直径D的另一侧)时，接近开关的电平再次发生变化；

步骤二，带有金属棒4的摆臂1分别进行放过程和收过程的运动；令摆臂1处于回收到位点时的角度记为0°，到达布放到位点时为最大摆角，即摆臂1的放过程中摆角单向增大，收过程摆角单向减小；

摆臂1的放过程为：回收到位点为摆臂1行程的起点，布放到位点为摆臂1行程的终点，摆臂1从回收到位点运动到布放到位点，即在图2上从右向左运动；在此过程中，三个接近开关产生的完整的信号顺序为：当摆臂1位于回收到位点时，由于摆臂1上的金属棒4位于接近开关a处，且未离开接近开关a，因此，此时接近开关a输出高电平，当摆臂1开始运动，金属棒4越过接近开关a，并离开所述接近开关a时，接近开关a上形成一个下降沿a1，接近开关a输出低电平；当摆臂1运动到金属棒4开始与所述接近开关b接触时，接近开关b形成一个上升沿b1，接近开关b输出高电平，当金属棒4越过接近开关b，并离开所述接近开关b时，接近开关b上形成一个下降沿b2，接近开关b输出低电平；当摆臂1继续运动到金属棒4开始与所述接近开关c接触时，接近开关c形成一个上升沿c1，接近开关c输出高电平，摆臂1上的金属棒4在接近开关c处停止运动，此时摆臂1位于布放到位点；

摆臂1的收过程为：布放到位点为摆臂1行程的起点，回收到位点为摆臂1行程的终点，摆臂1从布放到位点运动到回收到位点，即在图2上从左向右运动；在此过程中，三个接近开关产生的完整的信号顺序为：由于摆臂1位于布放到位点，摆臂1上的金属棒4位于接近开关c处，此时接近开关c输出高电平，当摆臂1开始运动，金属棒4越过接近开关c，并离开所述接近开关c时，接近开关c上形成一个下降沿c2，接近开关c输出低电平；当摆臂1运动到金属棒4开始与所述接近开关b接触时，接近开关b形成一个上升沿b3，接近开关b输出高电平，当金属棒4越过接近开关b，并离开所述接近开关b时，接近开关b上形成一个下降沿b4，接近开关b输出低电平；当摆臂1继续运动到金属棒4开始与所述接近开关a接触时，接近开关a形成一个上升沿a2，接近开关a输出高电平，摆臂1上的金属棒4在接近开关a处停止运动，此时摆臂1位于回收到位点；

步骤三，根据摆臂1的放过程和收过程中的三个接近开关产生的信号，对三个接近开关的状态进行检测，判断接近开关是否发生故障；以摆臂1的放过程为例，摆臂1处于回收到位点时，接近开关a被触发，信号输出为高，当摆臂1启动布放(即摆臂1开始运动)后的一定时间窗内对接近开关a的状态进行判断，若存在下降沿a1则认为接近开关a正常，否则接近开关a故障；同样方法判断接近开关b是否存在上升沿b1或下降沿b2，若存在则认为接近开关b正常，否则接近开关b故障；同样方法判断接近开关c是否存在上升沿c1，若存在则认为接近开关c正常，否则接近开关c故障；发生故障的接近开关在摆臂1的摆角计算过程中不再用来做位置校正，仅依靠计时方法计算摆臂1的摆角，同时也要对发生故障的接近开关进行定位，报送检测信息；其中，若接近开关a故障，则只对位于中间点和布放到位点之间的摆臂1的摆角进行校正，若接近开关c故障，则只对位于中间点和回收到位点之间的摆臂1的摆角进行校正；若接近开关b故障，则无法对摆臂1的摆角进行校正；

步骤四，第一种情况：在三个接近开关均正常的状态下，通过三个定位位置对摆臂1的摆角进行校正计算，校正计算时因摆臂1的摆角由摆臂1、基座和伸缩臂2组成的三角形△ABC确定，且AB边和AC边已知，BC边为伸缩臂2长度，伸缩臂2的长度变化接近匀速变化，则先确定伸缩臂2长度，然后再由三边关系去计算摆臂1的摆角，则简化了摆角的计算过程；但是通过三个定位位置对摆臂1的摆角进行校正计算是有条件的，摆臂1必须进行完整且连续的相邻的两个定位位置之间的运动才可以，即摆臂1的运动至少为放过程的a1到b1或b2到c1半行程，或收过程的c2到b3或b4到a2半行程，若摆臂1的运动为放过程的全行程运动或收过程的全行程运动，则校正就会发生在a1到c1或c2到a2过程的末尾，同时都在可校正的运动末尾对摆角强制置位，对摆角的校正不起任何作用；

实测伸缩臂2伸出长度的最大值为L_total(单位m)和伸出总用时为T_total(单位s)，可以得出伸缩臂2平均伸出速度S_default，S_default＝_total/T_total；

(1)摆臂1的放过程，即摆臂1从回收到位点向布放到位点运动时，当检测到接近开关a的下降沿a1后，开始计时，以10Hz频率计算摆角

摆角/>

由余弦定理可得，

式中，l_ab为AB边的长度，l_ac为AC边的长度，均为固定值，l_bc为BC边的长度，即伸缩臂2长度，为变化值，计算公式如下；

l_bc＝l₁+t_x×S_default 公式(2)

式中，l₁为摆臂1在回收到位点时伸缩臂2的长度，为固定值，t_x是摆臂1运动的计时时间；

当检测接近开关b的上升沿b1后，此时摆臂1处于中间点，并记下此时的t_x值，记为t₁(t₁为摆臂1从回收到位点到中间点的运动时间)，将t₁代入公式(1)和公式(2)计算此时摆臂1的摆角

对计算得到的摆角/>

与摆臂1处于中间点时的预设角度/>

进行比较；

若差值小于0.5％，则无需对此时摆臂1的摆角

及摆臂1伸缩臂2平均伸出速度S_default进行校正，根据S_default、摆臂1运动的计时时间t_x及公式(1)和公式(2)即可计算得到摆臂1在中间点和布放到位点之间的任意位置的摆角/>

若差值大于或等于0.5％，则需对此时摆臂1的摆角

及摆臂1伸缩臂2平均伸出速度S_default进行校正；校正方法有两种：

第一种为：将此时摆臂1的摆角

修正为/>

即令/>

将/>

和t₁代入公式(1)和公式(2)中，推导出修正后的伸缩臂2平均伸出速度S_default，令修正后的伸缩臂2平均伸出速度记为S_avr，将S_avr代入公式(1)和公式(2)中，则根据摆臂1运动的计时时间t_x即可计算得到摆臂1在中间点和布放到位点之间的任意位置的摆角/>

第二种为：对伸缩臂2平均伸出速度S_default进行修正，修正后的伸缩臂2平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂1处于上升沿b1点时，伸缩臂2的长度，为已知量；

将S_avr代入公式(1)和公式(2)中，则根据摆臂1运动的计时时间t_x即可计算得到摆臂1在中间点和布放到位点之间的任意位置的摆角

(2)摆臂1的收过程，即摆臂1从布放到位点向回收到位点运动时，当检测到接近开关c的下降沿c2后，开始计时，以10Hz频率计算摆角

摆角/>

由余弦定理可得，

式中，l_ab为AB边的长度，l_ac为AC边的长度，均为固定值，l_bc为BC边的长度，即伸缩臂2长度，为变化值，计算公式如下；

l_bc＝l₂+t_x×S_default 公式(5)

式中，l₂为摆臂1在布放到位点时伸缩臂2的长度，为固定值，t_x是摆臂1运动的计时时间；

当检测接近开关b的上升沿b3后，此时摆臂1处于中间点，并记下此时的t_x值，记为t₂(t₂为摆臂1从回收到位点到中间点的运动时间)，将t₂代入公式(4)和公式(5)计算此时摆臂1的摆角

对计算得到的摆角/>

与摆臂1处于中间点时的预设角度/>

进行比较；

若差值小于0.5％，则无需对此时摆臂1的摆角

及摆臂1伸缩臂2平均伸出速度S_default进行校正，根据S_default、摆臂1运动的计时时间t_x及公式(4)和公式(5)即可计算得到摆臂1在中间点和回收到位点之间的任意位置的摆角/>

若差值大于或等于0.5％，则需对此时摆臂1的摆角

及摆臂1伸缩臂2平均伸出速度S_default进行校正；校正方法有两种：

第一种为：将此时摆臂1的摆角

修正为/>

即令/>

将/>

和t₂代入公式(4)和公式(5)中，推导出修正后的伸缩臂2平均伸出速度S_default，令修正后的伸缩臂2平均伸出速度记为S_avr，将S_avr代入公式(4)和公式(5)中，则根据摆臂1运动的计时时间t_x即可计算得到摆臂1在中间点和回收到位点之间的任意位置的摆角/>

第二种为：对伸缩臂2平均伸出速度S_default进行修正，修正后的伸缩臂2平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂1处于上升沿b3点时，伸缩臂2的长度，为已知量；

将S_avr代入公式(4)和公式(5)中，则根据摆臂1运动的计时时间t_x即可计算得到摆臂1在中间点和回收到位点之间的任意位置的摆角

第二种情况：若接近开关a故障，则仅在摆臂1的收过程，计算摆臂1在中间点和回收到位点之间的任意位置的摆角

第三种情况：若接近开关c故障，则仅在摆臂1的放过程，计算摆臂1在中间点和布放到位点之间的任意位置的摆角

步骤五，根据步骤四计算出的摆角

可对摆臂1在布放到位点和回收到位点之间的任意位置进行调节，实现对摆臂1的控制。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种收放点位和模拟角度联合的摆臂控制方法，所述摆臂一端与遥控艇销接，伸缩臂的一端与遥控艇铰接，另一端与摆臂的中部铰接，所述摆臂与遥控艇的销接点为A点，所述伸缩臂与遥控艇的铰接点为C点，伸缩臂与摆臂的铰接点为B点，A点、B点及C点组成三角形ΔABC；摆臂的三个定位位置分别为布放到位点、中间点和回收到位点；

其特征在于，所述摆臂控制方法的具体步骤如下：

步骤一，在摆臂的三个定位位置分别安装三个接近开关，并在所述摆臂上固连金属棒；

步骤二，带有金属棒的摆臂分别进行放过程和收过程的运动，三个接近开关均产生电平变化；

步骤三，计算伸缩臂的平均伸出速度S_default；当摆臂从行程的起点到达中间点时，计算此时摆臂的摆角

并将摆臂的摆角/>

与摆臂处于中间点时的预设角度/>

进行比较；

若差值小于0.5％，则无需对伸缩臂平均伸出速度S_default进行校正，根据S_default、摆臂运动的计时时间即可计算得到摆臂在中间点和行程的终点之间的任意位置的摆角

若差值大于或等于0.5％，则需对伸缩臂平均伸出速度S_default进行校正；校正后的S_default记为S_avr，根据S_avr、摆臂运动的计时时间即可计算得到摆臂在中间点和行程的终点之间的任意位置的摆角

步骤四，根据步骤三计算出中间点和行程的终点之间的摆角

可对摆臂在布放到位点和回收到位点之间的任意位置进行调节，实现对摆臂的控制。

2.如权利要求1所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，根据三角形ΔABC的余弦定理计算摆臂的摆角

计算公式如下：

式中，l_ab为AB边的长度，l_ac为AC边的长度，均为固定值，l_bc为BC边的长度，即伸缩臂长度。

3.如权利要求2所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，当摆臂的行程的起点为回收到位点时，l_bc的计算公式如下；

l_bc＝l₁+t_x×S_default公式(2)

式中，l₁为摆臂在回收到位点时伸缩臂的长度，为固定值，t_x是摆臂运动的计时时间。

4.如权利要求2所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，当摆臂的行程的起点为布放到位点时，l_bc的计算公式如下；

l_bc＝l₂+t_x×S_default公式(3)

式中，l₂为摆臂在布放到位点时伸缩臂的长度，为固定值，t_x是摆臂运动的计时时间。

5.如权利要求3所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：将此时摆臂的摆角

修正为/>

即令/>

将/>

和t₁代入公式(1)和公式(2)中，推导出修正后的伸缩臂平均伸出速度S_default，令修正后的伸缩臂平均伸出速度记为S_avr，将S_avr代入公式(1)和公式(2)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和布放到位点之间的任意位置的摆角/>

6.如权利要求3所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

对伸缩臂平均伸出速度S_default进行修正，修正后的伸缩臂平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂处于中间点时，伸缩臂的长度，为已知量；t₁为摆臂从回收到位点到中间点的运动时间；

将S_avr代入公式(1)和公式(2)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和布放到位点之间的任意位置的摆角

7.如权利要求4所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：将此时摆臂的摆角

修正为/>

即令/>

将/>

和t₂代入公式(1)和公式(3)中，推导出修正后的伸缩臂平均伸出速度S_default，令修正后的伸缩臂平均伸出速度记为S_avr，将S_avr代入公式(1)和公式(3)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和回收到位点之间的任意位置的摆角/>

8.如权利要求4所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，对摆臂伸缩臂平均伸出速度S_default进行校正的方法如下：

对伸缩臂平均伸出速度S_default进行修正，修正后的伸缩臂平均伸出速度记为S_avr，S_avr的计算公式如下

式中，L_b是摆臂处于中间点时，伸缩臂的长度，为已知量；t₂为摆臂从布放到位点到中间点的运动时间；

将S_avr代入公式(1)和公式(3)中，则根据摆臂运动的计时时间t_x即可计算得到摆臂在中间点和回收到位点之间的任意位置的摆角

9.如权利要求1-8任一项所述的一种收放点位和模拟角度联合的摆臂控制方法，其特征在于，在步骤三中，计算伸缩臂的平均伸出速度S_default的方法如下：实测伸缩臂伸出长度的最大值为L_total和伸出总用时为T_total，得出伸缩臂平均伸出速度S_default，S_default＝L_total/T_total。

Images