基于二阶滤波器的水下机器人三维路径跟踪控制方法.pdf

本发明提供的是一种基于二阶滤波器的水下机器人三维路径跟踪控制方法，利用滤波反步法进行水下机器人三维路径跟踪控制，通过引入两个基于水下机器人三维路径跟踪运动学误差模型建立的二阶滤波器，获取姿态、速度、角速度的虚拟控制量及其导数，再结合水下机器人动力学模型获取路径跟踪控制器的控制输入，作用于机器人推进器与舵机，进而实现对三维路径的跟踪；并依据李雅普诺夫能量函数对位置、姿态控制回路设计滤波反馈补偿项，对速度控制回路引入积分环节，构成系统误差补偿回路，提升跟踪系统的精度。

权利要求书
1.  一种基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于包含以下步骤：
步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet坐标系，获取期望路径，水下机器人开始路径跟踪，完成两个二阶滤波器的初始化；
步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器，采集水下机器人当前位置、姿态角、角速度和速度数据信息，并结合期望路径的方向与速度，根据视线角导引思想计算得到水下机器人理想的姿态控制量ψco、θco，和理想速度控制量uco；
步骤3.将步骤2中得到的理想控制量ψco、θco、uco输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的姿态与速度控制量ψc、θc、uc，及其导数结合机器人运动变量ψ、θ、u，得到滤波姿态与速度跟踪误差量与理想角速度控制量rco、qco；
步骤4.将步骤3中得到的水下机器人理想角速度控制量rco、qco输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的角速度控制量rc、qc，及其导数结合机器人角运动变量r、q，得到滤波角速度跟踪误差量
步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量以及步骤4中得到的滤波角速度跟踪误差量解算得到水下机器人推进器推力Fu，与水平舵角δs、垂直舵角δr，分别作用于机器人推进器及舵机，实现三维路径跟踪控制；
步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψc、θc、uc，滤波姿态与速度跟踪误差量与理想角速度控制量rco、qco，结合步骤4中得到的水下机器人的角速度控制量rc、qc，以及滤波角速度跟踪误差量构造滤波误差补偿回路；
步骤7.计算当前水下机器人位置ηn=(x,y,z)与标定的转向点WPk=(xk,yk,zk)之间的距离若小于设定的切换半径R，则表示完成当前指定路径的跟踪任务，否则继续步骤2。

2.  根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤2中所涉及的水下机器人理想的姿态控制量ψco、θco，和理想速度控制量uco的计算表达式为：
ψco=-arcsin(k2e/1+(k2e)2)---(1)]]>
θco=arcsin(k3h/1+(k3h)2)---(2)]]>
uco=-k1s+urcosψcocosθco   (3)
其中增益因子k1>0，k2>0，k3>0为视线角导引律归一化参数，变量s、e和h分别表示机器人载体坐标系下机器人与期望路径参考点的前向、横向和垂向跟踪误差。

3.  根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤3、4中所涉及的水下机器人三维路径跟踪运动学误差模型为：
s·=re-qh+u-urcosψecosθee·=-rs+ursinψecosθe+vh·=qs-ursinθe+w---(4)]]>
ψ·e=rcosθ-rFθ·e=q-qF---(5)]]>
其中ψe=ψ-ψF，θe=θ-θF，机器人纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，ur为待设计期望路径上虚拟向导点的期望速度，其方向沿曲线路径的切线方向；ψF为ur速度方向与固定坐标系水平轴的夹角，θF为ur速度方向与固定坐标系垂直轴的夹角；ψ为机器人艏向角，θ为机器人纵倾角。

4.  根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤5中涉及的水下机器人推进器推力Fu，与水平舵角δs、垂直舵角δr的表达式为：
Fu=m1(-kuu~-kiuϵ1+u·c-ubs)-fuδs=b1-1[m4(-kqq~-kiqϵ2+q·c-qbs)-fq]δr=b2-1[m5(-krr~-kirϵ3+r·c-rbs)-fr]---(6)]]>
其中fu、fq和fr为模型非线性水动力项；ubs、qbs和rbs为反馈补偿鲁棒项；m1、m4、m5分别为由流体产生的附加质量；ku、kq、kr、kiu、kiq和kir均为控制器参数；
所涉及的水下机器人动力学模型为：
u·=m2m1vr-m3m1wq-d1m1u+1m1Fu+ω1]]>
v·=-m1m2ur-d2m2v+ω2]]>
w·=m1m3uq-d3m3w+g1+ω3---(7)]]>
q·=m3-m1m5uw-d4m5q-g2+1m5b1δs+ω4]]>
r·=m1-m2m6uv-d5m6r+1m6b2δr+ω5]]>
其中
m1=m-Xu·,m2=m-Yv·,m3=m-Zw·]]>
m5=Iy-Mq·,m6=Iz-Nr·]]>
g1=(W-B)cosθ,g2=(zgW-zbB)sinθ
d1=Xu+Xu|u||u|,d2=Yv+Yv|v||v|
d3=Zw+Zw|w||w|,d4=Mq+Mq|q||q|
d5=Nr+Nr|r||r|
b1=u2Mδs,b2=u2Nδr]]>
其中，m和m(·)分别表示机器人质量和由流体作用产生的附加质量，Iy为机器人绕y轴的转动惯量，Iz为机器人绕z轴的转动惯量，X(·)、Y(·)、Z(·)、M(·)和N(·)为粘性流体水动力系数；zg和zb分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，ω(·)表示为干扰作用项。

