把角速度 和 平移到地理坐标系的原点,并投影到地理

第六章导引技术（Guidance）对于航行器来说，导引和控制系统通常包含：姿态控制系统路径控制系统姿态反馈控制系统的最简单的形式是航向自动驾驶仪。姿态控制系统的主要功能是通过控制航行器的横滚角、俯仰角和航向角，使得航行器在指定的路径上保持期望的姿态。路径控制器的主要任务是通过给姿态控制系统特定的指令，使航行器在规定的路径上航行，并具有预先确定的动态特性。6.1参考模型最简单的参考模型是采用低通滤波器（LP）结构：()()dlpxshsr参考模型rdx滤波器结构的选取应能反映航行器的动态特性，从而能确保所构造的路径能够实现。譬如，应考虑航行器的物理速度和加速度约束。选取参考模型的带宽必须小于航行器控制系统的带宽，从而可确保满意的跟踪性能和稳定性。6.1参考模型对于水下航行器来说，选用质量-阻尼器-弹簧参考模型：222()2nilpininihsss写成MIMO质量-阻尼器-弹簧参考模型dddddddMDGGr对应状态空间模型ddddxAxBr11100,,0dddddddddIABCIMGMDMG6.1.1速度参考模型为了得到平滑的速度指令dv和加速度指令dv，速度参考模型至少是二阶。dddddddMDGGr其中22,ddddDMGM120ndiag120nnnndiag对速度参考模型进行求导，并令,bddvrr，则6.1.1速度参考模型222bdddvvvr式中，dv为期望速度，dv为期望加速度。即使br是阶跃信号，通过低通滤波器后，dv和dv在控制系统中仍然是平滑信号。()limbdtvtr参考模型brdvdv6.1.2位置和姿态参考模型位置和姿态参考模型d至少是三阶。222()(1,,)(1)(2)idniniiininisinrTsss式中1iinT上式也可以写成23223()(1,,)(21)(21)idniniiniininisinrsss6.1.2位置和姿态参考模型写成向量形式：(3)233(2)(2)nddddIIr状态空间模型表示：32300000,0(2)(2)ddIAIBII即使nr是阶跃信号，通过低通滤波器后，d、d和d在控制系统中仍然是平滑信号。()limndttr6.1.3饱和元素线性参考模型的缺点。maxmaxsgn()()xxifxxsatxxelsemaxmax,iiiivvvv6.1.4非线性阻尼(3)233(2)(2)()ndddddIdIr式中(),(1,)jiiipidijddjdin式中，0ij，jp为大于0的整数。例如6.1222dddndddndnxvvvvvxr1n6.1.4非线性阻尼0，1，max1v6.2航路点导引系统6.2.1轨迹跟踪控制定义（跟踪问题）：当目的是使系统的输出()yt跟踪期望的输出()dyt，我们称之问题为跟踪问题（或轨迹跟踪问题）。定义（参数化路径）：参数化路径是由连续路径变量表示的几何曲线(),1qdRq。注意到路径与t是无关，且当=t，路径就变成轨迹。6.2.2操纵控制定义（操纵问题）：操纵问题包含解决二个任务1）几何任务：使系统的输出()yt收敛到期望的路径(())dyt：lim[()(())]0tdytyt2）动态任务：使系统的速度收敛到期望的速度sv：lim[()(())]0tstvt由上面的定义可知：路径的动态特性()t可以独立定义，与误差动态特性无关。操纵问题的一个特殊情况是：()1,(0)0t这是，操纵问题变成跟踪问题。6.2.2操纵控制路径特征向量：3DOF()()()Tddddxyz操纵问题：1）收敛到期望路径()()Tddxy，例如通过定义()d2）收敛到期望速度()t(0)0h()()Tddxy速度向量6.2.3航路点表示航路点：(,,)kkkxyz航路点数据库：000111wpt.pos(,,)(,,)(,,)nnnxyzxyzxyz其它特性包括：01wpt.speednUUU01wpt.headingn6.2.3航路点表示在工程实际中，通常用直线和圆弧来连接各航路点。在每个航路点用半径iR来表示圆弧：01wpt.radiusnRRR其缺点是在直线段和圆弧段，其偏航角速度指令dr存在跳变。6.2.3航路点表示圆弧和直线的交点就是航行器的转弯点，内切圆的半径为taniiiRRi如图中定义。6.2.4用航行器模拟器的轨迹生成时变参考轨迹可以用航行器模拟器来产生，模拟器的时间常数、相对阻尼比、自然频率要反映航行器的实际约束。()()ddddddJvMvNvg其中自然频率简化为对角矩阵160Ndiagnn平滑参考轨迹(,)ddv采用P-D控制器()()()TddpdrefddgJKk式中，ref为航路点(,,)kkkxyz。6.2.4用航行器模拟器的轨迹生成非线性PID控制器包含物理约束的航行器动态模型航路点反馈（从航路点数据库）时变参考轨迹(,)ddv（给控制系统）例如：航行器的期望参考轨迹为cos,sinddddddxUyU轴向速度dU的动态为21()2udddmXUCAU6.2.4用航行器模拟器的轨迹生成航向的动态特性选为ddddrTrrK式中，,TK为设计参数。导引系统有两个输入：推力和舵角。1102220()()()()tpdrefidreftpdrefidrefddKUUKUUdKKdKr式中tan2((),())refkdkdayytxxt6.2.5用插值方法生成路径和轨迹将各个航路点用样条插值或多项式插值方法可以生成路径((),())ddxy。轨迹((),())ddxtyt可令k，使得(1,,)kttN得到。三次Hermite插值（pchip.m）可以确保一阶导数((),())ddxtyt连续。三次样条插值（spline.m）可以确保二阶导数((),())ddxtyt连续。令三次多项式323210323210()()ddxaaaaybbbb6.2.5用插值方法生成路径和轨迹偏导数：时间导数：23212321()()32()()32dddddxxaaaddyybbbd()()()()dddddxxddyyd速度：2222()()()dddddUtxyxy未知参数：32103210,,,,,,,aaaabbbb6.2.5用插值方法生成路径和轨迹三次样条算法：路径通过航路点11(,)kkxy和(,)kkxy必须满足：1111()()dkkdkkxxyy，()()dkkdkkxxyy平滑性要求必须满足：lim()lim()lim()lim()kkkkdkdkdkdkxxxx边界条件（速度或加速度）：0000()()ddxxyy，()()dnndnnxxyy或0000()()ddxxyy，()()dnndnnxxyy6.2.5用插值方法生成路径和轨迹多项式()dkx由参数3210Tkkkkkaaaaa确定，6.2.5用插值方法生成路径和轨迹用期望速度/加速度分布图，可将路径转换为参考轨迹：令refU为一阶系统的输入：()(),0ddrefTUtUtUT其中，refU为每一个航路点的期望速度。对上式积分可得()dUt，()dUt为两航路点之间的速度分布图。将下式积分：22()()()()()dddUtttxy对应的轨迹：((),())((()),(()))ddddxtytxtyt6.2.5用插值方法生成路径和轨迹例如：现有两个航路点：0011(,)(0,0)(,)(200,200)xyxy满足上述条件的三次多项式为：3232()29.89135.6394.25,0,1()29.89135.6394.25ddxy令速度的动态时间常数10T（s），并假设航行器在初始时刻处于静止状态0(()0)dUt，第一个航路点的期望速度1()5.0(/)refdUUtms对下面两式进行积分可得到对应于航路点0和1的数值解6.2.5用插值方法生成路径和轨迹()(),0ddrefTUtUtUT22()()()()()dddUtttxy航路点0对应于00()0t，航路点1对应于11()1t，其中00t，1t未知。从图中可以看出，要达到期望速度5.0（m/s），至少需要67（s）。因此，选择167()ts。6.3视线导引用于路径跟踪控制一种吸引人的方法是视线导引法。视线：()tan2(,)dktkttayyxx视线指向下一个航路点的缺点是当下一个航路点比较远时，会产生较大的横向误差。6.3视线导引改进的视线导引算法()tan2(,)dlostlosttayyxx式中2221111(())(())()constantloslospploskkkloskkkyytxxtnLyyyyxxxx6.4方位线导引初始位置目标位置),(ii),(ff),(if),(figcostangtancosgdd1tanlntan()/tan()4242figfi