CN104503231B - 两栖蛙板机器人的摆臂驱动式运动控制方法 - Google Patents

Abstract

本发明提供一种两栖蛙板机器人的摆臂驱动式运动控制方法，具体过程为：测量机器人的腿臂位置θ_a和机器人速度V_c；令机器人质心在前进方向上的加速度a_c＝K×(V_aim‑V_c)，V_aim为目标速度，K为预设参数；利用θ_a、V_c和a_c，根据所建立的机器人在陆地和水中的前进运动控制模型，计算电机控制量ω；根据所述电机控制量ω控制电机驱动机器人在陆地或水中的前进运动。本发明运用统一的控制器即可实现机器人水陆环境中的纵向速度控制，大大简化了两栖机器人控制算法的复杂程度，避免了两栖机器人因进行环境检测、执行机构切换和控制算法切换带来的时间和能源的损耗。

Description

两栖蛙板机器人的摆臂驱动式运动控制方法

技术领域

本发明涉及一种两栖机器人的运动控制方法，特别涉及一种两栖蛙板机器人的摆臂驱动式运动控制方法。

背景技术

在机器人技术中，运动控制算法的设计影响机器人的整体性能。水陆两栖机器人因其多变的环境和复杂的任务需求，对运动控制算法有着更高的要求。在陆地上，运动控制算法主要针对机器人的纵向前进速度和横向的方向进行控制；在水中时，运动控制算法则需要控制机器人的纵向前进速度和水下姿态。对于机器人的运动控制有很多控制算法，如PID控制、模糊控制、最优控制、鲁棒控制、神经网络控制方法等。而两栖机器人的控制难点在于机器人运动控制算法的适应性。在水陆环境转换时，由于外部环境变化、机器人执行机构切换等因素，机器人的控制算法需要进行适应性地改变。

发明内容

本发明的目的是提出一种两栖蛙板机器人的摆臂驱动式运动控制方法，该方法可以适应机器人水陆运动的需求，使用同种控制算法即可实现机器人在不同环境的纵向速度控制。

实现本发明的技术方案如下：

一种两栖蛙板机器人的摆臂驱动式运动控制方法，具体过程为：

(1)测量机器人的腿臂位置θ_a和机器人速度V_c；

(2)令机器人质心在前进方向上的加速度a_c＝K×(V_aim-V_c)，V_aim为目标速度，K为预设参数；

(3)利用θ_a、V_c和a_c，根据所建立的机器人在陆地和水中的前进运动控制模型，计算电机控制量ω，所述前进运动控制模型如公式(1)所示；

其中，M为机器人质量，ρ为水的密度，S₁为机器人左右尾鳍的面积，L_a机器人腿臂长度，当机器人在陆地前进时，β_a表示左右后轮的偏转角度，当机器人在水中前进时，β_a表示左右尾鳍的变形角度，S₃为机器人前进时的等效对水面积，e₁,e₂分别为机器人腿部侧面的长和宽；

(4)根据所述电机控制量ω控制电机驱动机器人在陆地或水中的前进运动。

进一步地，本发明当机器人处于水中时，其俯仰姿态控制过程为：

S101、测量机器人俯仰的角加速度ω₁和机器人速度V_c；

S102、利用ω₁和V_c，根据所建立的机器人在水中俯仰姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝θ₂；

其中，J₁为机器人在俯仰方向的转动惯量，β₁为设定的机器人俯仰的角加速度，A为左右胸鳍的面积，L₁为机器人在水中做俯仰运动时的力臂，b₁,b₂分别为机器人外壳前后端到机器人质心的距离；

S103、根据所计算的偏转角度θ₁和θ₂，控制机器人的俯仰运动。

进一步地，本发明当机器人处于水中时，其滚转姿态控制过程为：

S201、测量机器人滚转的角加速度ω₂和机器人速度V_c；

S202、利用ω₂和V_c，根据所建立的机器人在水中滚转姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝-θ₂；

其中，J₂为机器人在滚转方向的转动惯量，β₂为设定的机器人滚转的角加速度，L₂为机器人在水中做滚转运动时的力臂，a为机器人头部宽度；

