CN112596547B - 一种海上作业直升机快速回收的力觉引导遥操纵控制方法 - Google Patents

Abstract

本发明公开了一种海上作业直升机快速回收的力觉引导遥操纵控制方法，包括：直升机和系留装置路径规划、虚拟引导力的生成以及手控器的人机共享控制；路径规划部分将优化后的控制点构造B样条曲线作为直升机的移动路径，之后通过运动学解算得出系留装置的行驶路径；虚拟引导力部分是由规划的期望路径求解系留装置的虚拟引力以及周围环境对系留装置的虚拟斥力，二者通过加权生成虚拟引导力；手控器的人机共享控制部分是将虚拟引导力与操作人员的人手操纵力融合，通过力‑位置混合控制策略实现手控器对操作人员行为的引导和示教。本发明的方法降低了操作员由于熟练程度不足或环境复杂引发误操作的概率，提高了系留装置的工作效率和安全性。

Description

一种海上作业直升机快速回收的力觉引导遥操纵控制方法

技术领域

本发明涉及遥操纵技术领域，具体涉及一种海上作业直升机快速回收的力觉引导遥操纵控制方法。

背景技术

受发动机动力系统特点的影响，直升机在船上甲板机库、停机坪、起飞线等区域的移动需通过系留装置牵引的方式实现。

系留装置工作时通过末端的机械爪抓紧并锁住直升机探杆，并通过横向和纵向移动牵引直升机行走。系留装置与直升机之间的耦合系统呈现复杂的运动学和动力学特性。为了实现直升机在船上降落区域的预定行驶轨迹，系留装置的牵引路线需要很高的操作技巧。存在直升机牵引的作业效率低，且对操作人员专业技能要求高的问题。

发明内容

有鉴于此，本发明提出了一种海上作业直升机快速回收的力觉引导遥操纵控制方法，通过系留装置手控器的虚拟力反馈，对操作人员的操纵行为予以引导，从而提高海上直升机回收的作业效率，降低对操作人员熟练程度的要求。

为了达到上述发明目的，本发明采用以下技术方案：

一种海上作业直升机快速回收的力觉引导遥操纵控制方法，所述方法包括：

根据实时反馈的直升机位置和船上降落区域信息规划出直升机的最优牵引路径，进而经过运动学解算得出系留装置的最优路径；

根据所述系留装置的最优路径，通过人工势场求解虚拟引导力；

将所述虚拟引导力同操作人员的操纵力共享融合，共同作为系留装置手控器的输入信号，通过力-位置混合控制实现系留装置手控器对操作人员行为的力觉引导。

进一步地，根据船上降落区域信息规划出直升机的最优牵引路径，包括：

利用船上降落区域信息获得直升机牵引的起点、终点以及中间关键点；

以所述直升机牵引的起点、终点以及中间关键点作为B样条函数曲线的型值点，利用所述型值点反求控制点，加入约束条件对所求的控制点进行优化处理，采用逐步二次规划的方法求解最优控制点，将所述最优控制点拟合成B样条函数曲线，得到直升机的最优牵引路径。

进一步地，所述B样条函数曲线为均匀三次B样条曲线。

进一步地，所述约束条件包括：尽可能多地通过事先预定好的最优路径关键点并期望每段曲线的初始位置能落在最优路径关键点上；直升机的起点和终点方位角对曲线首尾切线的约束；满足最优路径最小转弯半径对路径曲线的曲率约束。

进一步地，根据所述系留装置的最优路径，通过人工势场求解虚拟引导力，包括：

基于所述系留装置的最优路径，构建系留装置运动的人工势能场，所述系留装置的最优路径代表低势能区，船上建筑环境代表高势能区；所述低势能区产生虚拟引力，所述高势能区产生虚拟斥力；

对所述虚拟引力和所述虚拟斥力进行加权处理，得到所述系留装置的虚拟引导力。

进一步地，基于所述系留装置的最优路径，构建系留装置运动的人工势能场，包括：

基于随着系留装置距离目标位置的距离越远而单调递增的原则构建引力场V_att：

其中，d(r)＝||r₀-r||为系留装置当前位姿r与期望位姿r₀的距离，ζ表示的是引力势场的增益比例系数；

基于随着系留装置距离障碍物距离的增大而减小的原则构建斥力场V_rep：

其中，ρ(r)为系留装置沿其速度方向与障碍物边界的最短距离；η为斥力势场中的增益比例系数，ρ₀为障碍物的作用距离，超过这个距离将不作为障碍物处理；

依据系留装置的当前位置ξ_c，构建沿着期望路径的“吸引子”位置ξ_d：

ξ_d＝ξ(s_p+Δs)；

其中，s_p＝arg min_s||ξc_c-ξ(s)||，表示的是路径点ξ(s_p)为系留装置当前位置ξ_c在期望路径ξ(s)的最邻近投影点，Δs为前瞻量；

基于所述引力场和所述斥力场，构建“吸引子”位置ξ_d与系留装置当前位置ξ_c的虚拟引力F_ξ：

其中，

为梯度符号，

为势函数V_ξ的梯度向量的转置，λ为虚拟力的比例系数；

根据直升机牵引过程中车身运动速度方向与障碍物的距离，构建系留装置的虚拟斥力F_γ如下：

其中，k是比例因子，ρ为系留装置沿其速度方向与周围环境的距离，ρ₀为系留装置与障碍物之间的安全距离，

为系留装置运动速度的单位向量。

进一步地，将所述虚拟引导力同操作人员的操纵力共享融合，共同作为系留装置手控器的输入信号，通过力-位置混合控制实现系留装置手控器对操作人员行为的力觉引导，包括：

将所述虚拟引力F_ρ和所述虚拟斥力F_γ进行加权合成，映射至手控器的各个关节，得到手控器的虚拟引导力τ_a：

τ_a＝αF_ξ+βF_γ；

其中，α、β分别为不同引力、斥力的影响因数；

手控器在虚拟引导力τ_a、操作人员的操纵力F_h共同作用下，产生期望运动趋势ξ_D；将手控器的关节运动简化为质量-阻尼系统，基于所述运动趋势设计手控器的位置预测控制器：

其中，M、B分别为手控器的惯量矩阵和阻尼矩阵，K_a、K_h分别为手控器自主控制和操作者手动控制的权值系数，J_ξ为手控器的雅克比矩阵；

按下式设计手控器的控制量u_m，使其跟踪位置预测器输出运动趋势信号；

其中，K_m、B_m分别为位置预测控制器的比例增益和微分增益，ξ_D为期望运动趋势，ξ_C为当前运动趋势。

本发明将力觉反馈/引导技术用于直升机系留装置的遥操纵控制上，首先，根据实时反馈的直升机位置和船上降落区域建筑物(障碍物)信息对直升机进行最优路径(期望路径)规划，运动学解算处理得到系留装置的最优牵引路径；随后基于人工势场理论，构建目标方向对系留装置的虚拟引力以及障碍物对系留装置的虚拟斥力，并经过合适的加权处理之后得到引导力；最后将引导力同操作员的操纵力进行融合，共同作用于系留装置手控器，从而引导操作人员控制系留装置以实现预定的行驶路径。有效提高了海上直升机快速回收的作业效率，也降低了对操作人员熟练程度的要求。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中一种海上作业直升机快速回收的力觉引导遥操纵控制方法的流程图；

图2为本发明实施例中路径规划优化前路径曲线图；

图3为本发明实施例中路径规划优化后的路径曲线图；

图4为本发明实施例中虚拟引力构建模型示意图；

图5为本发明实施例中基于虚拟引导力的人机共享控制的技术路线图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本发明公开了一种海上作业直升机快速回收的力觉引导遥操纵控制方法，该方法基于机器智能实时规划的直升机牵引路径，对操作人员的遥控驾驶行为予以力觉引导。主要包括路径规划、虚拟引导力生成以及人机协同控制这三部分的内容，其中，基于路径规划的虚拟力反馈是本发明的主要特点。

