[0003] 本发明所要解决的技术问题是克服现有技术的缺陷，提供判别水下机器人姿态稳定的方法，能够更好的判定水下机器人在水中的姿态，使得水下机器人在遇到复杂水域情况时，更加能够保持姿态不会倾覆，能够固定维持在某一地点，完成较长时间的探测。
[0004] 为解决上述技术问题，一种判别水下机器人姿态稳定的方法，其特征是，包括如下步骤：
[0005] 步骤一：建立水下机器人的载体坐标系，并与地面坐标系进行坐标变换；
[0006] 步骤二：通过传感器采集船体信息，并根据步骤一得到水下机器人的地面坐标系六自由度参数和系统状态；
[0007] 步骤三：通过步骤一和步骤二将系统状态方程化处理，方便在MATLAB和Simulink环境下模拟；
[0008] 步骤四：使用MATLAB对此模型系统进行稳定性分析，判断系统安全运行的工作范围。
[0009] 进一步地，所述运动稳定性分析：Lyapunov指数可以通过动力学方程获得，其计算公式为：
[0010]
[0011] 其中λ表示Lyapunov指数，x表示函数f(x)的自变量，n表示迭代次数；
[0012] 进一步地，所述系统状态包括12个系统状态分别为：载体坐标系相对地面坐标系的x，y，z位置， θ，ψ表示x，y，z轴上的俯仰偏航角，定义u，v，w在x，y，z轴上的矢量分量，定义p，q，r为分别绕x，y，z轴旋转的角速度。
[0013] 进一步地，所述12个系统状态方程化如下：
[0014] 在分析6自由度刚体的运动时需定义两个主坐标系，选择载体坐标系的原点ob作为物体对称的在主体对称的主平面上，通常被选择在以主体对称的主平面内，体轴Xb、Yb、Zb通常选择符合物体的惯性主轴，通常定义为：
[0015] Xb：纵轴，Yb：横轴，Zb：垂直轴
[0016] 载体坐标系与地面坐标系相关，相对于另一个坐标系的方向可以通过旋转矩阵获得，坐标系x,y,z轴的单位向量用ia，ja，ka表示，得式：
[0017]
[0018] 容易证明载体坐标系变化后的定位与地面坐标系相关，通过载体坐标系关于地面坐标系简单旋转后实现，定义矩阵S(v')使得v'×w＝S(v')w，即：
[0019]
[0020] 获得：
[0021]
[0022] I是3×3的单位矩阵，α是载体坐标系的旋转角度，v'是载体坐标系旋转的单位向量，旋转矩阵 可以看作三个旋转矩阵的乘积：
[0023]
[0024] 其中俯仰角θ；偏航角ψ；横摇角
[0025] 一般用下面的向量来描述水下机器人运动的六个自由度：
[0026]
[0027] η表示广义的位置矢量，x，y，z表示位置， θ，ψ表示俯仰偏航角。
[0028] 现在将η作为载体坐标系相对于地面坐标系的位置，定义u，v，w在x，y，z轴上的矢量分量，定义p，q，r为分别绕x，y，z轴旋转的角速度，
[0029]
[0030] V是载体相对于地面的速度，v是载体坐标系相对地面坐标系的广义速度矢量，η的导数与v和矩阵J(η)相关：
[0031]
[0032] O3是一个3×3零矩阵，J1(η2)表示
[0033] τ是作用在物体上的力，在一个三维空间中的刚体的运动可以表示为：
[0034] MRB随质心的运动，CRB绕质心的运动，
[0035]
[0036] 式中：
[0037]
[0038] I3是3×3单位矩阵，m是物体的质量，bG是质量中心的位置， 是惯性张量，假设ρ(p)是物体在P点质量的密度，bP是p物体，Vol是它的容量，定义其它三个量为：
[0039]
[0040]
[0041]
[0042] 矩阵CRB逆矩阵MRB12没有唯一的参数化，同时它经常被表示为一个斜对称矩阵(CRB(v)+CRBT(v)＝0),如下所示：
[0043]
[0044] 将力矩向量τ1看作不同部分的总和：
[0045] τ1＝τREST+τDAMP+τADD+τFK+τWAVE+τWIND+τEXT
[0046] τREST表示恢复力矩，由于重力和浮力，它是物体的位置和方向的函数通常由-g(η)表示：
[0047]
[0048] bg是重力加速度，ρm是海水密度，bB是浮力中心
[0049] τDAMP表示因不同类型阻尼而产生的力和力矩，这个向量通常取决于vr＝v-vC，vC是水流速度，δ表示偏转表面的位置，D表示矩阵，它表示为：
[0050] τDAMP＝-D(vr δ)r
[0051] τADD为附加质量力和力矩，由于周围流体的惯性，这些力量取决于相对于流体的物体的加速度，表示为：
[0052]
[0053] MA和CA(Vr)类似于MRB和CRB(v)
[0054] τWAVE表示由于风产生的波浪力，根据物体的速度和姿态，风速因素的影响，很明显，τWAVE一对于深海操作水下机器人为零，τWIND风的力对水下机器人也为零，最后，τEXT代表一般的外力，τFK推进器牵引力，将τFK和MAdvr/dt移动到式左边，CRB(v)移动到右边，定义τCOR为科里奥利力：
[0055] τCOR＝-CRB(v)-CA(Vr)Vr
[0056] 式(1)可以写为：
[0057]
[0058] 因此，运动学和动力学的最终方程：
[0059]
[0060]
[0061] 其中：
[0062]
[0063] 进一步地，所述步骤三中，使用MATLAB进行仿真，输入一个24维向量，包含：
[0064] (1)12个系统状态；
[0065] (2)外力和力矩相对于物体，6个元素，重力，浮力，随周围流体运动的力，波浪力，风的力，推进器的力；
[0066] (3)相对于地面坐标系的外部力和扭矩，4个元素，绕周围流体运动的力，水下机器人产生的波浪引起的阻尼，波浪漂移阻尼电缆的牵引力；
[0067] (4)相对于地面坐标系的海流的速度和加速度。
[0068] 进一步地，当所述Lyapunov指数小于0时，系统的相轨道吸引到一个稳定的固定点上，整个系统是稳定的；负的Lyapunov指数是耗散系统或者非保守系统的基本特征，而且这个负值越大相轨道收敛的越快，系统到达稳定状态的速度越快，当这个负值趋向于无穷大时系统是超稳定的；如果系统是稳定的，其Lyapunov指数中至少有一个小于0，并且所有指数之和同时小于0；当Lyapunov指数大于0时，系统是不稳定的或混沌的；Lyapunov指数为0时，相轨迹是周期性运动。
[0069] 本发明所达到的有益效果：
[0070] 1)建立水下机器人运动系坐标，有助于分析水下机器人水下运动情况；2)建立水下机器人六自由度模型，帮助分析水下机器人在水中六个自由度的情况；3)利用李雅普诺夫指数分析机器人是否处于稳定情况，帮助改进系统的稳定性；4)通过模拟，可以调试出遇到什么样的情况，系统会处于不稳定的状态，帮助设定水下机器人安全运行的工作范围。