S203、根据所计算的偏转角度θ₁和θ₂，控制机器人的滚转运动。

进一步地，本发明利用神经网络实现对机器人水下俯仰和滚转角的同时控制，其中所利用的神经网络的层数为2层；第一层神经元的个数为5个，对应的输入变量为ω₁，ω₂，ω_1aim，ω_2aim和V_c，第二层神经元个数为2个，对应的输出变量为θ₁和θ₂；第一层和第二层传递函数均是tan-sigmoid，神经网络的训练方法选用弹性梯度下降法trainrp；具体过程为：

301、测量机器人在俯仰和滚转方向上的运动角速度ω₁和ω₂，测量机器人前进速度V_c；

302、设定机器人的目标俯仰角速度和滚转角速度为ω_1aim和ω_2aim；

303、将ω₁，ω₂，ω_1aim，ω_2aim和V_c输入给神经网络，经过解算得到机器人左右胸鳍的偏转角度θ₁和θ₂。

S303、根据所计算的偏转角度θ₁和θ₂，控制机器人的姿态运动。

有益效果：

(1)本发明运用统一的控制器即可实现机器人水陆环境中的纵向速度控制，大大简化了两栖机器人控制算法的复杂程度，避免了两栖机器人因进行环境检测、执行机构切换和控制算法切换带来的时间和能源的损耗。

(2)本发明针对机器人的水下姿态控制，利用基础控制算法的计算结果训练神经网络控制算法，可同时控制机器人俯仰运动和滚转运动，使得两栖蛙板机器人的水下姿态控制算法在极限情况下也可以得到稳定的控制量，适用范围更广、运算量小。

附图说明

图1机器人整体结构示意图；

图2机器人结构参数示意图；

图3机器人结构参数示意图；

图4机器人结构参数示意图；

图5为本发明两栖蛙板机器人的摆臂驱动式运动控制方法的流程图；

图6神经网络控制算法系统结构；

图7机器人纵向速度控制效果曲线图；

图8机器人水下姿态控制效果曲线图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

图1本实例中针对的两栖蛙板机器人的结构示意图。图中1为头部，2为腿部，3为胸鳍，4为尾鳍，5为前轮和前舵，6为后轮。头部内安装有驱动腿部运动的电机、驱动胸鳍转动的舵机以及测量机器人运动的传感器如光电编码器、陀螺仪和流速计。图2、图3、图4是机器人上重要物理参数的定义，用于建立机器人的动力学模型。O为机器人质心，M为机器人质量，a_c为机器人质心在前进方向上的加速度，V_c为机器人质心在前进方向上的速度，ρ为水的密度，e₁,e₂分别为腿部侧面的长和宽。J₁,J₂分别为机器人在俯仰和滚转方向的转动惯量，β₁,β₂分别为机器人俯仰和滚转的角加速度，ω₁,ω₂分别为机器人俯仰和滚转的角加速度，S₁＝S₂为机器人左右尾鳍的面积，S₃为机器人前进时的等效对水面积，β_a,β_b在路上分别表示左右后轮的偏转角度，在水中分别为左右尾鳍的变形角度，θ_a,θ_b分别为左右腿的摆动角度，θ₁,θ₂分别为左右胸鳍的偏转角度，L₁,L₂机器人在水中做俯仰运动和滚转运动时的力臂，b₁,b₂分别为机器人外壳前后端到机器人质心的距离。

本发明的设计思想为：分析两栖蛙板机器人在水陆环境下运动的动力学模型，提出形式统一的水陆环境下的机器人直行的初等控制算法，以及水中的姿态初等控制算法。在此基础上，为了提高控制算法的鲁棒性和适用范围，设计了神经网络控制算法，利用初等控制算法提供网络的训练样本，实现机器人的两栖运动控制算法。

如图5所示，本发明一种两栖蛙板机器人的摆臂驱动式运动控制方法，具体过程为：

(1)测量机器人的腿臂位置θ_a和机器人速度V_c；

(2)令机器人质心在前进方向上的加速度a_c＝K×(V_aim-V_c)，V_aim为目标速度，K为预设参数；

