买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工程大学

摘要：本发明公开了一种单领航多AUV协同定位及轨迹跟踪控制方法，步骤一：建立跟踪器运动学模型、领航器运动学模型以及两者之间距离量测的模型；步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计；步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹；步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。本发明所提出的方法具有很强的鲁棒性和有效性。对不同的多个跟踪器均有很好的效果。

主权项：1.一种单领航多AUV协同定位及轨迹跟踪控制方法，其特征在于，包括：步骤一：建立跟踪器运动学模型、领航器运动学模型以及两者之间距离量测的模型；建立跟踪器运动学模型：假设有N个跟踪器N≥1，{Ι}＝{xI,yI,zI}为惯性坐标系，为第i个跟踪器的自身坐标系，i∈N；代表惯性坐标系中第i个跟踪器的坐标；表示第i个跟踪器在自身坐标系中的速度向量，则跟踪器的运动学模型是： 其中，Rη[i]是从自身坐标系到惯性系的转换矩阵，其中向量是由滚转角、俯仰角、偏航角组成的向量，转换矩阵的导数满足：其中ω[i]＝[p[i],q[i],r[i]]T为自身固定的角速度向量，p[i],q[i],r[i]分别为x、y、z方向对应的角速度，S是一个矩阵，当跟踪器运动为三维运动时，定义为： 当跟踪器运动为二维运动时： 建立领航器运动学模型：将领航器看作被跟踪的目标，则代表目标的轨迹，代表惯性系下目标的速度向量，假设目标在缓慢的改变速度，其运动模型采用准稳态模型的形式描述： 其中代表目标的状态，其中υt是缓慢变化，w～N0,Qt为零均值高斯噪声，协方差为Qt， 代表着k时刻的目标状态，进行离散化得：xk+1＝Fxk+wk其中，当跟踪器运动为三维运动时，当跟踪器运动为二维运动时Ts是采样时间，w～N0,Q，建立距离量测模型： 表示跟踪器和目标之间在k时刻的真实距离其中为跟踪器i在k时刻所处位置，跟踪器与目标之间的量测模型为： 其中，i∈N为高斯量测噪声；步骤二：根据步骤一建立的模型通过扩展卡尔曼滤波算法对领航器状态进行估计；步骤三：根据可观测性进行协同轨迹规划，确定跟踪器理想跟踪轨迹；步骤四：根据步骤二以及步骤三中的目标状态以及跟踪轨迹，设计控制器，完成对目标的跟踪。

全文数据：

权利要求：

百度查询： 哈尔滨工程大学 一种单领航多AUV协同定位及轨迹跟踪控制方法