参见图1，其示出了本发明实施例中一种海上作业直升机快速回收的力觉引导遥操纵控制方法的流程图，该方法包括：

一、路径规划

B样条曲线包括一组基底函数和控制点参数，其具有构造灵活、快速收敛、分段调整的优点，适合作为直升机路径曲线描述的工具。

B样条曲线是B样条基曲线的线性组合，给定n+1个控制点的坐标P_i(i＝0,1,2,...,n)，n次B样条曲线段的参数表达式为：

其中，B_i,n(u)为n次B样条曲线的基函数，P_i为B样条曲线的控制点。

二次B样条曲线n＝2，三次B样条曲线n＝3，以此类推。本发明的最优路径需要一定的平滑度，故此处选择均匀三次B样条曲线作为最优路径。

由递推deBoor-Cox公式可以求得均匀三次B样条曲线的基底函数：

将(2)式代入(1)式可得均匀三次B样条曲线的矩阵表达式：

利用B样条函数构造曲线，就是根据n个已知点Q_h(h＝0,1,2,…,n-1)构造B样条函数曲线，该曲线经过上述n个点，此时的点称为型值点。如果直接将型值点作为控制点进行曲线的构建，会出现节点重复使用的问题，降低曲线在连接点处的平滑性，规划的路径就不准确了。为此，本发明提出了采用型值点反求控制点，然后通过设定最小转弯半径、移动始末方位角等限制条件对控制点进行最优化处理，采用逐步二次规划的方法来求解最优控制点，最后将最优控制点进行B样条曲线的拟合，由此来获得直升机的最优行驶路径。

B样条曲线控制点的优化过程如下：

控制点求取需满足三个约束条件：1、尽可能多地通过事先预定好的最优路径关键点并期望每段曲线的初始位置能落在最优路径关键点上；2、考虑到船上降落区域的场地要求，加入直升机的起点和终点方位角对曲线首尾切线的约束；3、满足最优路径最小转弯半径对路径曲线的曲率约束。

将船上降落区域简化为一个二维平面，路径曲线需要通过m+2个点，包括系留装置行驶起点

终点

以及中间关键点

各个点均有X和Y两个方向的分量，由(3)式可得均匀三次B样条曲线的二维矩阵表达式：

将B样条函数参数化处理之后，转换成了约束条件下求解最优控制点的问题。

针对约束条件1，将约束条件(1)作为约束的目标函数，保证曲线上m+1个起始位置和末端位置分别与最优路径的起点、中间关键点以及终点距离的平方和最小即可；

针对约束条件2，限制B样条曲线初始段和终点段的方位角即可；

针对约束条件3，理论上是将B样条曲线的每个曲线段各点均加入曲率约束，但是约束过多会导致优化失败，本发明采用的是步长约束，即在每段曲线上，取固定步长Δμ,μ∈[0,1)进行采点，在采集的点处进行曲率约束即可。

综上，B样条曲线控制点最优化问题数学模型如下：

其中，

θ₀和θ_m分别为起始段和终点段的方位角；Δμ为约束条件3中的固定步长，k为步长的整数倍，取值1,2,3…；R_min为最小曲率半径；

和

分别为式(4)的二阶导数和一阶导数。

本发明实施例中所用的路径规划方法优化前后对比如图2、图3所示。可以看出，优化后的控制点与预先设定的路径关键点拟合的曲线相似度更高一些。

最后，方程求解采用的是逐步二次规划法，由于其求解过程稳定、求解结果可靠的优势被广泛使用。由此得到的是直升机的最优路径，最终的系留装置最优路径只需根据上述求得的路径经过运动学解算即可得出。

二、虚拟引导力的生成

本发明根据系留装置规划的最优行驶路径，由人工势场理论构建相应的(机器自主)虚拟力，并与(操作人员手动)操纵力融合，通过阻抗控制模型输出操纵装置(手控器)需要产生的运动趋势，进而实现其对操作者人手的力/触觉引导效果。

1.人工势场建模

首先，引力场V_att的构建原则是随着系留装置距离目标位置的距离越远而单调递增。本发明采用了二次方增长的关系用于描述引力场与距离的关系，定义如下：