(3)根据所建立的机器人在陆地和水中的前进运动控制模型，计算电机控制量ω，所述前进运动控制模型如公式(1)所示；

其中，M为机器人质量，ρ为水的密度，S₁为机器人左右尾鳍的面积，L_a机器人腿臂长度，当机器人在陆地前进时，β_a表示左右后轮的偏转角度，当机器人在水中前进时，β_a表示左右尾鳍的变形角度，S₃为机器人前进时的等效对水面积，e₁,e₂分别为腿部侧面的长和宽；

(4)根据所述电机控制量ω控制电机驱动机器人在陆地或水中的前进运动。

本发明在通过反馈机器人的状态量θ_a和V_c，利用式(1)即可解算出电机控制量ω，再将其转化为PWM控制信号，实现机器人的控制。针对水路两种环境提出了统一的前进运动控制模型，大大简化了两栖机器人控制算法的复杂程度，避免了两栖机器人因进行环境检测、执行机构切换和控制算法切换带来的时间和能源的损耗。

下面对前进运动控制模型的推导过程进行简单说明：

基本假设：机器人的结构对称时，有：L_a＝L_b，S₁＝S₂；机器人前进时为对称运动，有：β_a＝β_b，θ_a＝θ_b，因此有F_a＝F_b，其中F_a,F_b分别为左右尾鳍提供的推力；机器人的质心和浮心重合，机器人静止在水中时不会发生俯仰；尾鳍的曲面受到的合力等效为一个平面受到的合力。

由此，可以进行机器人的动力学分析。由牛顿第二定律可知：

Ma_c＝F-f (1.1)

根据平面在水下运动时的力学公式：

其中，S为平面垂直于水流方向的有效对水面积，V为平面相对于水流的流速；

对于柔性的尾鳍，可以认为β_a＝kω²，k为常系数，可由实验测定。

f＝f₁+f₂+f₃ (1.4)

其中，f₁,f₂,f₃分别是头部、左腿和右腿受到的阻力。

其中，S₃是头部的有效对水面积。

故由(1.3)和(1.7)可以推导得出机器人在陆地和水中的前进模型，统一形式如下。

由于陆地和水中环境的物理参数不同，模型中的参数会有差异，但是该模型的形式统一，可以直接用于机器人的水陆纵向的速度控制。

本发明当机器人处于水中时，其俯仰姿态控制过程为：

S101、测量机器人俯仰的角加速度ω₁和机器人速度V_c；

S102、根据所建立的机器人在水中俯仰姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝θ₂；

其中，J₁为机器人在俯仰方向的转动惯量，β₁为设定的机器人俯仰的角加速度，A为左右胸鳍的面积，L₁为机器人在水中做俯仰运动时的力臂，b₁,b₂分别为机器人外壳前后端到机器人质心的距离；

S103、根据所计算的偏转角度θ₁和θ₂，控制机器人的俯仰运动。

下面对机器人在水中俯仰姿态控制的动力学模型的推导过程进行简单说明：

机器人在水中的俯仰和滚转运动均为角运动，则由角动量定理分析得：

J₁β₁＝(F₁cosθ₁+F₂cosθ₂)L₁-M₁ (2.1)

其中F₁和F₂分别为左右胸鳍受到的水的推力，方向垂直于胸鳍所在平面。M₁为流体对机器人俯仰运动的阻力矩。

将式(2.2)(2.3)和(2.4)代入式(2.1)可得机器人俯仰方向的运动模型

其中

本发明当机器人处于水中时，其滚转姿态控制过程为：

S201、测量机器人滚转的角加速度ω₂和机器人速度V_c；

S202、根据所建立的机器人在水中滚转姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝-θ₂；

其中，J₂为机器人在滚转方向的转动惯量，β₂为设定的机器人滚转的角加速度，L₂为机器人在水中做滚转运动时的力臂，

S203、根据所计算的偏转角度θ₁和θ₂，控制机器人的滚转运动。

下面对机器人在水中滚转姿态控制的动力学模型的推导过程进行简单说明：

与俯仰姿态控制的分析类似，F₁和F₂分别为左右胸鳍受到的水的推力，方向垂直于胸鳍所在平面。M₂为流体对机器人滚转运动的阻力矩。

J₂β₂＝(F₁cosθ₁-F₂cosθ₂)L₂-M₂ (3.1)

将式(2.3)(2.4)和(3.2)代入式(3.1)则机器人的横滚方向的动力学方程为

其中

本发明在初等控制算法的基础上，为了实现对机器人水下俯仰角和滚转角的同时控制，并且得到适用范围更大、运算速度更快的机器人水下姿态控制算法，设计了针对蛙板机器人姿态控制的神经网络控制算法，如图6为神经网络控制算法系统框图。以初等控制算法运算的结果为训练样本，训练神经网络控制算法。

具体配置如下：

网络层数：2

●输入层(第一层)神经元个数：5，对应于变量ω₁，ω₂，ω_1aim，ω_2aim和V_c

●输出层(第二层)神经元个数：2，对应于胸鳍控制量θ₁和θ₂

●输入向量的5个元素的取值范围分别对应于[-0.4,0.4],[-0.4,0.4],[-0.4,0.4],[-0.4,0.4],[-0.5,0.5]，该取值范围即对应于变量ω₁，ω₂，ω_1aim，ω_2aim和V_c的取值范围。

●第一层和输出层的传递函数均是tan-sigmoid

该传递函数的一般形式为

其中，缩放系数T和位移参数θ决定了单个神经元的响应特性。

●网络的训练方法选用弹性梯度下降法trainrp

●输入层到输出层的权值为[1,1]，阈值为1，输出层权值为[2,1]，阈值为2

该神经网络训练样本来源于实际初等控制算法的计算结果。因为初等控制算法的计算过程中要通过解非线性方程组来获得控制量，计算量大且解不一定存在，通过初等控制算法的解样本训练该神经网络，并利用该神经网络计算控制量可以简化计算过程且避免了解不存在的问题。

具体过程为：

301、测量机器人在俯仰和滚转方向上的运动角速度ω₁和ω₂，测量机器人前进速度V_c；

302、设定机器人的目标俯仰角速度和滚转角速度为ω_1aim和ω_2aim；

303、将ω₁，ω₂，ω_1aim，ω_2aim和V_c输入给神经网络，经过解算得到机器人左右胸鳍的偏转角度θ₁和θ₂。

S303、根据所计算的偏转角度θ₁和θ₂，控制机器人的姿态运动。

本发明在初等控制算法基础上设计的改进控制算法，利用神经网络控制算法实现，算法的鲁棒性强、使用范围广、运算量小。

实例：

以实际的两栖蛙板机器人样机的控制为例，实测的机器人参数如下：

b₁,b₂机器人外壳前后端到机器人质心的距离，b₁＝0.3m b₂＝0.1m，机器人头部宽度a＝0.3m，左右胸鳍的面积A＝0.02m²，机器人俯仰运动和滚转运动的转动惯量J₁＝0.134kgm²，J₂＝0.287kgm²，机器人质量M＝15kg，水密度ρ＝1000kg/m³，尾鳍面积S₁＝S₂＝0.04m²，腿臂长度L_a＝0.5m，机器人对水面积S₃＝0.057m²，腿部侧面长度e₁＝0.4m，腿部侧面宽度e₂＝0.05m，刚度常数k＝0.75。

带入以上参数，可得两栖蛙板机器人的动力学模型的具体表达：

两栖蛙板机器人在水陆环境的纵向速度控制动力学模型

两栖蛙板机器人在水中的俯仰运动的动力学模型

两栖蛙板机器人在水中的滚转运动的动力学模型

以这三个具体的动力学模型为基础，具体的水陆环境下的纵向速度控制和水中的运动姿态控制算法如下(其中给定目标前进速度V_aim＝0.3m/s，控制周期0.01s，腿臂摆动范围θ_a,θ_b∈(0,90°))。

机器人在陆地和水中的纵向速度控制算法步骤

(1)利用光电编码器测量腿臂位置θ_a，利用光电编码器(陆上)和流速计(水中)测量机器人前进速度V_c。

(2)令a_c＝K*(V_aim-V_c)，K＝3为可调参数。

(3)将得到的将a_c,θ_a,V_c代入式(4)，求解方程得到ω。

