CN112665574A - 基于动量梯度下降法的水下机器人姿态采集方法 - Google Patents
基于动量梯度下降法的水下机器人姿态采集方法 Download PDFInfo
- Publication number
- CN112665574A CN112665574A CN202011351037.0A CN202011351037A CN112665574A CN 112665574 A CN112665574 A CN 112665574A CN 202011351037 A CN202011351037 A CN 202011351037A CN 112665574 A CN112665574 A CN 112665574A
- Authority
- CN
- China
- Prior art keywords
- attitude
- underwater robot
- data
- quaternion
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000011478 gradient descent method Methods 0.000 title claims abstract description 17
- 239000013598 vector Substances 0.000 claims abstract description 25
- 230000001133 acceleration Effects 0.000 claims abstract description 22
- 238000012545 processing Methods 0.000 claims abstract description 15
- 230000005484 gravity Effects 0.000 claims abstract description 11
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 238000010606 normalization Methods 0.000 claims abstract description 4
- OAICVXFJPJFONN-UHFFFAOYSA-N Phosphorus Chemical compound [P] OAICVXFJPJFONN-UHFFFAOYSA-N 0.000 claims description 3
- 239000004745 nonwoven fabric Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 4
- 238000005259 measurement Methods 0.000 abstract 1
- 238000002474 experimental method Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000006866 deterioration Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
- Gyroscopes (AREA)
Abstract
本发明公开了一种基于动量梯度下降法的水下机器人姿态采集方法，具体步骤如下：对系统采集的加速度计和磁力计数据采用中位值平均滤波算法进行滤波；对参考坐标系下的重力加速度值与上一时刻最优姿态四元数做叉乘处理后与加速度计的实际测量值做误差处理得到误差向量，磁力数据做相似处理，得到另一个误差向量；对误差向量构造目标函数，利用梯度下降法求解目标函数的梯度，并进行归一化处理；将归一化后的梯度值与收敛步长β相结合，并将此结果来修正由陀螺仪数据求解姿态四元数过程产生的累积误差；通过一阶龙格库塔法更新出当前时刻的最优姿态值。本发明所得数据精度高，实时性好且过程计算量小，在水下机器人姿态控制中具有很好的应用前景。
Description
技术领域
本发明涉及一种水下机器人姿态采集方法，尤其涉及一种基于动量梯度下降法的水下机器人姿态采集方法。
背景技术
随着世界人口飞增、土地资源紧缺、生态环境恶化，践行海洋战略是一条急需而有效的道路。在对海洋情况进行探查的过程中，水下机器人(RemotelyOperatedVehicle—ROV)正在成为目前各国进行海洋资源探索与开发的有效工具。由于水下工作条件恶劣、周围环境复杂多变，水下机器人由于受到机械手以及水流等干扰因素的影响，且机械手的姿态和持重、载荷等情况、水流情况很难定量得出，所以姿态解算的准确性和快速性直接决定着水下机器人执行任务过程中的稳定性和可靠性。
由于机械陀螺仪及光纤陀螺仪等高精度惯性导航传感器价格昂贵、体积较大，因此在应用中大多利用微机电系统(MEMS)技术制造的加速度计、陀螺仪和磁力计等传感器进行姿态检测。九轴原始数据由三轴陀螺仪、三轴加速度计和三轴磁力计提供，陀螺仪用来检测ROV运动时的角速度，对其进行积分处理再经过坐标转换后可得到参考坐标系下的姿态角度，短时间内数据精确度高，但随着积分的存在，误差也随时间的增加而增大；加速度计检测ROV运动的加速度信息，其静态性能良好，长时间工作不存在漂移现象，但是由于电机运动产生的噪声会使数据出现异常；磁力计检测ROV周围的磁场信息，精确度高，但对周围磁场强度敏感，易受干扰。
发明内容
发明目的：本发明的目的旨在提供一种对水下机器人姿态数据实时、准确采集且计算量小的采集方法。
技术方案：本发明所述一种基于动量梯度下降法的水下机器人姿态采集方法，包括以下步骤：
(1)利用九轴传感器采集姿态原始数据并输出到单片机；
(2)采用中位值平均滤波法对三轴加速度原始数据和三轴磁力计原始数据进行滤波处理，再将陀螺仪数据、加速度数据和磁力数据分别用四元数法表示；
(3)对参考坐标系下的重力加速度值和地磁值分别与上一时刻最优姿态四元数做叉乘处理，再将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到重力加速度误差向量，将得到的载体坐标系下的地磁估计值与磁力计的实际测量值再做误差处理得到地磁误差向量；
(4)对重力加速度和地磁的误差向量构造目标函数，利用梯度下降法求解目标函数的梯度并进行归一化处理；
(5)将归一化后的梯度值与收敛步长β相结合，并将此结果用修正由陀螺仪数据求解姿态四元数过程产生的累积误差；
(6)通过一阶龙格库塔法更新出当前时刻的最优姿态值。
所述步骤2中，四元数法表示的数据分别为：
其中ω为陀螺仪数据，gb为加速度数据，mb为磁力数据。
所述步骤3中，重力加速度和地磁的误差向量为分别为：
进一步的，所述步骤4中，所述误差向量的目标函数为f(Qt-1,gb,mb)＝[fg fm]T，J＝[Jg Jm]T，误差目标函数的梯度为
其中：
所述步骤5中，修正后的四元数更新方程为：
进一步的，所述四元数更新方程引入的收敛步长β和动量νi分别为：
β＝αTω+||f||2，
其中：
α为系数；||f||2为目标函数的二范数的平方，κ为衰减系数，进一步的，当κ优选为0.9时，改进后的四元数更新方程为：Qt＝Qt-1-vi。
所述步骤6中，通过一阶龙格库塔法更新出当前时刻的最优姿态值为：
有益效果：与现有技术相比，本发明具有以下显著优点：采用中位值平均滤波算法对加速度计和磁力计数据进行滤波去噪，数据更加平滑准确；在保证了解算出来的姿态数据精度高的同时，计算量小且实时性好；动量梯度下降算法避免了磁力计补偿需要知道当地磁场角度的局限。
附图说明
图1为本发明所述基于动量梯度下降法的水下机器人姿态采集方法流程图；
图2为本发明实验过程中采集到的姿态数据图表；
图3为实验过程中采集到的翻滚角数据图表；
图4为实验过程中采集到的俯仰角数据图表；
图5为实验过程中采集到的偏航角数据图表。
具体实施方式
下面结合附图和实施例对本发明的技术方案作进一步的说明。
发明人在对水下机器人姿态采集的研究中发现：通过多传感器数据融合后解算的信息，能够有效利用不同MEMS器件之间的互补特性，稳定地、精确地反应参考坐标系下的ROV姿态角，其结果胜于单个MEMS器件的处理效果。由此发明人进行了进一步的基于动量梯度下降法的水下机器人姿态采集方法研究。
如图1所示，本发明所述的基于动量梯度下降法的水下机器人姿态采集方法，流程如下：
(1)在本实施例中，单片机通过IIC通信获取MPU9250输出的九轴传感器姿态原始数据，采用中位值平均滤波法对三轴加速度原始数据和三轴磁力计原始数据进行滤波处理，并用四元数法表示得：
对参考坐标系下的重力加速度值和地磁值分别与上一时刻最优姿态四元数做叉乘处理，再将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到重力加速度误差向量，将得到的载体坐标系下的地磁估计值与磁力计的实际测量值再做误差处理得到地磁误差向量。
在参考坐标系下加速度计输出值用四元数表示恒为gn＝(0,0,0,g)，将gn旋转至载体坐标系得到预测值g′b为：
同理推导磁力计的预测值，将载体坐标系下测得磁场矢量mb旋转到参考系下的矢量mn为：
而参考系下的磁场方向与横轴垂直，如果上述旋转过程不存在误差，则mny＝0，但实际过程由于存在偏航误差，在绕z轴旋转一定角度，预测的地磁向量bn为：
将参考系下的预测地磁向量旋转至载体坐标系，其表达式为：
(2)对误差向量构造目标函数，利用梯度下降法求解目标函数的梯度，并进行归一化处理；梯度下降法的核心是构建目标函数，本文的姿态四元数最优估计方法通过对加速度计和磁力计的误差矢量
构造误差目标函数，重力加速度误差向量为：
同理，地磁误差向量为：
(3)将归一化后的梯度值与收敛步长β相结合，并将此结果来修正由陀螺仪数据求解姿态四元数过程产生的累积误差：
目标函数的雅可比矩阵Jg、Jm通过
计算获得，表示为：
当目标函数f(Qt-1,gb,mb)＝[fg fm]T，J＝[Jg Jm]T时，误差目标函数的梯度为：
在误差目标函数下降最快的方向时，计算其梯度值并作归一化处理，并将此梯度修正经陀螺仪解算的单位姿态四元数向量。采用梯度下降法，沿着负梯度方向不断迭代修正四元数向量，修正后的四元数更新方程为：
其中，β为收敛步长，β值的设定需依据水下机器人角速度ω和系统采样的实际周期T。β过小会导致四元数更新过程收敛速度慢，而β过大会引起系统震荡，静态效果差。传统的梯度下降法采用固定步长解算姿态角，当水下机器人运动速度变化较快时，容易导致解算的姿态角收敛缓慢、精度低，甚至发生错误。所以对传统梯度下降法进行改进，引入收敛步长和动量：
其中：
α为系数；||f||2为目标函数的二范数的平方，κ为衰减系数，本算法中取值为0.9。
(4)通过一阶龙格库塔法更新出当前时刻的最优姿态值。
在实验验证过程中中，采集得到姿态数据如图2所示，将翻滚角、俯仰角和偏航角的数据图表分别列出，如图3-5所示。
根据图2较大范围内数据图表可知，本发明所述水下机器人姿态采集方法获取到的姿态数据，大范围内数据受到噪声干扰造成的误差更少，更加平滑。
根据图3-5，得到翻滚角、俯仰角和偏航角的数据情况，明显可知，虽偶有偏差，但本发明所述姿态采集方法所采集到的姿态数据整体的误差小，准确度高且实时性好。
Claims (7)
1.一种基于动量梯度下降法的水下机器人姿态采集方法，其特征在于，包括以下步骤：
(1)利用九轴传感器采集姿态原始数据并输出到单片机；
(2)采用中位值平均滤波法对三轴加速度原始数据和三轴磁力计原始数据进行滤波处理，再将陀螺仪数据、加速度数据和磁力数据分别用四元数法表示；
(3)对参考坐标系下的重力加速度值和地磁值分别与上一时刻最优姿态四元数做叉乘处理，再将得到的载体坐标系下的重力加速度估计值与加速度计的实际测量值再做误差处理得到重力加速度误差向量，将得到的载体坐标系下的地磁估计值与磁力计的实际测量值再做误差处理得到地磁误差向量；
(4)对重力加速度和地磁的误差向量构造目标函数，利用梯度下降法求解目标函数的梯度并进行归一化处理；
(5)将归一化后的梯度值与收敛步长β相结合，并将此结果用修正由陀螺仪数据求解姿态四元数过程产生的累积误差；
(6)通过一阶龙格库塔法更新出当前时刻的最优姿态值。
2.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤2中，四元数法表示的数据分别为：
3.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤3中，重力加速度和地磁的误差向量为分别为：
4.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤4中，所述误差向量的目标函数为f(Qt-1,gb,mb)＝[fg fm]T，J＝[Jg Jm]T，误差目标函数的梯度为
其中：
5.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤5中，修正后的四元数更新方程为：
6.根据权利要求5所述的水下机器人姿态采集方法，其特征在于，所述四元数更新方程引入的收敛步长β和动量νi分别为：β＝αTω+||f||2，
其中：
α为系数；||f||2为目标函数的二范数的平方，κ为衰减系数。
7.根据权利要求1所述的水下机器人姿态采集方法，其特征在于，所述步骤6中，通过一阶龙格库塔法更新出当前时刻的最优姿态值为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011351037.0A CN112665574B (zh) | 2020-11-26 | 2020-11-26 | 基于动量梯度下降法的水下机器人姿态采集方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011351037.0A CN112665574B (zh) | 2020-11-26 | 2020-11-26 | 基于动量梯度下降法的水下机器人姿态采集方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112665574A true CN112665574A (zh) | 2021-04-16 |
| CN112665574B CN112665574B (zh) | 2024-02-27 |
Family
ID=75403839
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011351037.0A Active CN112665574B (zh) | 2020-11-26 | 2020-11-26 | 基于动量梯度下降法的水下机器人姿态采集方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112665574B (zh) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113672137A (zh) * | 2021-08-17 | 2021-11-19 | 北京字节跳动网络技术有限公司 | 光标位置更新方法、装置和电子设备 |
| CN114061571A (zh) * | 2021-11-12 | 2022-02-18 | 同济大学 | 一种自适应梯度下降惯性测量单元的姿态解算方法及系统 |
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN116007597A (zh) * | 2022-12-19 | 2023-04-25 | 北京工业大学 | 基于动量梯度下降法对框架柱的垂直度测量方法及装置 |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107664498A (zh) * | 2017-08-25 | 2018-02-06 | 广州新维感信息技术有限公司 | 一种姿态融合解算方法及系统 |
| CN109682377A (zh) * | 2019-03-08 | 2019-04-26 | 兰州交通大学 | 一种基于动态步长梯度下降的姿态估计方法 |
-
2020
- 2020-11-26 CN CN202011351037.0A patent/CN112665574B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107664498A (zh) * | 2017-08-25 | 2018-02-06 | 广州新维感信息技术有限公司 | 一种姿态融合解算方法及系统 |
| CN109682377A (zh) * | 2019-03-08 | 2019-04-26 | 兰州交通大学 | 一种基于动态步长梯度下降的姿态估计方法 |
Non-Patent Citations (1)
| Title |
|---|
| 张帅华;郑芳;李霞;王丙元: "基于变步长动量梯度下降法的姿态解算算法", 电光与控制, no. 009 * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113672137A (zh) * | 2021-08-17 | 2021-11-19 | 北京字节跳动网络技术有限公司 | 光标位置更新方法、装置和电子设备 |
| CN114061571A (zh) * | 2021-11-12 | 2022-02-18 | 同济大学 | 一种自适应梯度下降惯性测量单元的姿态解算方法及系统 |
| CN114061571B (zh) * | 2021-11-12 | 2023-08-04 | 同济大学 | 一种自适应梯度下降惯性测量单元的姿态解算方法及系统 |
| CN114252073A (zh) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN114252073B (zh) * | 2021-11-25 | 2023-09-15 | 江苏集萃智能制造技术研究所有限公司 | 一种机器人姿态数据融合方法 |
| CN116007597A (zh) * | 2022-12-19 | 2023-04-25 | 北京工业大学 | 基于动量梯度下降法对框架柱的垂直度测量方法及装置 |
| CN116007597B (zh) * | 2022-12-19 | 2024-06-11 | 北京工业大学 | 基于动量梯度下降法对框架柱的垂直度测量方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112665574B (zh) | 2024-02-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112665574B (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
| WO2020253854A1 (zh) | 移动机器人姿态角解算方法 | |
| Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
| CN108225308B (zh) | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 | |
| KR101988786B1 (ko) | 관성 항법 장치의 초기 정렬 방법 | |
| WO2020220729A1 (zh) | 基于角加速度计/陀螺/加速度计的惯性导航解算方法 | |
| CN109682377B (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
| CN112630813B (zh) | 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法 | |
| CN110017837B (zh) | 一种姿态抗磁干扰的组合导航方法 | |
| CN108225370B (zh) | 一种运动姿态传感器的数据融合与解算方法 | |
| CN106885566B (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
| CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
| CN109724602A (zh) | 一种基于硬件fpu的姿态解算系统及其解算方法 | |
| CN109764870B (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
| Liu et al. | A complementary filter based on multi-sample rotation vector for attitude estimation | |
| CN110954102A (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
| CN111121820B (zh) | 基于卡尔曼滤波的mems惯性传感器阵列融合方法 | |
| CN104296745A (zh) | 一种基于9-dof传感器组的姿态检测数据融合方法 | |
| CN114526731A (zh) | 一种基于助力车的惯性组合导航方向定位方法 | |
| CN106197376B (zh) | 基于单轴mems惯性传感器的车身倾角测量方法 | |
| CN110207647B (zh) | 一种基于互补卡尔曼滤波器的臂环姿态角计算方法 | |
| CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| CN109506674B (zh) | 一种加速度的校正方法及装置 | |
| CN110375773B (zh) | Mems惯导系统姿态初始化方法 | |
| Zhe et al. | Adaptive complementary filtering algorithm for imu based on mems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |