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

第六章 导引技术（Guidance） 对于航行器来说，导引和控制系统通常包含： ?姿态控制系统 ?路径控制系统 姿态反馈控制系统的最简单的形式是航向自动驾驶仪。 姿态控制系统的主要功能是通过控制航行器的横滚角、俯仰角和航向角，使得航行器在指定的路径上保持期望的姿态。 路径控制器的主要任务是通过给姿态控制系统特定的指令，使航行器在规定的路径上航行，并具有预先确定的动态特性。 6.1 参考模型 最简单的参考模型是采用低通滤波器（LP）结构： 滤波器结构的选取应能反映航行器的动态特性，从而能确保所构造的路径能够实现。譬如，应考虑航行器的物理速度和加速度约束。 选取参考模型的带宽必须小于航行器控制系统的带宽，从而可确保满意的跟踪性能和稳定性。 6.1 参考模型 对于水下航行器来说，选用质量-阻尼器-弹簧参考模型： 写成MIMO质量-阻尼器-弹簧参考模型 对应状态空间模型 6.1.1 速度参考模型 为了得到平滑的速度指令和加速度指令，速度参考模型至少是二阶。 其中 对速度参考模型进行求导，并令，则 6.1.1 速度参考模型 式中，为期望速度，为期望加速度。即使是阶跃信号，通过低通滤波器后，和在控制系统中仍然是平滑信号。 6.1.2 位置和姿态参考模型 位置和姿态参考模型至少是三阶。 式中 上式也可以写成 6.1.2 位置和姿态参考模型 写成向量形式： 状态空间模型表示： 即使是阶跃信号，通过低通滤波器后，、和在控制系统中仍然是平滑信号。 6.1.3 饱和元素 线性参考模型的缺点。 6.1.4 非线性阻尼 式中 式中，，为大于0的整数。 例如6.1 6.1.4 非线性阻尼 ，， 6.2 航路点导引系统 6.2.1 轨迹跟踪控制 定义（跟踪问题）：当目的是使系统的输出跟踪期望的输出，我们称之问题为跟踪问题（或轨迹跟踪问题）。 定义（参数化路径）：参数化路径是由连续路径变量表示的几何曲线。 注意到路径与是无关，且当=，路径就变成轨迹。 6.2.2 操纵控制 定义（操纵问题）：操纵问题包含解决二个任务 1）几何任务：使系统的输出收敛到期望的路径： 2）动态任务：使系统的速度收敛到期望的速度： 由上面的定义可知：路径的动态特性可以独立定义，与误差动态特性无关。 操纵问题的一个特殊情况是： 这是，操纵问题变成跟踪问题。 6.2.2 操纵控制 路径特征向量： 操纵问题： 收敛到期望路径，例如通过定义 收敛到期望速度 6.2.3 航路点表示 航路点： 航路点数据库： 其它特性包括： 6.2.3 航路点表示 在工程实际中，通常用直线和圆弧来连接各航路点。在每个航路点用半径来表示圆弧： 其缺点是在直线段和圆弧段， 其偏航角速度指令存在跳变。 6.2.3 航路点表示 圆弧和直线的 交点就是航行器的 转弯点，内切圆的 半径为 如图中定义。 6.2.4 用航行器模拟器的轨迹生成 时变参考轨迹可以用航行器模拟器来产生，模拟器的时间常数、相对阻尼比、自然频率要反映航行器的实际约束。 其中自然频率简化为对角矩阵 平滑参考轨迹采用P-D控制器 式中，为航路点。 6.2.4 用航行器模拟器的轨迹生成 例如：航行器的期望参考轨迹为 轴向速度的动态为 6.2.4 用航行器模拟器的轨迹生成 航向的动态特性选为 式中，为设计参数。 导引系统有两个输入：推力和舵角。 式中 6.2.5 用插值方法生成路径和轨迹 将各个航路点用样条插值或多项式插值方法可以生成路径。 轨迹可令，使得得到。 三次Hermite插值（pchip.m）连续。 三次样条插值（spline.m）可以确保二阶导数连续。 令三次多项式 6.2.5 用插值方法生成路径和轨迹 偏导数： 时间导数： 速度： 未知参数： 6.2.5 用插值方法生成路径和轨迹 三次样条算法： 路径通过航路点和必须满足： ， 平滑性要求必须满足： 边界条件（速度或加速度）： ， 或 ， 6.2.5 用插值方法生成路径和轨迹 多项式由参数确定， 6.2.5 用插值方法生成路径和轨迹 用期望速度/加速度分布图，可将路径转换为参考轨迹： 令为一阶系统的输入： 其中，为每一个航路点的期望速度。对上式积分可得，为两航路点之间的速度分布图。 将下式积分： 对应的轨迹： 6.2.5 用插值方法生成路径和轨迹 例