无人艇航向运动控制方法与流程

文档序号:13222450阅读:来源:国知局
技术特征:
1.一种无人艇航向运动控制方法,其特征在于,包括:接收传感器模块采集的无人艇实时航向角,所述传感器模块包括:陀螺仪、加速度计以及磁场强度传感器;对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率;采用模糊PID控制算法根据所述航向偏差与所述航向偏差率确定指令舵角,所述指令舵角包括打舵方向和打舵速度;将所述指令舵角发送至电机驱动器;所述电机驱动器根据所述打舵方向和所述打舵速度控制无人艇运动。2.根据权利要求1所述的方法,其特征在于,所述对比所述实时航向角与所述设定航向角得到无人艇航向偏差、航向偏差率之前,还包括:接收陀螺仪检测加速度计三个轴的角速度,磁场强度传感器采集无人艇的航向角,加速度计采集无人艇的横摇角、纵摇角;根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角;将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角。3.根据权利要求2所述的方法,其特征在于,所述根据所述角速度采用卡尔曼滤波将修正所述加速度计采集的无人艇的横摇角、纵摇角,包括:根据姿态传感器模块中的陀螺仪采集的角速率偏差、角速率构造系统的状态方程为αβk=1-Δt01αβk-1+Δt0ωk-1+wgΔt0---(1)]]>量测方程为Z(k)=[10]αβ+wa---(2)]]>其中,α表示姿态角,所述姿态角包括横摇角、纵摇角,β表示陀螺仪输出的角速率偏差,Δt表示采样周期,ωk-1表示k-1(k=1,2,…,n)时刻陀螺仪检测的角速率,wg表示陀螺仪输出的白噪声,wa为加速度计输出白噪声,所述Z(k)为加速度计的测量值;根据状态方程和量测方程结合公式X(k|k-1)=AX(k-1|k-1)+BU(k-1)(3)得到k-1时刻的最优姿态角,由k-1时刻的最优姿态角预测当前时刻的姿态角,其中,X(k-1|k-1)为k-1时刻的最优姿态角,X(k|k-1)为k时刻的初步估计姿态角,U(k)为当前时刻的控制量,所述A为系统矩阵,所述B为控制输入矩阵;根据公式P(k|k-1)=AP(k-1|k-1)AT+Q(4)计算预测误差的协方差,其中,P(k|k-1)为k时刻预测误差的协方差,P(k-1|k-1)为k-1时刻最优估计值的协方差,Q为系统噪声协方差,AT为系统矩阵的转置矩阵;根据所述预测误差的协方差,采用公式Kg(k)=P(k|k-1)HT[HP(k|k-1)HT+R]-1(5)计算卡尔曼增益,其中,所述Kg为卡尔曼滤波器增益,H为观测矩阵,R为测量噪声协方差矩阵,所述HT为观测矩阵的转置矩阵;根据所述卡尔曼增益,采用公式X(k|k)=X(k|k-1)+Kg(k)[Z(k)-HX(k|k-1)](6)修正k时刻的最优姿态角,其中,所述Z(k)为加速度计的测量值。4.根据权利要求3所述的方法,其特征在于,所述将所述修正后的横摇角、纵摇角与所述航向角融合确定精确航向角,包括:根据修正后的横摇角、纵摇角采用公式Mh=CbhMb---(7)]]>将所述航向角由传感器坐标系转换到水平坐标系内的磁场强度,其中,Mb为对应传感器坐标系的磁场强度,Mh为对应水平坐标系的磁场强度,为转换矩阵;根据水平坐标系内的磁场强度的分量计算无人艇艇体平面的磁航向角;根据所述磁航向角与磁偏角确定精确航向角。5.根据权利要求1所述的方法,其特征在于,所述采用模糊PID控制算法根据所述航向偏差确定指令舵角,包括:采用三角形函数f(x,a,b,c)=0,x≤ax-ab-a,a≤x≤bc-xc-b,b≤x≤c0,c≤x---(8)]]>将航向偏差、航向偏差率的精确值进行模糊化处理,得到模糊输出集合,其中,x为航向偏差或航向偏差率,a,b,c指定三角形函数的形状,且要求a≤b≤c;根据所述模糊输出集合确定模糊整定表,并根据所述模糊整定表整定所述模糊输出集合确定PID控制器的比例系数、积分系数以及微分系数;采用加权平均法对所述比例系数、积分系数以及微分系数惊醒模糊判决,获得所述比例系数、积分系数以及微分系数的精确值;将所述比例系数、积分系数以及微分系数的精确值输入所述PID控制器获取指令舵角。6.根据权利要求5所述的方法,其特征在于,所述模糊整定表为:其中,e为航向偏差,ec为航向偏差率,NB、NM、NS、Z、PS、PM、PB、PM为模糊子集,kp为比例系数;其中,ki为积分系数;其中,kd为微分系数。
当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1