其中，d(r)＝||r₀-r||为系留装置当前位姿与期望位姿的距离，ζ表示的是引力势场的增益比例系数。

其次，斥力场V_rep的构建原则是随着系留装置距离障碍物距离的增大而减小，本发明斥力场的定义如下：

其中，d(r)＝||r₀-r||为系留装置当前位姿r与期望位姿r₀的距离；η为斥力势场中的增益比例系数，ρ₀为障碍物的作用距离，超过这个距离将不作为障碍物处理。

2.虚拟引导力建模

如图4所示，首先，依据系留装置的当前位置ξ_c，构建沿着期望路径的“吸引子”位置ξ_d：

ξ_d＝ξ(s_p+Δs) (8)

其中，s_p＝arg min_s||ξ_c-ξ(s)||，表示的是路径点ξ(s_p)为系留装置当前位置ξ_c在期望路径ξ(s)的最邻近投影点，Δs为前瞻量。

在此基础上，基于人工势场理论，构建“吸引子”位置ξ_d与系留装置当前位置ξ_c的虚拟引力F_ξ：

其中，

为梯度符号，

为势函数V_ξ的梯度向量的转置，λ为虚拟力的比例系数。

其次，根据直升机牵引过程中车身运动速度方向与障碍物的距离，构建系留装置的虚拟斥力F_γ如下：

其中，k是比例因子，ρ为系留装置沿其速度方向与周围环境的距离，ρ₀为系留装置与障碍物之间的安全距离，

为系留装置运动速度的单位向量。

三、基于虚拟引导力的人机共享控制

将上述求得的虚拟引力F_ξ和虚拟斥力F_γ经过一定的加权合成之后，然后映射至手控器的各个关节，得到手控器的虚拟引导力/力矩τ_a。

τ_a＝αF_ξ+βF_γ (11)

式中，α、β分别为不同引力、斥力的影响因数。

手控器在虚拟力/力矩τ_a、人手操纵力F_h共同作用下将产生期望运动趋势ξ_D。将手控器的关节运动简化为质量-阻尼系统，设计手控器的位置预测控制器：

其中，M、B分别为手控器的惯量矩阵和阻尼矩阵，K_a、K_h分别为手控器自主控制和操作者手动控制的权值系数，J_ξ为手控器的雅克比矩阵。

手控器向操作人员提供的力/触觉导航信息提示，宏观上表现为手控器产生对人手“推拽”的运动趋势，按下式(13)设计手控器的控制量u_m，使其跟踪位置预测控制器输出运动趋势信号。

其中，K_m、B_m分别为位置预测控制器的比例增益和微分增益，ξ_D为期望运动趋势，ξ_C为当前运动趋势。

如图5所示，虚拟引力与虚拟斥力经过加权合成虚拟合力之后，由系留装置空间映射至手控器关节空间，得到了虚拟引导力，之后虚拟引导力与操作者的操纵力共享融合，将产生的期望运动趋势动力学建模，设计手控器的位置预测控制器，之后设计操纵手控器的控制量，使其跟踪位置预测控制器输出的运动趋势信号，宏观上表现为对人手的“推拽”效果。

本发明提出的一种海上作业直升机快速回收的力觉引导遥操纵控制方法，通过系留装置操纵装置(手控器)上的虚拟力反馈，对操作人员的操纵行为予以引导，从而提高直升机回收的作业效率，降低对操作人员熟练程度的要求。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (6)

1.一种海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，所述方法包括：

根据实时反馈的直升机位置和船上降落区域信息规划出直升机的最优牵引路径，进而经过运动学解算得出系留装置的最优路径；

根据所述系留装置的最优路径，通过人工势场求解虚拟引导力；

将所述虚拟引导力同操作人员的操纵力共享融合，共同作为系留装置手控器的输入信号，通过力-位置混合控制实现系留装置手控器对操作人员行为的力觉引导；

其中，根据船上降落区域信息规划出直升机的最优牵引路径，包括：

利用船上降落区域信息获得直升机牵引的起点、终点以及中间关键点；