(4)将ω转换为控制电机的PWM信号的占空比，作为该控制周期内的控制输出。

图7是机器人的纵向速度控制的MATLAB仿真效果，其中根据机器人实际使用的执行机构的极限，为电机添加约束条件，其角速度范围为ω∈(-7.24rad/s,7.24rad/s)，角加速度β∈(-550rad/s²,550rad/s²)。

机器人在水中运动姿态控制算法步骤为：

机器人在水中俯仰运动的算法设计流程可参见附图。以初等控制器为基础训练机器人水下姿态控制的神经网路控制算法，用于实时控制机器人在水中的运动姿态。神经网络算法的具体步骤如下，(其中控制周期0.01s，腿臂摆动范围θ_a,θ_b∈(0,90°)，胸鳍角度范围θ₁,θ₂∈(-90°,90°))。

(1)机器人内部安装的陀螺仪测量机器人在俯仰和滚转方向上的运动角速度，分别为ω₁和ω₂，利用流速计测量机器人前进速度V_c。

(2)给定机器人的目标俯仰角速度和滚转角速度为ω_aim1＝ω_aim2＝0.3rad/s，。

(3)将ω_aim1,ω_aim2,ω₁,ω₂,V_c作为输入提供给神经网络姿态控制算法，该算法经过解算得到机器人胸鳍应输出的角度θ₁,θ₂。

(4)将θ转换为控制舵机的PWM信号的占空比，作为该控制周期内的控制输出。

图8为机器人的水下姿态控制的MATLAB仿真效果。胸鳍很快达到稳定状态，机器人的姿态角速度也在段时间内达到目标角速度。

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

Claims (4)

1.一种两栖蛙板机器人的摆臂驱动式运动控制方法，其特征在于，具体过程为：

(1)测量机器人的腿臂位置θ_a和机器人速度V_c；

(2)令机器人质心在前进方向上的加速度a_c＝K×(V_aim-V_c)，V_aim为目标速度，K为预设参数；

(3)利用θ_a、V_c和a_c，根据所建立的机器人在陆地和水中的前进运动控制模型，计算电机控制量ω，所述前进运动控制模型如公式(1)所示；

${\mathrm{Ma}}_c={\mathrm{\ρS}}_1{\mathrm{\ω}}^2{L}_a^2{\mathrm{cos\β}}_a\sin ({\mathrm{\θ}}_a+{\mathrm{\β}}_a)-\frac{1}{2}{\mathrm{\ρS}}_3{V}_c^2-{\mathrm{\ρe}}_2{\mathrm{sin\θ}}_a(V_c{e}_1+\frac{\mathrm{\ω}}{2}{e}_1^2{\mathrm{sin\θ}}_a)---\left(1\right)$

其中，M为机器人质量，ρ为水的密度，S₁为机器人左右尾鳍的面积，L_a机器人腿臂长度，当机器人在陆地前进时，β_a表示左右后轮的偏转角度，当机器人在水中前进时，β_a表示左右尾鳍的变形角度，S₃为机器人前进时的等效对水面积，e₁,e₂分别为机器人腿部侧面的长和宽；

(4)根据所述电机控制量ω控制电机驱动机器人在陆地或水中的前进运动。

2.根据权利要求1所述两栖蛙板机器人的摆臂驱动式运动控制方法，其特征在于，当机器人处于水中时，其俯仰姿态控制过程为：

S101、测量机器人俯仰的角加速度ω₁和机器人速度V_c；

S102、利用ω₁和V_c，根据所建立的机器人在水中俯仰姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝θ₂；

$J_1{\mathrm{\β}}_1=\frac{1}{2}{\mathrm{\ρAV}}_c^2{L}_1({\mathrm{sin\θ}}_1{\mathrm{cos\θ}}_1+{\mathrm{sin\θ}}_2{\mathrm{cos\θ}}_2)-k_1{\mathrm{\ω}}_1^2---\left(2\right)$

$k_1=\frac{{\mathrm{\ρb}}_1^4}{40(b_1+b_2)}+\frac{\mathrm{\ρ}}{2(b_1+b_2)}(\frac{b_1{b}_2^4}{4}+\frac{b_2^5}{5})$