5.  根据权利要求1所述的基于二阶滤波器的水下机器人三维路径跟踪控制方法，其特征在于，步骤6中涉及的滤波误差补偿回路中误差补偿鲁棒项的表达式为：
ψbs=gT(ψ~)BTurυxυyυz---(8)]]>
θbs=gT(θ~)CTurυxυyυz---(9)]]>
ubs=1p21ATυxυyυz---(10)]]>
rbs=1p23υψcosθ---(11)]]>
qbs=1p22υθ---(12)]]>
其中，p21、p22、p23为李雅普诺夫方程的解矩阵中的元素；
A=100,]]>B=cosθecosψc-cosθesinψccosθcsinψccosθccosψc00,]]>C=cosψccosθc-cosψcsinθcsinψecosθc-sinψesinθc-sinθc-cosθc,]]>g(ψ~)=cosψ~-1ψ~sinψ~ψ~,]]>g(θ~)=cosθ~-1θ~sinθ~θ~,]]>且满足limψ~→0g(ψ~)=01,]]>limθ~→0g(θ~)=01;]]>
所涉及的位置滤波信号补偿误差为
υxυyυz=s~-ζxe~-ζyh~-ζz---(13)]]>
式中，位置滤波补偿动态量ζx、ζy和ζz表达式为
ζ·xζ·yζ·z=rζy-qζz-rζxqζx-kxζx-kyζy-kzζz+s·c-s·coe·c-e·coh·c-h·co+urABg(ψ~)Cg(θ~)ζuζψζθ---(14)]]>
且有ζx(0)=0、ζy(0)=0、ζz(0)=0；sco、eco、hco为期望位置信号，sc、ec、hc为位置控制信号；
所涉及的滤波器输出的姿态信号补偿量为：
υψυθ=ψ~-ζψθ~-ζθ---(15)]]>
式中，滤波姿态补偿动态表达式为：
ζ·ψ=-kψζψ+rc-rcocosθ+ζrcosθ---(16)]]>
ζ·θ=-kθζθ+(qc-qco)+ζq---(17)]]> 且有ζψ(0)=0，ζθ(0)=0，ζr=0，ζq=0。

说明书基于二阶滤波器的水下机器人三维路径跟踪控制方法
技术领域
本发明涉及的是一种水下机器人的运动控制方法，特别是涉及一种基于二阶滤波器的水下机器人三维路径跟踪控制方法。
背景技术
水下机器人的跟踪控制问题一直是国内外的研究热点。路径跟踪问题仅要求水下机器人的运动轨迹收敛到期望路径，而对何时到达何处并不作要求。路径跟踪技术通过引入期望路径上虚拟向导点的概念，避免了轨迹跟踪时由于水下机器人的动力学模型受到环境扰动作用，无法精确获得期望状态，导致跟踪控制系统性能变差的问题。路径跟踪相比于轨迹跟踪问题，由于期望位置不受时间条件约束，从而不易导致控制器输出饱和信号，符合工程实际，更具有实际应用价值。
基于系统分层递推设计思想的反步法，为水下机器人三维跟踪控制系统的设计提供了有效的手段。然而，在反步法递推设计控制器的过程中，需要逐步计算中间虚拟控制量的导数，然后基于反馈线性化的思想通过设计后续子系统的等价输入，实现对前级子系统的镇定，通过不断迭代直到获得最终真实控制输入。当系统的阶数增加或虚拟控制的形式较为复杂时，求导过程将变得十分繁琐。为此本发明采用反馈增益反步法设计三维曲线路径跟踪控制器，通过合理选择控制器的参数消除部分非线性项，相比于传统反步法设计过程简化了虚拟控制量的形式，但仍需要逐步计算虚拟控制导数的解析形式。为克服常规反步法递推过程中对虚拟控制信号逐步求导的不足，文献《The Use of Sliding Modes to Simplify the Backstepping Control》和《The use of linear filtering of simplified integrator backstepping control of nonlinear systems》分别采用滑模滤波器和线性滤波器逼近虚拟控制的导数；文献《Command Filtered Backstepping》提出基于滤波器设计的反步法轨迹跟踪控制（IEEE Transactions On Automatic Control.2009,第54卷第6期），采用二阶滤波器逼近虚拟控制信号，简化了控制器设计过程，基于奇异扰动理论证明了滤波轨迹与常规反步法的控制输出之间的误差能够收敛到零点较小邻域。文献《Land Vehicle Control Using a Command Filtered Backstepping Approach》将滤波反步法应用于陆地车辆的轨迹跟踪控制中；文献《基于滤波反步法的无人直升机轨迹跟踪控制》（控制与决策.2012,第27卷第4期）和文献《状态受限的小小型型无人直升机轨迹跟踪控制》（控制理论与应用.2012,第29卷第6期）直升机的轨迹跟踪控制中，大大简化了控制器设计过程。目前还没有相关文献讨论基于滤波器的水下机器人三维路径跟踪控制设计。
发明内容
本发明提出一种过程简化、跟踪精度高的基于二阶滤波器的水下机器人三维路径跟踪控制方法。
本发明的基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体过程为：
步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet（路径参考）坐标系，获取期望路径，水下机器人开始路径跟踪，完成两个二阶滤波器的初始化；
步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器，采集水下机器人当前位置、姿态角、角速度和速度数据信息，并结合期望路径的方向与速度，根据视线角导引思想计算得到水下机器人理想的姿态控制量ψco、θco，和理想速度控制量uco；
所涉及的水下机器人理想的姿态控制量ψco、θco，和理想速度控制量uco的计算表达式为：
ψco=-arcsin(k2e/1+(k2e)2)---(1)]]>
θco=arcsin(k3h/1+(k3h)2)---(2)]]>
uco=-k1s+urcosψcocosθco   (3)
其中增益因子k1>0，k2>0，k3>0为视线角导引律归一化参数，变量s、e和h分别表示机器人载体坐标系下机器人与期望路径参考点的前向、横向和垂向跟踪误差。
步骤3.将步骤2中得到的理想控制量ψco、θco、uco输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的姿态与速度控制量ψc、θc、uc，及其导数结合机器人运动变量ψ、θ、u，得到滤波姿态与速度跟踪误差量与理想角速度控制量rco、qco；
所涉及的水下机器人三维路径跟踪运动学误差模型为：
s·=re-qh+u-urcosψecosθee·=-rs+ursinψecosθe+vh·=qs-ursinθe+w---(4)]]>
ψ·e=rcosθ-rFθ·e=q-qF---(5)]]>
其中ψe=ψ-ψF，θe=θ-θF，机器人纵向速度u、横向速度v和垂向速度w，艏摇角速度r和纵倾角速度q，ur为待设计期望路径上虚拟向导点的期望速度，其方向沿曲线路径的切线方向；ψF为ur速度方向与固定坐标系水平轴的夹角，θF为ur速度 方向与固定坐标系垂直轴的夹角；ψ为机器人艏向角，θ为机器人纵倾角。
步骤4.将步骤3中得到的水下机器人理想角速度控制量rco、qco输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器，得到水下机器人的角速度控制量rc、qc，及其导数结合机器人角运动变量r、q，得到滤波角速度跟踪误差量
步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量以及步骤4中得到的滤波角速度跟踪误差量解算得到水下机器人推进器推力Fu，与水平舵角δs、垂直舵角δr，分别作用于机器人推进器及舵机，实现三维路径跟踪控制；
所涉及的水下机器人推进器推力Fu，与水平舵角δs、垂直舵角δr的表达式为：
Fu=m1(-kuu~-kiuϵ1+u·c-ubs)-fuδs=b1-1[m4(-kqq~-kiqϵ2+q·c-qbs)-fq]δr=b2-1[m5(-krr~-kirϵ3+r·c-rbs)-fr]---(6)]]>
其中fu、fq和fr为模型非线性水动力项；ubs、qbs和rbs为反馈补偿鲁棒项；m1、m4、m5分别为由流体产生的附加质量；ku、kq、kr、kiu、kiq和kir均为控制器参数；
所涉及的水下机器人动力学模型为：
u·=m2m1vr-m3m1wq-d1m1u+1m1Fu+ω1]]>
v·=-m1m2ur-d2m2v+ω2]]>
w·=m1m3uq-d3m3w+g1+ω3---(7)]]>
q·=m3-m1m5uw-d4m5q-g2+1m5b1δs+ω4]]>
r·=m1-m2m6uv-d5m6r+1m6b2δr+ω5]]>
其中
m1=m-Xu·,m2=m-Yv·,m3=m-Zw·]]>
m5=Iy-Mq·,m6=Iz-Nr·]]>
g1=(W-B)cosθ,g2=(zgW-zbB)sinθ
d1=Xu+Xu|u||u|,d2=Yv+Yv|v||v|
d3=Zw+Zw|w||w|,d4=Mq+Mq|q||q|
d5=Nr+Nr|r||r|
b1=u2Mδs,b2=u2Nδr]]>
其中，m和m(·)分别表示机器人质量和由流体作用产生的附加质量，Iy为机器人绕y轴的转动惯量，Iz为机器人绕z轴的转动惯量，X(·)、Y(·)、Z(·)、M(·)和N(·)为粘性流体水动力系数；zg和zb分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，ω(·)表示为干扰作用项。
步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψc、θc、uc，滤波姿态与速度跟踪误差量与理想角速度控制量rco、qco，结合步骤4中得到的水下机器人的角速度控制量rc、qc，以及滤波角速度跟踪误差量构造滤波误差补偿回路；
所涉及的滤波误差补偿回路中误差补偿鲁棒项的表达式为：
ψbs=gT(ψ~)BTurυxυyυz---(8)]]>
θbs=gT(θ~)CTurυxυyυz---(9)]]>
ubs=1p21ATυxυyυz---(10)]]>
rbs=1p23υψcosθ---(11)]]>
qbs=1p22υθ---(12)]]>
其中，p21、p22、p23为李雅普诺夫方程的解矩阵中的元素；
A=100,]]>B=cosθecosψc-cosθesinψccosθcsinψccosθccosψc00,]]>C=cosψccosθc-cosψcsinθcsinψecosθc-sinψesinθc-sinθc-cosθc,]]>g(ψ~)=cosψ~-1ψ~sinψ~ψ~,]]>g(θ~)=cosθ~-1θ~sinθ~θ~,]]>且满足limψ~→0g(ψ~)=01,]]>limθ~→0g(θ~)=01;]]>
位置滤波信号补偿误差为
υxυyυz=s~-ζxe~-ζyh~-ζz---(13)]]>
其中位置滤波补偿动态量ζx、ζy和ζz表达式为
ζ·xζ·yζ·z=rζy-qζz-rζxqζx-kxζx-kyζy-kzζz+s·c-s·coe·c-e·coh·c-h·co+urABg(ψ~)Cg(θ~)ζuζψζθ---(14)]]>
且有ζx(0)=0、ζy(0)=0、ζz(0)=0；sco、eco、hco为期望位置信号，sc、ec、hc为位置控制信号，ζu、ζψ和ζθ分别为构造的滤波补偿变量；
滤波器输出的姿态信号补偿量为：
υψυθ=ψ~-ζψθ~-ζθ---(15)]]>
滤波姿态补偿动态表达式为：
ζ·ψ=-kψζψ+rc-rcocosθ+ζrcosθ---(16)]]>
ζ·θ=-kθζθ+(qc-qco)+ζq---(17)]]>
且有ζψ(0)=0，ζθ(0)=0，ζr=0，ζq=0。
步骤7.计算当前水下机器人位置ηn=(x,y,z)与标定的转向点WPk=(xk,yk,zk)之间的距离若小于设定的切换半径R，则表示完成当前指定路径的跟踪任务，否则继续步骤2。
本发明采用滤波反步法进行水下机器人三维路径跟踪控制，通过设计二阶滤波器，能够实现对虚拟控制和其导数信号的估计，避免了对虚拟信号的解析求导，引入滤波补偿系统保证滤波信号的跟踪精度，基于李雅普诺夫稳定性理论保证了系统跟踪误差收敛于零点，控制性能优于动态面控制。
本发明具有如下优点及效果：
1.将三维路径跟踪控制问题，分解为对位置、姿态和速度回路分别控制的问题；
2.采用二阶滤波器获得虚拟控制的滤波信号及其导数形式，避免了反步法设计中由于需要逐步计算中间虚拟控制的导数形式而导致“项数膨胀”的问题，简化了控制器设计过程；
3.通过构造滤波补偿系统，保证了滤波器对参考输入信号的跟踪精度，实现了系统跟踪误差渐近收敛到零点；
4.引入速度的积分环节消除跟踪信号稳态误差。
附图说明
图1中给出基于滤波反步法的AUV三维路径跟踪控制器框图；
图2机器人三维路径跟踪示意图；
图3二阶滤波器结构图；
图4机器人三维路径跟踪轨迹；
图5机器人三维路径跟踪XY平面投影；
图6机器人三维路径跟踪XZ平面投影；
图7机器人三维路径跟踪误差曲线；
图8机器人三维路径跟踪速度响应；
图9机器人三维路径跟踪姿态角响应；
图10机器人三维路径跟踪纵向速度及补偿项；
图11机器人三维路径跟踪艏摇角速度及补偿项；
图12机器人三维路径跟踪纵倾角速度及补偿项；
图13机器人三维路径跟踪艏向角及补偿项；
图14机器人三维路径跟踪纵倾角及补偿项；
图15机器人三维路径跟踪控制输入。
具体实施方式
本发明内容基于二阶滤波器的水下机器人三维路径跟踪控制方法的具体实施方式如下：
1.图中标出了AUV系统的状态信号、虚拟控制量和滤波信号及其导数值的相互关系，前向回路主要通过二阶滤波器获得虚拟控制的滤波信号和导数信号，通过设计滤波误差补偿回路保证滤波器对输入信号的跟踪精度。
2.图3为机器人三维路径跟踪示意图，lk为期望路径，{I}、{B}和{F}分别表示固定坐标系、机器人载体坐标系和Serret-Frenet坐标系；P点为期望路径lk上的虚拟向导，Q点表示机器人质心位置，对于给定期望路径lk为为确定的路径参 数，以lk上虚拟向导P的为原点的移动坐标系{F}定义为将坐标系{I}分别绕ζ轴和η旋转ψF和θF角度，然后平移使固定坐标系原点O与路径上P点重合得到，这里旋转角度定义为


其中首先给出水下机器人三维路径跟踪运动学误差模型
s·=re-qh+u-urcosψecosθee·=-rs+ursinψecosθe+vh·=qs-ursinθe+w]]>
ψ·e=rcosθ-rFθ·e=q-qF---(4)]]>
其中变量s、e和h分别表示{B}坐标系下机器人与期望路径上参考点的前向、横向和垂向跟踪误差，ψ和θ为机器人当前艏向角和纵倾角，状态变量u，v，w，q和r分别表示载体坐标系下机器人的纵向速度、横向速度、垂向速度、纵倾角速度和艏摇角速度；ur为虚拟目标移动速度，其中
忽略横摇对机器人三维运动的影响，建立如下五自由度机器人动力学模型。
u·=m2m1vr-m3m1wq-d1m1u+1m1Fu+ω1]]>
v·=-m1m2ur-d2m2v+ω2]]>
w·=m1m3uq-d3m3w+g1+ω3---(5)]]>
q·=m3-m1m5uw-d4m5q-g2+1m5b1δs+ω4]]>
r·=m1-m2m6uv-d5m6r+1m6b2δr+ω5]]>
其中
m1=m-Xu·,m2=m-Yv·,m3=m-Zw·]]>
m5=Iy-Mq·,m6=Iz-Nr·]]>
g1=(W-B)cosθ,g2=(zgW-zbB)sinθ
d1=Xu+Xu|u||u|,d2=Yv+Yv|v||v|
d3=Zw+Zw|w||w|,d4=Mq+Mq|q||q|
d5=Nr+Nr|r||r|
b1=u2Mδs,b2=u2Nδr]]>
其中，状态变量u，v，w，q和r分别表示载体坐标系下机器人的纵向速度、横向速度、垂向速度、纵倾角速度和艏摇角速度；m和m(·)分别表示机器人质量和由流体作用产生的附加质量，Iy为机器人绕y轴的转动惯量，Iz为机器人绕z轴的转动惯量，X(·)、Y(·)、Z(·)、M(·)和N(·)为粘性流体水动力系数；zg和zb分别为载体坐标下垂直轴上重心和浮心的坐标位置，W和B分别表示机器人受到的重力和浮力，d(·)为非线性阻尼水动力项，和为水平舵和垂直舵舵效系数，控制输入Fu、δs和δr分别表示AUV推进器推力、水平舵角和垂直舵角，ω(·)表示为干扰作用项。
3.步骤3中滤波器的设计过程为
为了避免对虚拟控制量直接解析求导而引入复杂的计算过程，利用二阶滤波器的特性，将虚拟控制量作为滤波器的参考输入，通过积分而非微分的过程获得其滤波信号和导数值，滤波器结构定义如下：
x1x·2=01-ωn2-2ζωnx1x2+0ωn2x‾---(6)]]>
其中x1x2T=xcx·cT,]]>为滤波器的参考输入信号，上式为线性稳定系统，可见当为有界值，则xc和均为连续有界信号，从输入信号到输出信号xc的传递函数为
H(s)=Xc(s)X‾(s)=ωn2s2+2ζωns+ωn2---(7)]]>
其中ζ和ωn分别表示阻尼比和自然角频率；如果信号的带宽低于H(s)，那么误差信号将会很小，假设在已知带宽的情况下，通过选择足够大的自然角频率ωn就能够获得xc和xc且保证逼近误差很小。从上式可以看出，信号是通过积分过程而非微分过程得到的，这可以大大减少基于状态反馈设计的控制系统中测量噪声的影响，同时选择过大的ωn又会增加系统高频噪声的影响，这就需要综合考虑，选择合理的ωn满足控制性能。
4.步骤6中针对位置跟踪回路，根据视线角导引思想给出路径跟踪的期望视线角导引律和纵向速度，并设计位置跟踪滤波补偿系统的过程为
对于位置跟踪误差系统(3)设计机器人的运动学控制器u、姿态角ψ和θ的虚拟控制量分别为
uco=-k1s+urcosψcocosθco   (8)
ψco=-arcsin(k2e/1+(k2e)2)---(9)]]>
θco=arcsin(k3h/1+(k3h)2)---(10)]]>
其中增益因子k1>0，k2>0，k3>0为视线角导引律归一化参数，则系统(3)变为
s·=re-qh-k1se·=-rs-urk2e1+(k2e)211+(k3h)2+vh·=qs-urk3h1+(k3h)2+w---(11)]]>
对于位置跟踪误差系统式(3)，构造李雅普诺夫能量函数
V1=12l2---(12)]]>
其中对方程(12)求导，将式(11)代入得
V·1=-k1s2-k2ur11+(k2e)211+(k3h)2e2---(13)]]>
-k3ur11+(k3h)2h2+ev+hw]]>
为避免反步法后续设计中需要对θco和ψco进求导，引起计算“项数膨胀”的不足，定义θc，和ψc，为理想信号θco和ψco通过二阶滤波器的滤波信号和其导数值，滤波器定义如下
θ·cθ··c=01-ωn2-2ζωnθcθ·c+0ωn2θco---(14)]]>
滤波器的初始值θc(0)=θco(0)；
ψ·cψ··c=01-ωn2-2ζωnψcψ·c+0ωn2ψco---(15)]]> 滤波器的初始值ψc(0)=ψco(0)；
这里定义位置滤波跟踪误差信号
s~e~h~=s-sce-ech-hc---(16)]]>
对式(16)两边求导，将式(3)代入得
s~·e~·h~·=re~-qh~-rs~qs~+ABg(ψ~)urCg(θ~)uru~ψ~θ~---(17)]]>
其中，定义滤波跟踪误差为u~=u-uc,]]>ψ~=ψe-ψc,]]>θ~=θe-θc,]]>
A=100,]]>B=cosθecosψc-cosθesinψccosθcsinψccosθccosψc00]]>
C=cosψccosθc-cosψcsinθcsinψecosθc-sinψesinθc-sinθc-cosθc]]>
g(ψ~)=cosψ~-1ψ~sinψ~ψ~,]]>g(θ~)=cosθ~-1θ~sinθ~θ~,]]>且满足limψ~→0g(ψ~)=01,]]>limθ~→0g(θ~)=01]]>
对式(16)左右移项，增加和减少一项s·coe·coh·coT]]>得到
s·e·h·=s·coe·coh·co+s~·e~·h~·+s·c-s·coe·c-e·coh·c-h·co---(18)]]>
根据跟踪控制器设计目标，选择理想期望位置信号s·coe·coh·coT]]>为
s·coe·coh·co=-kxs~+s·c-kye~+e·c-kzh~+h~c---(19)]]>
定义角度滤波跟踪误差信号为
u~ψ~θ~=u-ucψe-ψcθe-θc---(20)]]>
将式(19)和(17)代入(18)整理得
s~·e~·h~·=re~-qh~-rs~qs~+-kxs~-kye~-kzh~+s·c-s·coe·c-e·coh·c-h·co+ABg(ψ~)urCg(θ~)uru~ψ~θ~---(21)]]>
定义滤波信号补偿误差
υxυyυz=s~-ζxe~-ζyh~-ζz---(22)]]>
其中构造位置滤波补偿动态ζx、ζy和ζz如下
ζ·xζ·yζ·z=rζy-qζz-rζxqζx-kxζx-kyζy-kzζz+s·c-s·coe·c-e·coh·c-h·co+urABg(ψ~)Cg(θ~)ζuζψζθ---(23)]]>
这里ζx(0)=0、ζy(0)=0、ζz(0)=0。
5.步骤6中针对姿态角跟踪回路，设计角速度控制律和姿态角跟踪滤波补偿系统的过程为：
对滤波跟踪误差式(20)求导，将姿态角跟踪误差模型式(4)代入得
ψ~·=rcosθ-rF-ψ·c=rco+(rc-rco)+r~cosθ-rF-ψ·c---(24)]]>
θ~·=q-qF-θ·c=qco+(qc-qco)+q~-qF-θ·c---(25)]]>
其中角速度跟踪误差定义为这里分别设计理想虚拟控制信号rco和qco为
rco=cosθ(rF+ψ·c-kψψ~-ψbs)---(26)]]>
qco=qF+θ·c-kθθ~-θbs---(27)]]>
这里qc和rc和分别为理想信号qco和rco通过二阶滤波器得到的滤波信号和其导数值，定义滤波器形式为
q·cq··c=01-ωn2-2ζωnqcq·c+0ωn2qco---(28)]]>
滤波器初始条件qc(0)=qco(0)；
r·cr··c=01-ωn2-2ζωnrcr·c+0ωn2rco---(29)]]>
滤波器初始条件rc(0)=rco(0)；
将式(26)和式(27)代入式(24)和(25)得
ψ~·=-kψψ~+(rc-rco)+r~cosθ-ψbs---(30)]]>
θ~·=-kθθ~+(qc-qco)+q~-θbs---(31)]]>
其中kψ>0和kθ>0为控制器参数，ψbs和θbs为待设计镇定项，这里通过定义补偿跟踪误差，对滤波器输出的信号进行反馈补偿
υψυθ=ψ~-ζψθ~-ζθ---(32)]]>
这里构造滤波补偿动态
ζ·ψ=-kψζψ+rc-rcocosθ+ζrcosθ---(33)]]>
ζ·θ=-kθζθ+(qc-qco)+ζq---(34)]]>
这里初始条件ζψ(0)=0，ζθ(0)=0，ζr=0，ζq=0。
6.步骤6中针对速度控制回路，引入积分环节减小稳态误差，设计真实控制输入的具体过程如下：
为保证跟踪系统存在外干扰下的鲁棒性，引入积分项增加系统的鲁棒性，定义这里设计机器人三维路径跟踪控制器为
Fu=m1(-kuu~-kiuϵ1+u·c-ubs)-fuδs=b1-1[m4(-kqq~-kiqϵ2+q·c-qbs)-fq]δr=b2-1[m5(-krr~-kirϵ3+r·c-rbs)-fr]---(35)]]>
其中fu=m2vr-m3wq+d1u、fq=(m1-m3)uw+d4q-g2和fr=(m1-m2)uv+d5r为模型非线性水动力项，ubs、qbs和rbs为待设计反馈补偿鲁棒项在步骤6中进行设计。
7.步骤6中反馈控制项设计的具体过程如下：
首先对于步骤2中位置跟踪系统结合式(22)构造李雅普诺夫能量函数
V1=12(υx2+υy2+υz2)---(36)]]>
对上式求导，将式(21)和(23)代入式(36)得
V1=υ·xυx+υ·yυy+υ·zυz]]>
=-kxυx2-kyυy2-kzυz2+]]>
υxυyυzABg(ψ~)urCg(θ~)urυuυψυθ]]>
=-kxυx2-kyυy2-kzυz2+ATυxυyυzυu+]]>
gT(ψ~)BTurυxυyυzυψ+gT(θ~)CTurυxυyυzυθ---(37)]]>
其中υu、υψ、υθ定义如式(32)。
然后对于姿态跟踪系统结合式(32)构造李雅普诺夫能量函数
V2=12(υψ2+υθ2)---(38)]]>
对上式求导，将式(30)～(34)代入得:
V·2=υ·ψυψ+υ·θυθ]]>
=(ψ~·-ζ·ψ)υψ+(θ~·-ζ·θ)υθ]]>
=(-kψψ~+r~cosθ-ψbs+kψζψ-ζrcosθ)υψ---(39)]]>
+(-kθθ~+q~-θbs+kθζθ-ζq)υθ]]>
=-kψυψ2-kθυθ2+υqυθ+υrcosθυψ]]>
-θbsυθ-ψbsυψ]]>
其中υu=u~-ζu,]]>υr=r~-ζr,]]>υq=q~-ζq.]]>
再次对于速度控制回路将式(35)代入式(5)得到u、q和r的误差系统为
u~·=-kuu~-kiuϵ1-ubsq~·=-kqq~-kiqϵ2-qbsr~·=-krr~-kirϵ3-rbs---(40)]]>
由于ζu=ζr=ζq=0，这里得到滤波补偿误差系统的动态为
υ·u=-kuυu-kiuϵ1-ubsυ·q=-kqυq-kiqϵ2-qbsυ·r=-krυr-kirϵ3-rbs---(41)]]>
由于所以系统(41)可以重写为
ϵ··1=-kuϵ·1-kiuϵ1-ubsϵ··2=-kqϵ·2-kiqϵ2-qbsϵ··3=-krϵ·3-kirϵ3-rbs---(42)]]>
定义误差向量ε=[ε1,ε2,ε3]T，则系统(42)可以表示为
E·=AE+BU---(43)]]>
其中A=03×3I3×3-KI3×3-kP3×3,]]>B=03×3I3×3,]]>U=-ubs-qbs-rbs,]]>KI=diag{-kiu,-kiq,-kir}，KP=diag{-ku,-kq,-kr}
最后结合式(36)、(43)和式(38)构造李雅普诺夫能量函数
V3=V1+V2+12ETPE---(44)]]>
其中正定对称矩阵P为线性李雅普诺夫方程的解
ATP+PA=-Q   (45)
其中P=P103×303×3P2,]]>Pi=diag{pi1,pi2,pi3}，i=1,2为正定对称矩阵，如果选择P1=KIP2，则Q=03×303×303×32KIP2;]]>对式(44)进行求导，将式(37)、(39)和(43)代入整理得
V·3=V·1+V·2+ETPE·]]>
=-kxυx2-kyυy2-kzυz2-kuυu2-kψυψ2-kθυθ2]]>
-12ETQE+ATυxυyυzυu+gT(ψ~)BTurυxυyυzυψ---(46)]]>
-ψbsυψ+υrcosθυψ+gT(θ~)CTurυxυyυzυθ]]>
+υqυθ-θbsυθ+ETPBU]]>
式(46)进一步变为
V·3=-kxυx2-kyυy2-kzυz2-kuυu2-kψυψ2-kθυθ2]]>
-12ETQE+ATυxυyυzυu+gT(ψ~)BTurυxυyυzυψ+υqυθ+υrcosθυψ+gT(θ~)CTurυxυyυzυθ-ψbsυψ---(47)]]>
-θbsυθ-p21υuubs-p22υqqbs-p23υrrbs]]>
如果设计反馈补偿项为
ψbs=gT(ψ~)BTurυxυyυz---(48)]]>
θbs=gT(θ~)CTurυxυyυz---(49)]]>
ubs=1p21ATυxυyυz---(50)]]>
rbs=1p23υψcosθ---(51)]]>
qbs=1p22υθ---(52)]]>
将式(48)(48)～(52)(52)代入式(47)(47)整理得
V·3=-kxυx2-kyυy2-kzυz2-kψυψ2-kθυθ2]]>
-12ETQE≤0---(53)]]>
上述定理证明了补偿跟踪误差系统υi的指数收敛性，由二阶滤波器的设计过程可知，当选择合适的自然频率ωn，xco为滤波器的参考输入信号，滤波器为线性稳定系统，可见当xco为有界值，则xc和均为连续有界信号，如果信号xco的带宽低于滤波器设计带宽，那么误差信号|xco(t)-xc(t)|将会很小，由于ζi是一阶稳定线性系统，所以ζi将趋近于零值，从而系统跟踪误差指数趋近于零值。由于在控制回路中引入了滤波器，滤波器的跟踪精度直接影响系统的控制性能，动态面控制由于未考虑滤波信号的跟踪精度，只能够保证系统跟踪误差收敛到原点较小的邻域。这里通过构造滤波补偿系统，提高滤波信号跟踪精度，通过稳定性分析保证闭环系统跟踪误差收敛到零点。
仿真验证
下面举例说明，验证本发明方法的有效性：
根据水动力系数建立机器人六自由度仿真模型，采用Matlab环境搭建机器人三维路径跟踪控制仿真系统。
针对机器人螺旋下潜作业，规划期望三维曲线路径为(单位：m)

