CN112696981A - 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 - Google Patents
一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 Download PDFInfo
- Publication number
- CN112696981A CN112696981A CN202011518365.5A CN202011518365A CN112696981A CN 112696981 A CN112696981 A CN 112696981A CN 202011518365 A CN202011518365 A CN 202011518365A CN 112696981 A CN112696981 A CN 112696981A
- Authority
- CN
- China
- Prior art keywords
- azimuth
- low
- control
- coefficient
- command
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 23
- 238000011105 stabilization Methods 0.000 title claims abstract description 18
- 238000001914 filtration Methods 0.000 claims abstract description 22
- 238000012937 correction Methods 0.000 claims abstract description 15
- 238000005259 measurement Methods 0.000 claims description 9
- 238000013461 design Methods 0.000 claims description 8
- 230000004907 flux Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 3
- 230000008901 benefit Effects 0.000 abstract description 4
- 238000010304 firing Methods 0.000 abstract description 3
- 238000011217 control strategy Methods 0.000 abstract description 2
- 230000004044 response Effects 0.000 abstract description 2
- 230000005540 biological transmission Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 5
- 230000006641 stabilisation Effects 0.000 description 5
- 230000008030 elimination Effects 0.000 description 4
- 238000003379 elimination reaction Methods 0.000 description 4
- 239000003638 chemical reducing agent Substances 0.000 description 3
- 230000000087 stabilizing effect Effects 0.000 description 3
- 230000033228 biological regulation Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003750 conditioning effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F41—WEAPONS
- F41G—WEAPON SIGHTS; AIMING
- F41G5/00—Elevating or traversing control systems for guns
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F41—WEAPONS
- F41G—WEAPON SIGHTS; AIMING
- F41G5/00—Elevating or traversing control systems for guns
- F41G5/26—Apparatus for testing or checking
Landscapes
- Engineering & Computer Science (AREA)
- General Engineering & Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
本发明提供了一种大地坐标系下全闭环干扰速率补偿自稳定控制方法,采集SINS的航向角、俯仰角和横滚角,以及火炮的高低角和方位角;采集SINS陀螺组、炮塔陀螺组、车体陀螺的角速度值,提取大地坐标系下方位向和高低向的干扰角速率,进行滤波校正,得到方位和高低补偿角速率控制量;合并稳定控制的速率控制指令和干扰补偿角速率控制指令,作为伺服驱动的速度总指令;高低和方位伺服驱动器采集各自电机角速率,并根据速度控制指令进行高阶滑模速度控制策略计算得到电流环控制指令,驱动电机按给定的控制量控制火炮调转运动。本发明具有响应带宽高、干扰速率补偿精准等优点,实现了动基座大射角条件下的火炮身管指向高精度稳定。
Description
技术领域
本发明属于火炮稳定控制***领域,主要涉及需要高动态动基座下精确稳定的火炮随动***控制方法。
背景技术
随着军事斗争的发展演变,新军事作战需求迫切需要压制武器具备行进间射击的能力,需要自行火炮能够遂行动对静、动对动的作战任务。那么火炮需要在瞄准稳定***的控制下,克服车体由于行走的路面对车体造成的扰动,保持火炮指向稳定。此类功能在坦克武器或两栖突击炮的炮控***早已得到实现。
专利“采用捷联惯导的火炮瞄准稳定***控制方法”应用于力矩电机直接拖动或者性能优异减速机的自行火炮稳定***上,其主要特点是采用捷联惯导(SINS)的航姿作为火炮随动***的空间角度反馈,和采用SINS中的三轴角速率陀螺作为速度闭环及干扰速率补偿的速率敏感器件,并观测高低侧角器和方位侧角器的角速度,获得炮塔和车体炮塔的角速率,从而完成火炮的稳定控制。但在工程中发现,传统压制火炮经过一定的适应改进,实现火炮大射角下火炮自稳定,减速机性能较差,欠刚度,齿隙大,难以满足陀螺速度闭环的要求,甚至影响测量火炮身管指向的捷联惯导作为位置全闭环控制***的稳定性,造成小误差抖动,齿隙“冲振”现象明显。通过安装在控制末端的捷联惯导中的陀螺推算炮塔速率和车体速率,由于受传动齿隙和刚度影响,会携带谐振产生的附加干扰角速度,其推算结果也难以满足使用要求。因而,该方法在传统高速电机结合低性能减速机上火炮难以实现火炮指向在高动态环境下精确稳定瞄准。
发明内容
为了克服现有技术的不足,本发明提供一种大地坐标系下全闭环干扰速率补偿自稳定控制方法,采用SINS作为火炮随动***的空间角度全闭环控制***,抑制车体姿态对火炮身管指向的扰动,实现火炮的高精度自稳定控制方法问题,同时实现高低向和方位向随动的控制解耦。针对利用SINS直接测量火炮身管指向,提高指向控制精度,又可以使自行火炮武器***完成自主定位导航功能,本发明仍然保留传统火炮随动***组成的侧角器,采用SINS的陀螺组、高低侧角器,炮塔陀螺组、车体陀螺等提取火炮干扰量,实现火炮的全闭环高精度自稳定。
本发明解决其技术问题所采用的技术方案包括以下步骤:
(1)启动控制,设定控制步数为j和i,其中j为稳定***速度环控制步数,控制周期Ts=1ms;i为稳定***位置环的控制步数,控制周期T=10ms;j和i初值为0;
(2)判断j%10=0是否成立,若是,控制步数i值加1,并转入第(3)步;否则转入第(9)步;
(4)计算航向角和俯仰角的带阻滤波ψ′(i)和θ′(i);
(5)采集高低侧角器测量值εb(i)和方位侧角器的测量值βb(i);
(6)判断是否同时接收到大地坐标下的火炮瞄准方位控制指令ψref(j)、高低控制指令θref(j),若是,进入第(7)步;否则转入第(9)步;
(7)求取方位位置控制误差eβ(i)和高低位置控制误差eε(i);
(9)采集安装在火炮摇架上SINS的陀螺组所测量的三轴角速率ωp(j),其中ωp(j)=[ωp1(j),ωp2(j),ωp3(j)]T,ωp1(j),ωp2(j),ωp3(j)分别惯导三轴陀螺轴角速率陀螺的测量值;
(10)采集炮塔陀螺组三轴陀螺所测量的三轴角速率ωh(j),其中ωh(j)=[ωh1(j),ωh2(j),ωh3(j)]T,ωh1(j),ωh2(j),ωh3(j)分别炮塔陀螺三轴陀螺轴角速率陀螺的测量值;
(11)采集车体陀螺组测量的角速率ωb(j);
(12)计算高低补偿角速率dε(j)和方位干扰补偿角速率dβ(j);
(13)计算高低干扰滤校正量udε(j)和方位干扰滤波校正量udβ(j);
所述的航向角和俯仰角的带阻滤波:
ψ′(i)=b10ψ(i)+b11ψ(i-1)+b12ψ(i-2)-a11ψ′(i-1)-a12ψ′(i-1)
θ′(i)=b20θ(i)+b21θ(i-1)+b22θ(i-2)-a21θ′(i-1)-a22θ′(i-1)
其中:b10,b11,b12,a11,a12,为航向角滤波系数;b20,b21,b22,a21,a22为俯仰角滤波系数;ψ′(i)为航向角ψ(i)滤波值;θ′(i)为俯仰角θ(i)滤波值;
其中T为位置采样周期;ωn为谐振中心频率;k1为陷波器深度系数,k2为陷波器宽度系数。
所述的方位位置控制误差eβ(i)=ψref(i)-ψ′(i),高低位置控制误差eε(i)=θref(i)-θ′(i)。
所述的方位位置和高低位置控制误差的设定阈值小于3mil/mrad。
所述的大幅调转下的方位随动伺服驱动器的速度控制指令
其中,keβ为方位根号e控制系数;umaxsβ和uminsβ为方位根号e位置控制器输出的上限和下限;
所述的大幅调转下的高低随动伺服驱动器的速度控制指令
其中,keε为高低根号e控制系数;umaxsε和uminsε为高低根号e位置控制器输出的上限和下限;
所述的稳定条件下的方位随动伺服驱动器的速度控制指令
所述的稳定条件下的高低随动伺服驱动器的速度控制指令;
所述的高低干扰滤校正量udε(j)=c11dε(j)+c12dε(j-1)-d11udε(j-1),方位干扰滤波校正量udβ(j)=c21dβ(j)+c22dβ(j-1)-d21udβ(j-1),其中,c11,c12,d11为高低干扰滤波校正系数;c21,c22,d21为方位干扰滤波校正系数;
Ts为速度控制周期;Tε1、Tβ1分别为高低和方位滤波时间系数;Tε2、Tβ2分别为高低和方位时间特性常数;kε1、kβ1分别为高低和方位增益常数;
所述的方位驱动的电流指令
z0lβ(j)=Tsv0lβ+z0lβ(j-1)
v0lβ=z1lβ(j-1)-λ0lβ|z0lβ(j-1)-leβ(j-1)|0.5sgn(z0lβ(j-1)-leβ(j-1))
z0ωβ(j)=Tsv0ωβ+z0ωβ(j-1)
leβ=eβω+γz1eβ(j)p/q
z0eβ(j)=Tsv0eβ+z0eβ(j-1)
v0eβ=z1eβ(j-1)-λ0eβ|z0eβ(j-1)-eβω(j-1)|0.5sgn(z0eβ(j-1)-eβω(j-1))
其中,iqeqβ,iqnβ分别为终端滑模等效控制量和滑模积分控制量;iqβmax,iqβmin分别为方位电流限幅值;Jβ为方位负载转动惯量;pβ为方位电机极对数;ψfβ为方位电机磁链系数;Bβ方位***综合粘性摩擦系数;ωβ(j)为方位电机转速;γβ,qβ,Pβ方位终端滑模系数;kβ,ηβ1,ηβ2为方位终端滑模控制系数。z0lβ(j),z1lβ(j)分别是leβ(j)的估计值和一阶估计值;λ0lβ,λ1lβ分别为终端滑模leβ(j)估计的0阶和一阶估计系数;z0ωβ(j),z1ωβ(j)分别是的估计值和一阶估计值;λ0ωβ,λ1ωβ分别为方位速度指令估计的0阶和一阶估计系数;z0eβ(j),z1eβ(j)分别是eβω(j)的估计值和一阶估计值;λ0eβ,λ1eβ分别为方位速度控制误差eβω(j)估计的0阶和一阶估计系数;v0lβ,v0ωβ,v0eβ分别为状态估计的中间变量。
所述的高低驱动的电流指令
z0lε(j)=Tsv0lε+z0lε(j-1)
v0lε=z1lε(j-1)-λ0lβ|z0lε(j-1)-leβ(j-1)|0.5sgn(z0lε(j-1)-leβ(j-1))
z1lε(j)=Ts[-λlε1sgn(z1lε(j-1)-v0lε)]+z1lε(j-1)
z0ωε(j)=Tsv0ωε+z0ωβ(j-1)
z1ωε(j)=Ts[-λ1ωεsgn(z1ωε(j-1)-v0ωε)]+z1ωε(j-1)
leε(j)=eεω(j)+γεz1eε(j)p/q
z0eε(j)=Tsv0eε+z0eε(j-1)
v0eε=z1eε(j-1)-λ0eε|z0eε(j-1)-eεω(j-1)|0.5sgn(z0eε(j-1)-eεω(j-1))
z1eε(j)=Ts[-λ1eεsgn(z1eε(j-1)-v0eε)]+z1eε(j-1)
其中,iqeqε,iqnε分别为高低终端滑模等效控制量和滑模积分控制量;iqεmax,iqεmin分别为高低电流限幅值;Jε为高低负载转动惯量;pε为高低电机极对数;ψfε为高低电机磁链系数;Bε高低***综合粘性摩擦系数;ωε(j)为高低电机转速;γε,qε,Pε高低终端滑模系数;kε,ηε1,ηε2为高低终端滑模控制系数;z0lε(j),z1lε(j)分别是leε(j)的估计值和一阶估计值;λ0lε,λ1lε分别为终端滑模leε(j)估计的0阶和一阶估计系数;z0ωε(j),z1ωε(j)分别是的估计值和一阶估计值;λ0ωε,λ1ωε分别为方位速度指令估计的0阶和一阶估计系数;z0eε(j),z1eε(j)分别是eεω(j)的估计值和一阶估计值;λ0eε,λ1eε分别为方位速度控制误差eεω(j)估计的0阶和一阶估计系数;v0lε,v0ωε,v0eε分别为状态估计的中间变量。
本发明的有益效果是:使稳定***对干扰的抑制控制完全由敏感的干扰速率决定,具有响应带宽高、干扰速率补偿精准等优点,且有效地克服了载体姿态对火炮身管指向控制干扰,高低和方位的指向控制独立,不耦合,充分发挥了SINS测量火炮身管在大地坐标系下指向实现全闭环控制精度高的优势,同时也抑制了传动机构因刚度不足导致的位置外环谐振,提高了弹性负载力矩干扰导致的伺服驱动速度控制精度,使速度控制与***参数无关,只与终端滑模参数有关,极大提升了干扰速度补偿精度,***鲁棒性和稳定性大大增强,实现了动基座大射角条件下的火炮身管指向高精度稳定。
附图说明
图1是本发明的控制原理图;
图2是本发明的控制传递函数结构图;
图3是本发明的计算流程图。
具体实施方式
下面结合附图和实施例对本发明进一步说明,本发明包括但不仅限于下述实施例。
本发明包括以下步骤:
(1)启动控制,设定控制步数为j,i,且:
j=j+1
其中j为稳定***速度环控制步数,控制周期Ts=1ms;i为稳定***位置环的控制步数,控制周期T=10ms,j比i快10倍;j和i初值为0;
(2)判断j%10=0?是,i=i+1,并转入第(3)步;否则转入(9)步;
(4)航向角和俯仰角的带阻滤波:
ψ′(i)=b10ψ(i)+b11ψ(i-1)+b12ψ(i-2)-a11ψ′(i-1)-a12ψ′(i-1)
θ′(i)=b20θ(i)+b21θ(i-1)+b22θ(i-2)-a21θ′(i-1)-a22θ′(i-1)
其中:b10,b11,b12,a11,a12,为航向角滤波系数;b20,b21,b22,a21,a22为俯仰角滤波系数;ψ′(i)为航向角ψ(i)滤波值;θ′(i)为俯仰角θ(i)滤波值;
其中T为位置采样周期;ωn为谐振中心频率;k1为陷波器深度系数,k2为陷波器宽度系数;
(5)采集高低侧角器测量值εb(i)和方位侧角器的测量值βb(i)
(6)是否同时接收到大地坐标下的火炮瞄准方位控制指令ψref(j)、高低控制指令θref(j)?是,进入第(7)步;否则转入第(9)步;
(7)求取方位位置控制误差eβ(i)和高低位置控制误差eε(i):
eβ(i)=ψref(i)-ψ′(i)
eε(i)=θref(i)-θ′(i);
其中:keβ为方位根号e控制系数;umaxsβ和uminsβ为方位根号e位置控制器输出的上限和下限;
其中:keε为高低根号e控制系数;umaxsε和uminsε为高低根号e位置控制器输出的上限和下限;
其中:usβ为方位位置PI控制量,Kpsβ、Kisβ为其PI比例控制系数和积分系数;
其中:uss为高低位置PI控制量,Kpsε、Kisε为其PI比例控制系数和积分系数;
(9)采集安装在火炮摇架上SINS的陀螺组所测量的三轴角速率ωp(j),其中ωp(j)=[ωp1(j),ωp2(j),ωp3(j)]T,ωp1(j),ωp2(j),ωp3(j)分别惯导三轴陀螺轴1、轴2、轴3角速率陀螺的测量值;
(10)采集炮塔陀螺组三轴陀螺所测量的三轴角速率ωh(j),其中ωh(j)=[ωh1(j),ωh2(j),ωh3(j)]T,ωh1(j),ωh2(j),ωh3(j)分别炮塔陀螺三轴陀螺轴1、轴2、轴3角速率陀螺的测量值;
(11)采集车体陀螺组测量的角速率ωb(j);
(12)计算高低补偿角速率dε(j)和方位干扰补偿角速率dβ(j);
(13)高低干扰滤校正量udε(j)和方位干扰滤波校正量udβ(j)计算;
udε(j)=c11dε(j)+c12dε(j-1)-d11udε(j-1)
udβ(j)=c21dβ(j)+c22dβ(j-1)-d21udβ(j-1)
其中,c11,c12,d11为高低干扰滤波校正系数;c21,c22,d21为方位干扰滤波校正系数;
其中:Ts为速度控制周期;Tε1,Tβ1分别为高低和方位滤波时间系数;Tε2,Tβ2分别为高低和方位时间特性常数;kε1,kβ1分别为高低和方位增益常数。
z0lβ(j)=Tsv0lβ+z0lβ(j-1)
v0lβ=z1lβ(j-1)-λ0lβ|z0lβ(j-1)-leβ(j-1)|0.5sgn(z0lβ(j-1)-leβ(j-1))
z0ωβ(j)=Tsv0ωβ+z0ωβ(j-1)
leβ=eβω+γz1eβ(j)p/q
z0eβ(j)=Tsv0eβ+z0eβ(j-1)
v0eβ=z1eβ(j-1)-λ0eβ|z0eβ(j-1)-eβω(j-1)|0.5sgn(z0eβ(j-1)-eβω(j-1))
其中:iqeqβ,iqnβ分别为终端滑模等效控制量和滑模积分控制量;iqβmax,iqβmin分别为方位电流限幅值;Jβ为方位负载转动惯量;pβ为方位电机极对数;ψfβ为方位电机磁链系数;Bβ方位***综合粘性摩擦系数;ωβ(j)为方位电机转速;γβ,qβ,Pβ方位终端滑模系数;kβ,ηβ1,ηβ2为方位终端滑模控制系数。z0lβ(j),z1lβ(j)分别是leβ(j)的估计值和一阶估计值;λ0lβ,λ1lβ分别为终端滑模leβ(j)估计的0阶和一阶估计系数;z0ωβ(j),z1ωβ(j)分别是的估计值和一阶估计值;λ0ωβ,λ1ωβ分别为方位速度指令估计的0阶和一阶估计系数;z0eβ(j),z1eβ(j)分别是eβω(j)的估计值和一阶估计值;λ0eβ,λ1eβ分别为方位速度控制误差eβω(j)估计的0阶和一阶估计系数;v0lβ,v0ωβ,v0eβ分别为状态估计的中间变量。
z0lε(j)=Tsv0lε+z0lε(j-1)
v0lε=z1lε(j-1)-λ0lβ|z0lε(j-1)-leβ(j-1)|0.5sgn(z0lε(j-1)-leβ(j-1))
z1lε(j)=Ts[-λlε1sgn(z1lε(j-1)-v0lε)]+z1lε(j-1)
z0ωε(j)=Tsv0ωε+z0ωβ(j-1)
v0ωε=z1ωε(j-1)-λ0ωε|z0ωε(j-1)-ωε *(j-1)|0.5sgn(z0ωε(j-1)-ωε *(j-1))
z1ωε(j)=Ts[-λ1ωεsgn(z1ωε(j-1)-v0ωε)]+z1ωε(j-1)
leε(j)=eεω(j)+γεz1eε(j)p/q
z0eε(j)=Tsv0eε+z0eε(j-1)
v0eε=z1eε(j-1)-λ0eε|z0eε(j-1)-eεω(j-1)|0.5sgn(z0eε(j-1)-eεω(j-1))
z1eε(j)=Ts[-λ1eεsgn(z1eε(j-1)-v0eε)]+z1eε(j-1)
其中:iqeqε,iqnε分别为高低终端滑模等效控制量和滑模积分控制量;iqεmax,iqεmin分别为高低电流限幅值;Jε为高低负载转动惯量;pε为高低电机极对数;ψfε为高低电机磁链系数;Bε高低***综合粘性摩擦系数;ωε(j)为高低电机转速;γε,qε,Pε高低终端滑模系数;kε,ηε1,ηε2为高低终端滑模控制系数;z0lε(j),z1lε(j)分别是leε(j)的估计值和一阶估计值;λ0lε,λ1lε分别为终端滑模leε(j)估计的0阶和一阶估计系数;z0ωε(j),z1ωε(j)分别是的估计值和一阶估计值;λ0ωε,λ1ωε分别为方位速度指令估计的0阶和一阶估计系数;z0eε(j),z1eε(j)分别是eεω(j)的估计值和一阶估计值;λ0eε,λ1eε分别为方位速度控制误差eεω(j)估计的0阶和一阶估计系数;v0lε,v0ωε,v0eε分别为状态估计的中间变量。
本发明实施例的控制原理见图1。图中利用SINS陀螺组测量火炮身管调转的绝对角速率,包括地球自转角速率、车体姿态变化的角速率、火炮身管相对车体调转角速率。而对控制有效的角速率是火炮身管相对车体调转角速率,其它都可以视为干扰。SINS的航向角和姿态角作为瞄准稳定***位置环的反馈与位置控制器构成大地坐标系下的位置闭环。由于该稳定***具有火炮身高低和方位侧角器,分别测量身管相对炮塔管俯仰角,炮塔相对车体方位角。稳定***可根据SINS陀螺组的角速率、高低角、炮塔陀螺组、车体陀螺,即可计算出火炮在大地坐标下的方位和高低干扰角速率。稳定***通过位置控制外环的串级控制消除高低和方位两个控制通道的干扰,从而达到理想的稳定瞄准精度。
本发明实施例的大致步骤包括:首先,采集SINS的航向角、俯仰角和横滚角,以及采集火炮的高低角和方位角,并对航向角和俯仰角进行带阻滤波;然后,接受稳定控制指令,进行位置闭环控制,得到驱动器的速度指令;其次,采集SINS陀螺组、炮塔陀螺组、车体陀螺的角速度值,提取大地坐标系下的方位向和高低向的干扰角速率;再次,进行干扰角速率的滤波校正,得到方位和高低补偿角速率控制量;再再次,合并稳定控制的速率控制指令和干扰补偿角速率控制指令,作为伺服驱动的速度总指令;最后,高低和方位伺服驱动器采集各自电机角速率,并根据速度控制指令进行高阶滑模速度控制策略计算得到电流环控制指令,以驱动电机按给定的控制量控制火炮调转运动。
本发明实施例的控制传递函数结构见图2。为了简化传递函数,可以将SINS的航向角和俯仰角测量通过简化为将耦合干扰视为各种通过的干扰;将高低和方位伺服驱动的电流控制器、逆变器、电流调理、电流力矩系数等构成的闭环简化为一阶惯性环节方位和高低速度环控制为Kpsβ,Kpsε分别为比例系数,Kisβ,Kisε为积分系数;方位和高低位置环控制为Kpcβ,Kpcε分别为比例系数,Kicβ,Kicε分别为积分系数;干扰速率前馈控制器为fdβ,fdε分别为增益系数,Tdβ,Tdε分别为时间常数,采用双线性变换将其离散化。
实施该控制方法的火炮稳定瞄准***主要由稳定控制***、驱动调速***、动力电源***、炮塔陀螺组、车体陀螺、高低和方位侧角器等组成。稳定瞄准控制***采用基于x86的嵌入式计算机。驱动调速***以DSP28335+FPGA为核心的控制板,驱动功率电子IPM(智能驱动)控制电机转动。方位永磁同步电机(PMSM),母线电压为56VDC,极对数np=3,额定功率4kW,定子电感为0.0098mH,定子电阻为3.5毫欧,额定转速3000RPM,额定转矩7.4Nm,电机转子及传动齿轮系的的等效转动惯量J总和为0.013kg·m2;高低PMSM,母线电压为56VDC,极对数np=3,额定功率2kW,额定转矩3.2Nm,定子电感为0.032mH,定子电阻为0.0105欧,额定转速为3000RPM,电机转子及传动齿轮系的等效转动惯量J总和为0.0075kg·m2。方位负载转动惯量约为2700kg·m2,传动速比为470。高低负载转动惯量为700kg·m2。传动速比为450。SINS的角速率测量范围为±300°/s,航向测量精度不大于0.3mil,姿态测量精度不大于0.1mil。
图3为本发明实施例的计算流程图,下面将结合流程图详述具体实施过程。
(1)启动控制,设定控制步数为j,i,且:
j=j+1
其中j为稳定***速度环控制步数,控制周期Ts=1ms;i为稳定***位置环的控制步数,控制周期T=10ms,j比i快10倍;j和i初值为0;
(2)判断j%10=0?是,i=i+1,并转入第(3)步;否则转入(9)步;
(4)航向角和俯仰角的带阻滤波:
ψ′(i)=b10ψ(i)+b11ψ(i-1)+b12ψ(i-2)-a11ψ′(i-1)-a12ψ′(i-1)
θ′(i)=b20θ(i)+b21θ(i-1)+b22θ(i-2)-a21θ′(i-1)-a22θ′(i-1)
其中:b10,b11,b12,a11,a12,为航向角滤波系数;b20,b21,b22,a21,a22为俯仰角滤波系数;ψ′(i)为航向角ψ(i)滤波值;θ′(i)为俯仰角θ(i)滤波值;
其中T=0.01s;ωn=3.5×2π;k1=100;k2=8;
(5)采集高低侧角器测量值εb(i)和方位侧角器的测量值βb(i)
(6)是否接收到大地坐标下的火炮瞄准方位控制指令ψref(j)、高低控制指令θref(j)?是,进入第(7)步;否则转入第(9)步;
(7)求取方位位置控制误差eβ(i)和高低位置控制误差eε(i):
eβ(i)=ψref(i)-ψ′(i)
eε(i)=θref(i)-θ′(i);
其中:keβ=120;umaxsβ=3000,uminsβ=-3000,此二者参数有方位电机额定转速决定;
其中:keε=150;umaxsε=3000,uminsε=-3000,此二者参数有高低电机额定转速决定;
其中:upsβ=17,uisβ=0.02;
其中:upsε=15,uisε=0.02;
(9)采集安装在火炮摇架上SINS的陀螺组所测量的三轴角速率ωp(j),其中ωp(j)=[ωp1(j),ωp2(j),ωp3(j)]T,ωp1(j),ωp2(j),ωp3(j)分别惯导三轴陀螺轴1、轴2、轴3角速率陀螺的测量值;
(10)采集炮塔陀螺组三轴陀螺所测量的三轴角速率ωh(j),其中ωh(j)=[ωh1(j),ωh2(j),ωh3(j)]T,ωh1(j),ωh2(j),ωh3(j)分别炮塔陀螺三轴陀螺轴1、轴2、轴3角速率陀螺的测量值;
(11)采集车体陀螺组测量的角速率ωb(j);
(12)计算高低补偿角速率dε(j)和方位干扰补偿角速率dβ(j);
(13)高低干扰滤校正量udε(j)和方位干扰滤波校正量udβ(j)计算;
udε(j)=c11dε(j)+c12dε(j-1)-d11udε(j-1)
udβ(j)=c21dβ(j)+c22dβ(j-1)-d21udβ(j-1)
其中,c11,c12,d11为高低干扰滤波校正系数;c21,c22,d21为方位干扰滤波校正系数;
其中:Ts=0.001;Tε1=0.01,Tβ1=0.01;Tε2=0.025,Tβ2=0.036;kε1=4.5,kβ1=4.7;
z0lβ(j)=Tsv0lβ+z0lβ(j-1)
v0lβ=z1lβ(j-1)-λ0lβ|z0lβ(j-1)-leβ(j-1)|0.5sgn(z0lβ(j-1)-leβ(j-1))
z0ωβ(j)=Tsv0ωβ+z0ωβ(j-1)
leβ=eβω+γz1eβ(j)p/q
z0eβ(j)=Tsv0eβ+z0eβ(j-1)
v0eβ=z1eβ(j-1)-λ0eβ|z0eβ(j-1)-eβω(j-1)|0.5sgn(z0eβ(j-1)-eβω(j-1))
其中:给定的方位电机轴Jβ=0.0252(含负载),其电机极对数pβ=3,其推算的磁链系数ψfβ=0.0031,给定的摩擦系数Bβ=0.005。设计参数如下:γβ=0.02,qβ=3,Pβ=5;kβ=250,ηβ1=300,ηβ2=2000;λ0lβ=5.321,λ1lβ=15.487;λ0ωβ=2.56,λ1ωβ=11.654;λ0eβ=7.662,λ1eβ=18.956;
z0lε(j)=Tsv0lε+z0lε(j-1)
v0lε=z1lε(j-1)-λ0lβ|z0lε(j-1)-leβ(j-1)|0.5sgn(z0lε(j-1)-leβ(j-1))
z1lε(j)=Ts[-λlε1sgn(z1lε(j-1)-v0lε)]+z1lε(j-1)
z0ωε(j)=Tsv0ωε+z0ωβ(j-1)
z1ωε(j)=Ts[-λ1ωεsgn(z1ωε(j-1)-v0ωε)]+z1ωε(j-1)
leε(j)=eεω(j)+γεz1eε(j)p/q
z0eε(j)=Tsv0eε+z0eε(j-1)
v0eε=z1eε(j-1)-λ0eε|z0eε(j-1)-eεω(j-1)|0.5sgn(z0eε(j-1)-eεω(j-1))
z1eε(j)=Ts[-λ1eεsgn(z1eε(j-1)-v0eε)]+z1eε(j-1)
其中:给定的高低电机轴Jε=0.011(含负载),其极对数pε=3,其推算的磁链系数ψfε=0.02,给定摩擦系数Bε=0.008。设计参数如下:γε=0.01,qε=5,Pε=7;kε=300,ηε1=450,ηε2=1500;λ0lε=4.365,λ1lε=13.78;λ0ωε=5.862,λ1ωε=14.953;λ0eε=7.632,λ1eε=19.564;
所用参数的范围见表1。
表1参数取值范围
Claims (8)
1.一种大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,包括以下步骤:
(1)启动控制,设定控制步数为j和i,其中j为稳定***速度环控制步数,控制周期Ts=1ms;i为稳定***位置环的控制步数,控制周期T=10ms;j和i初值为0;
(2)判断j%10=0是否成立,若是,控制步数i值加1,并转入第(3)步;否则转入第(9)步;
(4)计算航向角和俯仰角的带阻滤波ψ′(i)和θ′(i);
(5)采集高低侧角器测量值εb(i)和方位侧角器的测量值βb(i);
(6)判断是否同时接收到大地坐标下的火炮瞄准方位控制指令ψref(j)、高低控制指令θref(j),若是,进入第(7)步;否则转入第(9)步;
(7)求取方位位置控制误差eβ(i)和高低位置控制误差eε(i);
(9)采集安装在火炮摇架上SINS的陀螺组所测量的三轴角速率ωp(j),其中ωp(j)=[ωp1(j),ωp2(j),ωp3(j)]T,ωp1(j),ωp2(j),ωp3(j)分别惯导三轴陀螺轴角速率陀螺的测量值;
(10)采集炮塔陀螺组三轴陀螺所测量的三轴角速率ωh(j),其中ωh(j)=[ωh1(j),ωh2(j),ωh3(j)]T,ωh1(j),ωh2(j),ωh3(j)分别炮塔陀螺三轴陀螺轴角速率陀螺的测量值;
(11)采集车体陀螺组测量的角速率ωb(j);
(12)计算高低补偿角速率dε(j)和方位干扰补偿角速率dβ(j);
(13)计算高低干扰滤校正量udε(j)和方位干扰滤波校正量udβ(j);
2.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的航向角和俯仰角的带阻滤波:
ψ′(i)=b10ψ(i)+b11ψ(i-1)+b12ψ(i-2)-a11ψ′(i-1)-a12ψ′(i-1)
θ′(i)=b20θ(i)+b21θ(i-1)+b22θ(i-2)-a21θ′(i-1)-a22θ′(i-1)
其中:b10,b11,b12,a11,a12,为航向角滤波系数;b20,b21,b22,a21,a22为俯仰角滤波系数;ψ′(i)为航向角ψ(i)滤波值;θ′(i)为俯仰角θ(i)滤波值;
其中T为位置采样周期;ωn为谐振中心频率;k1为陷波器深度系数,k2为陷波器宽度系数。
3.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的方位位置控制误差eβ(i)=ψref(i)-ψ′(i),高低位置控制误差eε(i)=θref(i)-θ′(i)。
4.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的方位位置和高低位置控制误差的设定阈值小于3mil/mrad。
5.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的大幅调转下的方位随动伺服驱动器的速度控制指令
其中,keβ为方位根号e控制系数;umaxsβ和uminsβ为方位根号e位置控制器输出的上限和下限;
所述的大幅调转下的高低随动伺服驱动器的速度控制指令
其中,keε为高低根号e控制系数;umaxsε和uminsε为高低根号e位置控制器输出的上限和下限;
所述的稳定条件下的方位随动伺服驱动器的速度控制指令
所述的稳定条件下的高低随动伺服驱动器的速度控制指令;
所述的高低干扰滤校正量udε(j)=c11dε(j)+c12dε(j-1)-d11udε(j-1),方位干扰滤波校正量udβ(j)=c21dβ(j)+c22dβ(j-1)-d21udβ(j-1),其中,c11,c12,d11为高低干扰滤波校正系数;c21,c22,d21为方位干扰滤波校正系数;
Ts为速度控制周期;Tε1、Tβ1分别为高低和方位滤波时间系数;Tε2、Tβ2分别为高低和方位时间特性常数;kε1、kβ1分别为高低和方位增益常数;
7.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的方位驱动的电流指令
z0lβ(j)=Tsv0lβ+z0lβ(j-1)
v0lβ=z1lβ(j-1)-λ0lβ|z0lβ(j-1)-leβ(j-1)|0.5sgn(z0lβ(j-1)-leβ(j-1))
z0ωβ(j)=Tsv0ωβ+z0ωβ(j-1)
leβ=eβω+γz1eβ(j)p/q
z0eβ(j)=Tsv0eβ+z0eβ(j-1)
v0eβ=z1eβ(j-1)-λ0eβ|z0eβ(j-1)-eβω(j-1)|0.5sgn(z0eβ(j-1)-eβω(j-1))
其中,iqeqβ,iqnβ分别为终端滑模等效控制量和滑模积分控制量;iqβmax,iqβmin分别为方位电流限幅值;Jβ为方位负载转动惯量;pβ为方位电机极对数;ψfβ为方位电机磁链系数;Bβ方位***综合粘性摩擦系数;ωβ(j)为方位电机转速;γβ,qβ,Pβ方位终端滑模系数;kβ,ηβ1,ηβ2为方位终端滑模控制系数。z0lβ(j),z1lβ(j)分别是leβ(j)的估计值和一阶估计值;λ0lβ,λ1lβ分别为终端滑模leβ(j)估计的0阶和一阶估计系数;z0ωβ(j),z1ωβ(j)分别是的估计值和一阶估计值;λ0ωβ,λ1ωβ分别为方位速度指令估计的0阶和一阶估计系数;z0eβ(j),z1eβ(j)分别是eβω(j)的估计值和一阶估计值;λ0eβ,λ1eβ分别为方位速度控制误差eβω(j)估计的0阶和一阶估计系数;v0lβ,v0ωβ,v0eβ分别为状态估计的中间变量。
8.根据权利要求1所述的大地坐标系下全闭环干扰速率补偿自稳定控制方法,其特征在于,所述的高低驱动的电流指令
z0lε(j)=Tsv0lε+z0lε(j-1)
v0lε=z1lε(j-1)-λ0lβ|z0lε(j-1)-leβ(j-1)|0.5sgn(z0lε(j-1)-leβ(j-1))
z1lε(j)=Ts[-λlε1sgn(z1lε(j-1)-v0lε)]+z1lε(j-1)
z0ωε(j)=Tsv0ωε+z0ωβ(j-1)
z1ωε(j)=Ts[-λ1ωεsgn(z1ωε(j-1)-v0ωε)]+z1ωε(j-1)
leε(j)=eεω(j)+γεz1eε(j)p/q
z0eε(j)=Tsv0eε+z0eε(j-1)
v0eε=z1eε(j-1)-λ0eε|z0eε(j-1)-eεω(j-1)|0.5sgn(z0eε(j-1)-eεω(j-1))
z1eε(j)=Ts[-λ1eεsgn(z1eε(j-1)-v0eε)]+z1eε(j-1)
其中,iqeqε,iqnε分别为高低终端滑模等效控制量和滑模积分控制量;iqεmax,iqεmin分别为高低电流限幅值;Jε为高低负载转动惯量;pε为高低电机极对数;ψfε为高低电机磁链系数;Bε高低***综合粘性摩擦系数;ωε(j)为高低电机转速;γε,qε,Pε高低终端滑模系数;kε,ηε1,ηε2为高低终端滑模控制系数;z0lε(j),z1lε(j)分别是leε(j)的估计值和一阶估计值;λ0lε,λ1lε分别为终端滑模leε(j)估计的0阶和一阶估计系数;z0ωε(j),z1ωε(j)分别是的估计值和一阶估计值;λ0ωε,λ1ωε分别为方位速度指令估计的0阶和一阶估计系数;z0eε(j),z1eε(j)分别是eεω(j)的估计值和一阶估计值;λ0eε,λ1eε分别为方位速度控制误差eεω(j)估计的0阶和一阶估计系数;v0lε,v0ωε,v0eε分别为状态估计的中间变量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011518365.5A CN112696981B (zh) | 2020-12-21 | 2020-12-21 | 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011518365.5A CN112696981B (zh) | 2020-12-21 | 2020-12-21 | 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112696981A true CN112696981A (zh) | 2021-04-23 |
CN112696981B CN112696981B (zh) | 2023-02-21 |
Family
ID=75509206
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011518365.5A Active CN112696981B (zh) | 2020-12-21 | 2020-12-21 | 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112696981B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113280678A (zh) * | 2021-05-19 | 2021-08-20 | 中国人民解放军63966部队 | 一种火炮瞄准性能参数测试***的标校方法 |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1432786A (zh) * | 2002-01-16 | 2003-07-30 | 厄利康肯特维斯股份公司 | 射击误差的补偿方法和装置以及武器***的***计算机 |
CN1804539A (zh) * | 2005-12-22 | 2006-07-19 | 天津市无线电二厂 | 高炮自动水平规正*** |
US20090089001A1 (en) * | 2007-08-14 | 2009-04-02 | American Gnc Corporation | Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) |
CN106342284B (zh) * | 2008-08-18 | 2011-11-23 | 西北工业大学 | 一种飞行载体姿态确定方法 |
CN104457428A (zh) * | 2013-09-25 | 2015-03-25 | 中国兵器工业第二0二研究所 | 坦克炮方向机与炮口位移角偏差测试装置 |
CN106706003A (zh) * | 2017-02-15 | 2017-05-24 | 重庆邮电大学 | 一种基于三轴mems陀螺仪的寻北旋转在线校准方法 |
CN107389049A (zh) * | 2017-08-10 | 2017-11-24 | 北京联合大学 | 一种基于类卡尔曼因子的磁罗盘误差实时补偿方法 |
CN107783422A (zh) * | 2017-10-20 | 2018-03-09 | 西北机电工程研究所 | 采用捷联惯导的火炮瞄准稳定***控制方法 |
CN108333938A (zh) * | 2018-02-06 | 2018-07-27 | 贵州电网有限责任公司 | 一种多闭环复合陀螺稳定控制方法 |
CN111142380A (zh) * | 2019-12-11 | 2020-05-12 | 南京理工大学 | 一种火箭炮在行进间发射的位置伺服***的控制方法 |
-
2020
- 2020-12-21 CN CN202011518365.5A patent/CN112696981B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1432786A (zh) * | 2002-01-16 | 2003-07-30 | 厄利康肯特维斯股份公司 | 射击误差的补偿方法和装置以及武器***的***计算机 |
CN1804539A (zh) * | 2005-12-22 | 2006-07-19 | 天津市无线电二厂 | 高炮自动水平规正*** |
US20090089001A1 (en) * | 2007-08-14 | 2009-04-02 | American Gnc Corporation | Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS) |
CN106342284B (zh) * | 2008-08-18 | 2011-11-23 | 西北工业大学 | 一种飞行载体姿态确定方法 |
CN104457428A (zh) * | 2013-09-25 | 2015-03-25 | 中国兵器工业第二0二研究所 | 坦克炮方向机与炮口位移角偏差测试装置 |
CN106706003A (zh) * | 2017-02-15 | 2017-05-24 | 重庆邮电大学 | 一种基于三轴mems陀螺仪的寻北旋转在线校准方法 |
CN107389049A (zh) * | 2017-08-10 | 2017-11-24 | 北京联合大学 | 一种基于类卡尔曼因子的磁罗盘误差实时补偿方法 |
CN107783422A (zh) * | 2017-10-20 | 2018-03-09 | 西北机电工程研究所 | 采用捷联惯导的火炮瞄准稳定***控制方法 |
CN108333938A (zh) * | 2018-02-06 | 2018-07-27 | 贵州电网有限责任公司 | 一种多闭环复合陀螺稳定控制方法 |
CN111142380A (zh) * | 2019-12-11 | 2020-05-12 | 南京理工大学 | 一种火箭炮在行进间发射的位置伺服***的控制方法 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113280678A (zh) * | 2021-05-19 | 2021-08-20 | 中国人民解放军63966部队 | 一种火炮瞄准性能参数测试***的标校方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112696981B (zh) | 2023-02-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107783422B (zh) | 采用捷联惯导的火炮瞄准稳定***控制方法 | |
CN109508030B (zh) | 一种考虑多禁飞区约束的协同解析再入制导方法 | |
CN102981407B (zh) | 一种基于自抗扰控制技术的坦克炮控伺服控制方法 | |
CN101576750B (zh) | 航天器的姿态跟踪控制***及方法 | |
CN106871742A (zh) | 一种设置在弹体上的控制*** | |
CN110456630B (zh) | 一种控制力矩陀螺框架伺服***抗干扰控制方法 | |
WO2020103290A1 (zh) | 末子级留轨应用子***姿态控制方法 | |
CN109398763B (zh) | 一种基于有限推力有限工作时间情况下的航天器精确入轨控制方法 | |
CN109625334B (zh) | 卫星动量轮在轨姿态无偏差起旋和消旋的控制方法 | |
CN102288133B (zh) | 一种陀螺间接稳定***安装偏角标定方法 | |
CN111595210A (zh) | 一种大空域高动态火箭子级落区精确垂直回收控制方法 | |
WO2021098238A1 (zh) | 一种车载摄像头云台伺服***及控制方法 | |
CN108279695B (zh) | 一种航天器干扰力矩的快速在轨闭环辨识方法、***和介质 | |
CN110673623B (zh) | 一种基于双环pd控制算法控制的四旋翼无人机着陆方法 | |
CN110658854B (zh) | 一种基于组合惯导信息应用的光电转塔视频跟踪前馈补偿方法 | |
CN104085539B (zh) | 成像定标的姿态控制方法 | |
CN109649691B (zh) | 一种偏置动量卫星单飞轮与磁联合控制方法和*** | |
CN108759589A (zh) | 一种旋转导弹制导控制解耦合的方法及装置 | |
CN112696981B (zh) | 一种大地坐标系下全闭环干扰速率补偿自稳定控制方法 | |
CN106354147A (zh) | 一种基于发动机预摆的大质心横移下的姿轨控耦合控制***与方法 | |
CN105115508A (zh) | 基于后数据的旋转制导炮弹快速空中对准方法 | |
CN105180728A (zh) | 基于前数据的旋转制导炮弹快速空中对准方法 | |
CN114879511A (zh) | 一种基于自适应扩张状态观测器和全局快速终端滑模的三轴惯性稳定平台高精度控制方法 | |
CN110209192A (zh) | 战斗机航向增稳控制***设计方法 | |
CN113022893A (zh) | 一种空间交会拦截自主自适应远程制导方法和*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |