具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法.docx

具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法专利名称：具有pid反馈增益的自主水下航行器三维直线路径跟踪控制方法技术领域：本发明涉及的是一种自主水下航行器的控制方法，具体地说是一种欠驱动自主水下航行器的三维航迹点和直线路径跟踪控制方法背景技术：自主水下航行器(AUV,Autonomous Underwater Vehicle)三维路径跟踪控制相对于分别实现解耦的水平面和深度控制具有显著的优点由于考虑了模型自身的耦合作用，能够更加满足海底三维空间地形/地貌的测绘、海底管道的探测以及跟踪规划航迹等作业需求，由于自主水下航行器AUV的执行机构通常配置为尾部轴向推进器、尾部方向舵和尾部升降舵，在横向和垂向方向上无直接驱动机构(如推进器)，使得AUV满足二阶非完整约束条件，为欠驱动系统，且由于受到外界海流干扰作用和自身模型参数的不确定性影响，这都成为AUV三维航迹跟踪控制器设计的难点AUV水下三维空间跟踪控制研究的三个方面I)航迹点跟踪，要求AUV跟踪并收敛到空间离散点；2)轨迹跟踪，要求AUV跟踪空间中一条以时间为参数的轨迹，具有时间条件约束，即要求AUV在指定时刻运动到指定位置；3)路径跟踪，要求AUV跟踪空间一条与时间无关的曲线，使AUV收敛到给定的曲线，而对何时到达何处没有要求。

航迹点也称作路径点为可通过水下三维空间坐标表示的离散点，路径则可由一系列离散的点相连构成，三维直线路径就是由一系列三维空间离散点的序列依次相连构成，而空间任意的曲线路径也可以通过多段直线路径进行拟合目前，国内外对AUV的航迹(包括航迹点、轨迹和路径)跟踪控制研究多集中于水平面的跟踪控制，或将三维跟踪控制问题解耦为水平面和垂直面的两个独立的子系统分别进行控制器设计，由于未整体考虑AUV的六自由度的耦合运动模型和三维跟踪误差方程，使得解耦的控制器设计思路无法实现对于给定空间航迹的精确跟踪控制《基于自适应Backst印ping的欠驱动AUV三维航迹跟踪控制》(控制与决策，2012，第38卷第2期)为三维航迹点跟踪控制，依据视线法(Line ofSight,，LOS)计算期望跟踪视线角，基于自适应反步法设计跟踪控制器，并未涉及将航迹点跟踪转化为参数化路径跟踪控制问题,且跟踪导引策略为视线法(Line of Sight, L0S)而本专利采用的为虚拟向导策略(Virtual Guidance)，通过跟踪期望路径上的“虚拟向导”点实现AUV收敛于期望路径；《自主式水下航行器三维路径跟踪的神经网络H00鲁棒自适应控制方法》(控制理论与应用，2012，第29卷第3期)基于正交投影Serret-Frenet坐标系建立AUV三维路径跟踪误差方程，运用H00鲁棒控制思想设计控制器，同时引入神经网络补偿模型不确定性，但由于基于正交投影Serret-Frenet坐标系建立AUV三维路径跟踪误差模型存在奇异值点，使得对AUV的初始条件有约束，即AUV初始位置必须位于跟踪曲线最小曲率半径内，因此无法实现AUV跟踪的全局收敛性，而本专利建立的三维跟踪误差模型不存在奇异值问题，因此能够保证AUV跟踪误差的全局收敛性,此外本专利控制器设计基于反步法(Backstepping)不同于H00鲁棒控制设计思想；《基于L2干扰抑制的水下机器人三维航迹跟踪控制》(控制理论与应用，2011，第28卷第5期)提出基于L2干扰抑制的鲁棒神经网络三维航迹跟踪控制器，其研究内容是以时间为参数的轨迹跟踪控制问题，而本专利针对参数化路径(不包含时间参数)设计跟踪控制器；《基于离散滑模预测的欠驱动AUV三维航迹跟踪控制》(控制与决策，2011，第26卷第10期)针对离散系统采用递归滑模思想设计离散滑模预测控制器，利用滚动优化和反馈校正方法补偿了不确定项对滑模预测模型的影响，而本专利针对连续系统基于反步法设计三维路径跟踪控制器；《基于非线性迭代滑模的欠驱动UUV三维航迹跟踪控制》(自动化学报，2012，第38卷第2期)基于工程控制器解耦的思想设计非线性迭代滑模航迹跟踪控制器，由于被控对象模型为六自由度耦合运动模型，因此针对纵向速度、艏向控制和纵倾控制分别设计的解耦控制器只能通过鲁棒项抑制模型中的耦合作用，当模型各自由度之间的耦合作用较明显时，控制器只能通过输出较高的控制器增益为代价消除耦合作用，引起控制器输出饱和信号，解耦的控制器只能够保证三个独立控制子系统的渐近稳定性，从而保证系统是稳定的，而无法获得整个控制系统的渐近稳定性，而本专利提出的三维航迹跟踪控制器采用反步法迭代构造李雅普诺夫函数，能够保证整个系统全局渐近稳定性，且不同于迭代滑模单独设计子系统跟踪控制器的思想；《3D Path Following forAutonomous Underwater Vehicle》 (Proceedings ofthe39th Conference on Decision and Control, 2000,悉尼，澳大利亚)利用正交投影的思想建立AUV三维路径跟踪误差模型，由于存在奇异值点，对AUV的初始条件有约束，而本专利建立的三维跟踪误差模型不存在奇异值问题，因此能够保证AUV跟踪误差的全局收敛性，基于传统反馈线性化方法，得到的控制器结构形式复杂，不利于工程应用，而本专利基于反馈增益反步法，通过设计控制器参数简化了虚拟控制量的形式，保证了跟踪系统稳定性的同时得到的最终控制器形式具有PID控制器的增益形式，且参数调节规律可以参考PID控制器参数整定方法，有助于工程应用。

发明内容本发明的目的在于提供一种能够实现对AUV三维空间航迹点和直线航迹的跟踪控制的具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法本发明的目的是这样实现的步骤I.初始化，通过AUV搭载的测量传感器(包括超短基线定位声纳、姿态传感器)采集数据，可获得AUV当前位置坐标、方位角和沿载体坐标系三轴的线速度和角速度数据信息，将给定期望航迹点转化为分段连续直线三维航迹的参数方程描述，并初始化“虚拟向导”点的位置坐标；步骤2.通过AUV三维航迹跟踪误差方程计算AUV与期望航迹上“虚拟向导”点在Serret-Frenet坐标系下的相对位置偏差(跟踪偏差表示在以“虚拟向导”点为坐标系原点，由固定坐标系分别绕 轴和Π旋转11^和Gf角度，然后平移使固定坐标系原点O与路径上P点重合得到，称为弗雷涅-塞雷(Serret-Frenet)坐标系)；步骤3.基于步骤2中计算得到的跟踪误差，采用基于反馈增益的反步法设计思想，计算期望路径上“虚拟向导”点的移动速度、纵倾角和艏摇角的虚拟控制量；步骤4.在步骤3的基础上，计算AUV纵倾角速度和艏摇角速度的虚拟控制量；步骤5.根据给定的AUV六自由度数学模型，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律包括推进器推力、纵倾控制力矩和转艏控制力矩控制信号，然后采用该控制器实现欠驱动AUV的三维直线航迹跟踪控制；步骤6.计算当前AUV位置nn = (X，y, z)与标定的转向点WPk = (xk, yk, zk)之间的距离J = V(X-Xt)2,若小于设定的航迹切换半径R，则表示完成当前指定航迹的跟踪任务停止航行或切换下一段期望航迹，否则继续步骤2。

本发明相对现有技术具有如下的优点及效果I.本发明将三维航迹点的跟踪转化为对三维直线航迹的跟踪控制，并给出了三维直线航迹跟踪误差方程，通过设计航迹切换规则，实现对分段连续航迹的跟踪，保证控制器能够实现对AUV三维空间航迹点和直线航迹的跟踪控制 2.基于反馈增益的反步法设计的跟踪控制器，通过设计控制器参数，简化了虚拟控制量的形式，避免了采用传统反步法思想设计跟踪控制器时导致虚拟控制量的复杂形式，同时避免了基于视线法设计虚拟控制量时，控制律在= 土 η /2存在奇异值点，因此控制器的设计必须保证前提为(π/2, π/2)，使得AUV的初始艏向误差受到约束，控制器无法实现跟踪误差系统的全局收敛的不足3.设计的控制器具有类似PID控制器的增益形式，线性部分为状态变量和误差变量的线性组合，且参数调节符合PID参数整定规则，非线性部分对模型已知动态进行补偿图I自主水下航行器三维航迹跟踪示意图；图2AUV三维航迹跟踪控制系统结构图；图3AUV三维航迹跟踪控制器解算流程图；图4AUV三维航迹跟踪曲线图；图5AUV水平面航迹跟踪投影图；图6AUV水平面航迹跟踪投影局部放大图；图7AUV垂直面航迹跟踪投影图；图8AUV垂直面航迹跟踪投影局部放大图；图9AUV三维航迹跟踪误差曲线图；图10AUV线速度响应图；图IlAUV纵倾角和艏向角变化曲线图；图12AUV控制输入响应曲线。

具体实施例方式下面举例对本发明作进一步描述步骤I中根据给定的期望航迹点计算规划直线航迹参数X' A、y'd的具体过程为根据设定的相邻期望路径点WPi = (Xi, Yi, Zi)和WPi+1 = (xi+1, yi+1, zi+1)的坐标信息计算此段期望直线航迹的参数X, d、’,具体形式权利要求1.一种具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法，其特征是 步骤I.初始化，通过AUV搭载的测量传感器采集数据，可获得AUV当前位置坐标、方位角和沿载体坐标系三轴的线速度和角速度数据信息，将给定期望航迹点转化为分段连续直线三维航迹的参数方程描述，并初始化“虚拟向导”点的位置坐标； 步骤2.通过AUV三维航迹跟踪误差方程计算AUV与期望航迹上“虚拟向导”点在Serret-Frenet坐标系下的相对位置偏差； 步骤3.基于步骤2中计算得到的跟踪误差，采用基于反馈增益的反步法，计算期望路径上“虚拟向导”点的移动速度、纵倾角和艏摇角的虚拟控制量； 步骤4.在步骤3的基础上，计算AUV纵倾角速度和艏摇角速度的虚拟控制量；· 步骤5.根据给定的AUV六自由度数学模型，推导欠驱动自主水下航行器AUV的三维路径跟踪的动力学控制律包括推进器推力、纵倾控制力矩和转艏控制力矩控制信号，实现欠驱动AUV的三维直线航迹跟踪控制； 步骤6.计算当前AUV位置n11 = (x, y, z)与标定的转向点WPk= (xk, yk, zk)之间的距离d =次X-xk f+(y-yk f+(=~zk f ,若小于设定的航迹切换半径R，则表示完成当前指定航迹的跟踪任务停止航行或切换下一段期望航迹，否则继续步骤2。

2.根据权利要求I所述的具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法，其特征是所述初始化的具体过程为 根据给定的期望航迹点计算规划直线航迹参数V d、太O为控制器设计参数，Xe为{F}坐标系下前向跟踪误差； (2)AUV纵倾角跟踪偏差等价虚拟控制量a e = C2Ze 其中C2 >O为控制器设计参数，Ze为{F}坐标系下垂向跟踪误差； (3)AUV艏摇角跟踪偏差等价虚拟控制量α V = -ClYe 其中C1 >O为控制器设计参数，Ye为{F}坐标系下横向跟踪误差5.根据权利要求I所述的具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法，其特征是按下式计算AUV纵倾角速度α,和艏摇角速度虚拟控制量 (DAUV艏摇角速度虚拟控制量 a r = -C3Z1 其中C3 >O为控制器设计参数其中Z1 = Ψ6-α ψ ； (2)AUV纵倾角速度虚拟控制量 a q = -C4Z2 其中C4 >O为控制器设计参数，Z2 = Θ e- α θ 06.根据权利要求I所述的具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法，其特征是按如下形式解算AUV执行机构控制指令信号 (I)AUV推进器推力控制输入信号全文摘要本发明提供的是一种具有PID反馈增益的自主水下航行器三维直线路径跟踪控制方法。

步骤1.初始化；步骤2.通过AUV三维航迹跟踪误差方程计算AUV与期望航迹上“虚拟向导”点在Serret-Frenet坐标系下的相对位置偏差；步骤3.采用基于反馈增益的反步法，计算期望路径上“虚拟向导。