选取机器人的初始位置为[x,y,z]T=[10,-5,1]T(m)，初始艏向为ψ=π/4(rad)，纵倾角θ=0(rad)，机器人初始速度为[u,v,w]T=0(m/s)，初始角速度q=0(rad/s)，r=0(rad/s)，控制器参数kx=ky=kz=2，kψ=kθ=5，ku=kq=kr=20，kiu=kiq=kir=10，p21=p22=p23=5。
为了避免对虚拟控制量直接解析求导，引入复杂的计算过程，本文利用二阶滤波器的特性，将理想虚拟控制量aco作为滤波器的参考输入，信号是通过积分而非微分的过程获得的，这可以大大减少基于状态反馈设计的控制系统中测量噪声的影响，假设在已知aco带宽的情况下，通过选择足够大的自然角频率ωn就能够获得ac和且保证逼近误差|αco(t)-αc(t)|很小；同时选择过大的ωn又会增加系统高频噪声的影响，这就需要综合考虑，选择合理的ωn满足控制性能，这里选取ζ=0.9，ωn=20；
图2～图13给出机器人三维曲线路径跟踪控制仿真对比结果。
图2为机器人三维螺旋下潜路径跟踪轨迹，图3和图4分别为机器人三维路径跟踪轨迹在XY平面和XZ平面的投影曲线。从中可以看出由于常规反步法基于精确数学模型设计的控制器应用于真实模型时，直接对虚拟控制求导获得其导数得解析形式，在存在模型不确定性和测量噪声时控制效果较差，而本文基于滤波器设计的非线性控制器，通过积分过程而非微分过程获得虚拟控制的滤波值和导数值因而对测量噪声具有一定的滤波作用，通过滤波补偿系统，能够保证理想虚拟控制量的滤波值对真实状态的逼近，进而补偿标称模型的状态响应与真实模型状态响应的偏差，对模型不确定性具有较好的鲁棒性，能够很好的实现跟踪控制，提高了跟踪精度。
图5为机器人三维路径跟踪控制中跟踪误差曲线，与常规反步法控制器相比，可以看出本文设计的三维路径控制器提高了路径跟踪的精度，缩短了机器人的冗余航程，具有更加稳定的控制能力保证机器人较快的跟踪并收敛到期望路径，使得跟踪误差最终收敛到零，表明了控制器具有良好的跟踪精度和响应速度。
图6和图7分别为机器人三维里路径跟踪控制过程中各状态变量包括线速度和姿态角的变化曲线，可以看出机器人在沿螺旋线下潜过程中横向速度和垂向速度相比于纵向速度较小，且为有界值，常规反步法设计过程无法处理系统状态的测量噪声，而本文基于滤波器设计的控制器对系统的测量噪声具有一定的滤波效果。
图8为机器人纵向速度u、理想虚拟控制量uco和其滤波信号uc的响应曲线，从局部放大图中可以看出滤波信号uc较好的跟踪了理想虚拟信号uco，滤波器对于状态u包含的测量噪声具有一定的滤波作用，ubs为滤波补偿项，当跟踪系统稳定时ubs最终稳定且收敛于零。图9为机器人艏摇角速度r、理想虚拟控制量rco和其滤波信号rc的响应曲线，从局部放大图中可以看出滤波信号rc较好的跟踪了理想虚拟信号rco，滤波器对于艏摇角速度r包含的测量噪声具有一定的滤波作用，rbs为滤波补偿项，当跟踪系统稳定时rbs最终收敛到零。
图10为机器人纵倾角速度q、理想虚拟控制量qco和其滤波信号qc的响应曲线，从局部放大图中可以看出滤波信号qc较好的跟踪了理想虚拟信号qco，滤波器对于艏摇角速度q包含的测量噪声具有一定的滤波作用，qbs为滤波补偿项，当跟踪系统稳定时qbs最终稳定收敛于零。
图11和12分别为滤波反步法设计中的机器人艏摇角和纵倾角、理想控制信号和滤波信号变化曲线，从局部放大图可以看出滤波信号ψc和θc较好的跟踪了理想虚拟信号ψco和θco，滤波器对于艏摇角ψ和纵倾角θ中包含的测量噪声具有一定的滤波作用，从滤波补偿项ψbs和θbs的变化趋势可以看出，当跟踪系统稳定时ψbs和θbs将最终收敛到零点。
图13为机器人三维路径跟踪控制输入响应，由图可知本发明的方法响应曲线更加平滑。