其中，J₁为机器人在俯仰方向的转动惯量，β₁为设定的机器人俯仰的角加速度，A为左右胸鳍的面积，L₁为机器人在水中做俯仰运动时的力臂，b₁,b₂分别为机器人外壳前后端到机器人质心的距离；

S103、根据所计算的偏转角度θ₁和θ₂，控制机器人的俯仰运动。

3.根据权利要求1所述两栖蛙板机器人的摆臂驱动式运动控制方法，其特征在于，当机器人处于水中时，其滚转姿态控制过程为：

S201、测量机器人滚转的角加速度ω₂和机器人速度V_c；

S202、利用ω₂和V_c，根据所建立的机器人在水中滚转姿态控制的动力学模型，计算机器人左右胸鳍的偏转角度θ₁和θ₂，其中计算时令θ₁＝-θ₂；

$J_2{\mathrm{\β}}_2=\frac{1}{2}{\mathrm{\ρAV}}_c^2{L}_2({\mathrm{sin\θ}}_1{\mathrm{cos\θ}}_1-{\mathrm{sin\θ}}_2{\mathrm{cos\θ}}_2)-k_2{\mathrm{\ω}}_2^2---\left(3\right)$

$k_2=\frac{{\mathrm{\ρa}}^4(b_1+b_2)}{20}$

其中，J₂为机器人在滚转方向的转动惯量，β₂为设定的机器人滚转的角加速度，L₂为机器人在水中做滚转运动时的力臂，a为机器人头部宽度，b₁,b₂分别为机器人外壳前后端到机器人质心的距离，A为左右胸鳍的面积；

S203、根据所计算的偏转角度θ₁和θ₂，控制机器人的滚转运动。

4.根据权利要求1所述两栖蛙板机器人的摆臂驱动式运动控制方法，其特征在于，当机器人处于水中，进行俯仰和滚转姿态同时控制时：

建立的机器人在水中俯仰姿态控制的动力学模型，

$J_1{\mathrm{\β}}_1=\frac{1}{2}{\mathrm{\ρAV}}_c^2{L}_1({\mathrm{sin\θ}}_1{\mathrm{cos\θ}}_1+{\mathrm{sin\θ}}_2{\mathrm{cos\θ}}_2)-k_1{\mathrm{\ω}}_1^2---\left(2\right)$

其中，J₁为机器人在俯仰方向的转动惯量，β₁为设定的机器人俯仰的角加速度，L₁为机器人在水中做俯仰运动时的力臂，A为左右胸鳍的面积；

建立的机器人在水中滚转姿态控制的动力学模型，

$J_2{\mathrm{\β}}_2=\frac{1}{2}{\mathrm{\ρAV}}_c^2{L}_2({\mathrm{sin\θ}}_1{\mathrm{cos\θ}}_1-{\mathrm{sin\θ}}_2{\mathrm{cos\θ}}_2)-k_2{\mathrm{\ω}}_2^2---\left(3\right)$

其中，J₂为机器人在滚转方向的转动惯量，β₂为设定的机器人滚转的角加速度，L₂为机器人在水中做滚转运动时的力臂；

利用神经网络实现对机器人水下俯仰和滚转角的同时控制，其中所利用的神经网络的层数为2层；第一层神经元的个数为5个，对应的输入变量为ω₁，ω₂，ω_1aim，ω_2aim和V_c，第二层神经元个数为2个，对应的输出变量为θ₁和θ₂；第一层和第二层传递函数均是tan-sigmoid，神经网络的训练方法选用弹性梯度下降法trainrp；具体过程为：

301、测量机器人在俯仰和滚转方向上的运动角速度ω₁和ω₂，测量机器人前进速度V_c；

302、设定机器人的目标俯仰角速度和滚转角速度为ω_1aim和ω_2aim；

303、将ω₁，ω₂，ω_1aim，ω_2aim和V_c输入给神经网络，经过解算得到机器人左右胸鳍的偏转角度θ₁和θ₂；

S303、根据所计算的偏转角度θ₁和θ₂，控制机器人的姿态运动。