以所述直升机牵引的起点、终点以及中间关键点作为B样条函数曲线的型值点，利用所述型值点反求控制点，加入约束条件对所求的控制点进行优化处理，采用逐步二次规划的方法求解最优控制点，将所述最优控制点拟合成B样条函数曲线，得到直升机的最优牵引路径。

2.根据权利要求1所述的海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，所述B样条函数曲线为均匀三次B样条曲线。

3.根据权利要求1所述的海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，所述约束条件包括：通过事先预定好的最优路径关键点并期望每段曲线的初始位置能落在最优路径关键点上；直升机的起点和终点方位角对曲线首尾切线的约束；满足最优路径最小转弯半径对路径曲线的曲率约束。

4.根据权利要求1所述的海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，根据所述系留装置的最优路径，通过人工势场求解虚拟引导力，包括：

基于所述系留装置的最优路径，构建系留装置运动的人工势能场，所述系留装置的最优路径代表低势能区，船上建筑环境代表高势能区；所述低势能区产生虚拟引力，所述高势能区产生虚拟斥力；

对所述虚拟引力和所述虚拟斥力进行加权处理，得到所述系留装置的虚拟引导力。

5.根据权利要求4所述的海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，基于所述系留装置的最优路径，构建系留装置运动的人工势能场，包括：

基于随着系留装置距离目标位置的距离越远而单调递增的原则构建引力场V_att：

其中，d(r)＝||r₀-r||为系留装置当前位姿r与期望位姿r₀的距离，ζ表示的是引力势场的增益比例系数；

基于随着系留装置距离障碍物距离的增大而减小的原则构建斥力场V_rep：

其中，ρ(r)为系留装置沿其速度方向与障碍物边界的最短距离；η为斥力势场中的增益比例系数，ρ₀为障碍物对系留装置的作用距离，超过这个距离将不作为障碍物处理；

依据系留装置的当前位置ξ_c，构建沿着期望路径的“吸引子”位置ξ_d：

ξ_d＝ξ(s_p+Δs)；

其中，s_p＝argmin_s||ξ_c-ξ(s)||，表示的是路径点ξ(s_p)为系留装置当前位置ξ_c在期望路径ξ(s)的最邻近投影点，Δs为前瞻量；

基于上述构建的引力场和斥力场，构建“吸引子”位置ξ_d与系留装置当前位置ξ_c的虚拟引力F_ξ：

其中，

为梯度符号，

为势函数V_ξ的梯度向量的转置，λ为虚拟力的比例系数；

根据直升机牵引过程中车身运动速度方向与障碍物的距离，构建系留装置的虚拟斥力F_γ如下：

其中，k是比例因子，ρ₀为障碍物对系留装置的作用距离，

为系留装置运动速度的单位向量。

6.根据权利要求5所述的海上作业直升机快速回收的力觉引导遥操纵控制方法，其特征在于，将所述虚拟引导力同操作人员的操纵力共享融合，共同作为系留装置手控器的输入信号，通过力-位置混合控制实现系留装置手控器对操作人员行为的力觉引导，包括：

将所述虚拟引力F_ξ和所述虚拟斥力F_γ进行加权合成，映射至手控器的各个关节，得到手控器的虚拟引导力τ_a：

τ_a＝αF_ξ+βF_γ；

其中，α、β分别为不同引力、斥力的影响因数；

手控器在虚拟引导力τ_a、操作人员的操纵力F_h共同作用下，产生期望运动趋势ξ_D；将手控器的关节运动简化为质量-阻尼系统，基于所述运动趋势设计手控器的位置预测控制器：

其中，M、B分别为手控器的惯量矩阵和阻尼矩阵，K_a、K_h分别为手控器自主控制和操作者手动控制的权值系数，J_ξ为手控器的雅克比矩阵；

按下式设计手控器的控制量u_m，使其跟踪位置预测器输出运动趋势信号；

其中，K_m、B_m分别为位置预测控制器的比例增益和微分增益，ξ_D为期望运动趋势，ξ_C为当前运动趋势。

Images