惯性导航 | 航迹推算与gazebo仿真

惯性导航 | 航迹推算与gazebo仿真

  • IMU数据进行短时间航迹推算
    • 代码
    • gazebo中进行仿真测试

IMU数据进行短时间航迹推算

代码

声明一个用与 IMU积分的类 ，来实现 短时间内的航迹推算

类的名字叫 IMUIntegration
构造函数 有三个变量进行私有变量初始化 重力、初始陀螺仪零偏、初始加速度零偏。

class IMUIntegration {public:IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba): gravity_(gravity), bg_(init_bg), ba_(init_ba) {}

下面是在imu数据一帧一帧进来，如何实现状态的积分，在使用的时候则是来一帧IMU数据，调用一次这个函数
函数名称

void AddIMU(const IMU& imu) {

计算时间间隔，与上一帧IMU数据时间间隔

double dt = imu.timestamp_ - timestamp_;

时间间隔满足要求，间隔时间不能过长

if (dt > 0 && dt < 0.1) {

更新位置，用上一帧的速度与姿态

p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;

对应公式：
p j = p k + ∑ k = i j − 1 [ v k △ t + 1 2 g △ t 2 ] + 1 2 ∑ k = i j − 1 R k ( a ~ − b a , k ) △ t 2 p_{j}=p_{k} +\sum_{k=i}^{j-1} [v_{k}\triangle t+\frac{1}{2} g\triangle t^{2} ] +\frac{1}{2}\sum_{k=i}^{j-1}R_{k}(\tilde{a}-b_{a,k})\triangle t^{2} pj​=pk​+k=i∑j−1​[vk​△t+21​g△t2]+21​k=i∑j−1​Rk​(a~−ba,k​)△t2

更新速度，用上一帧的姿态

v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;

对应公式：
v j = v k + ∑ k = i j − 1 [ R k ( a ~ − b a , k ) △ t + g △ t ] v_{j}=v_{k}+\sum_{k=i}^{j-1}[R_{k}(\tilde{a}-b_{a,k})\triangle t+g\triangle t] vj​=vk​+k=i∑j−1​[Rk​(a~−ba,k​)△t+g△t]

更新姿态（旋转矩阵）

R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);

R j = R i ∏ k = i j − 1 exp ⁡ ( ( ω ~ − b g , k ) △ t ) R_{j}=R_{i}\prod_{k=i}^{j-1} \exp((\tilde{\omega } -b_{g,k})\triangle t) Rj​=Ri​k=i∏j−1​exp((ω~−bg,k​)△t)

更新时间，用于判断下一帧数据，在时间间隔上是否满足要求

timestamp_ = imu.timestamp_;

返回有IMU积分的各个航迹状态

    /// 组成NavStateNavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }SO3 GetR() const { return R_; }Vec3d GetV() const { return v_; }Vec3d GetP() const { return p_; }

下面是私有变量的声明

   private:// 累计量SO3 R_;Vec3d v_ = Vec3d::Zero();Vec3d p_ = Vec3d::Zero();double timestamp_ = 0.0;// 零偏，由外部设定Vec3d bg_ = Vec3d::Zero();Vec3d ba_ = Vec3d::Zero();Vec3d gravity_ = Vec3d(0, 0, -9.8);  // 重力

gazebo中进行仿真测试

在gazebo中的IMU插件对惯导的噪声描述很简单，仅有一个高斯噪声可以设置。
其中xacro设置的一个例子如下：

     <gazebo reference="imu_base_link"><gravity>true</gravity><sensor name="imu_sensor" type="imu"><always_on>true</always_on><update_rate>200</update_rate><visualize>true</visualize><topic>/jk/imu</topic><plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin"><topicName>/jk/imu</topicName>         <bodyName>imu_base_link</bodyName><updateRateHZ>200.0</updateRateHZ>    <gaussianNoise>0.00329</gaussianNoise>   <xyzOffset>0 0 0</xyzOffset>     <rpyOffset>0 0 0</rpyOffset><frameName>imu_base_link</frameName>        </plugin><pose>0 0 0 0 0 0</pose></sensor></gazebo>

其中gaussianNoise噪声参数 可以进行手动设置。

与真实的IMU 噪声模型差别很大，用rotors的imu插件，可以尽可能模拟IMU噪声模型

    <gazebo><plugin filename="librotors_gazebo_imu_plugin.so" name="rotors_gazebo_imu${imu_suffix}_plugin"><robotNamespace>${namespace}</robotNamespace> <!-- (string, required): ros namespace in which the messages are published --><linkName>${namespace}/imu${imu_suffix}_link</linkName> <!-- (string, required): name of the body which holds the IMU sensor --><imuTopic>${imu_topic}</imuTopic> <!-- (string): name of the sensor output topic and prefix of service names (defaults to imu) --><gyroscopeNoiseDensity>${gyroscope_noise_density}</gyroscopeNoiseDensity> <!-- Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)] --><gyroscopeRandomWalk>${gyroscope_random_walk}</gyroscopeRandomWalk> <!-- Gyroscope bias random walk [rad/s/s/sqrt(Hz)] --><gyroscopeBiasCorrelationTime>${gyroscope_bias_correlation_time}</gyroscopeBiasCorrelationTime> <!-- Gyroscope bias correlation time constant [s] --><gyroscopeTurnOnBiasSigma>${gyroscope_turn_on_bias_sigma}</gyroscopeTurnOnBiasSigma> <!-- Gyroscope turn on bias standard deviation [rad/s] --><accelerometerNoiseDensity>${accelerometer_noise_density}</accelerometerNoiseDensity> <!-- Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)] --><accelerometerRandomWalk>${accelerometer_random_walk}</accelerometerRandomWalk> <!-- Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)] --><accelerometerBiasCorrelationTime>${accelerometer_bias_correlation_time}</accelerometerBiasCorrelationTime> <!-- Accelerometer bias correlation time constant [s] --><accelerometerTurnOnBiasSigma>${accelerometer_turn_on_bias_sigma}</accelerometerTurnOnBiasSigma> <!-- Accelerometer turn on bias standard deviation [m/s^2] --></plugin></gazebo>

其中关键参数包括：

  • accelerometer/gyroscopeNoiseDensity 测量噪声
  • accelerometer/gyroscope_random_walk 随机游走

其中发布出来的一包数据如下：

header:
seq: 128
stamp:
secs: 22
nsecs: 290000000
frame_id: “imu_base_link”
orientation:
x: 0.0016563453004138674
y: -0.0008542007888005784
z: 0.006514394955332766
w: 1.001055072733733
orientation_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]
angular_velocity:
x: 0.0014246123123964744
y: 0.0027524592879204523
z: 0.0007163285834896606
angular_velocity_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]
linear_acceleration:
x: -0.0029448312010755674
y: -0.00014593761997295991
z: 9.795575703789861
linear_acceleration_covariance: [1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05, 0.0, 0.0, 0.0, 1.08241e-05]

自定义的IMU数据结构体如下：

struct IMU {IMU() = default;IMU(double t, const Vec3d& gyro, const Vec3d& acce) : timestamp_(t), gyro_(gyro), acce_(acce) {}double timestamp_ = 0.0;Vec3d gyro_ = Vec3d::Zero();Vec3d acce_ = Vec3d::Zero();
};

所以需要在IMU的回调函数里面，赋值上面的结构体数据

        sad::IMU imu;imu.timestamp_ = Imu_msg->header.stamp.toSec();imu.gyro_ << Imu_msg->angular_velocity.x,Imu_msg->angular_velocity.y,Imu_msg->angular_velocity.z;imu.acce_ << Imu_msg->linear_acceleration.x,Imu_msg->linear_acceleration.y,Imu_msg->linear_acceleration.z;

然后将数据加入，在该函数内部，即完成航迹的推算

AddIMU(imu);

发布一个里程计数据，来查看航迹推算的结果

        nav_msgs::Odometry imu_Integration_Odom;//声明一个里程计 来记录推算的结果imu_Integration_Odom.header = Imu_msg->header;imu_Integration_Odom.pose.pose.position.x = p_[0];imu_Integration_Odom.pose.pose.position.y = p_[1];imu_Integration_Odom.pose.pose.position.z = p_[2];imu_Integration_Odom.twist.twist.linear.x = v_[0];imu_Integration_Odom.twist.twist.linear.y = v_[1];imu_Integration_Odom.twist.twist.linear.z = v_[2];

将旋转矩阵转成欧拉角单位为度，方便直观查看

        float yaw = atan2(R_.matrix()(1,0), R_.matrix()(0,0));  float roll = atan2(R_.matrix()(2,1), R_.matrix()(2,2));float pitch = atan2(-R_.matrix()(2,0), sqrt(R_.matrix()(0,0) * R_.matrix()(0,0) + R_.matrix()(1,0) * R_.matrix()(1,0)));imu_Integration_Odom.pose.pose.orientation.x = pitch/0.01745329252;imu_Integration_Odom.pose.pose.orientation.y = roll/0.01745329252;imu_Integration_Odom.pose.pose.orientation.z = yaw/0.01745329252;

发布里程计

imu_Integration_Odom_pub_.publish(imu_Integration_Odom);

无人机在地面静止不动情况下
曲线结果数据如下：

  • 位置（很快就飘掉了）
    在这里插入图片描述
  • 速度（与位置类似，越来越大）
    在这里插入图片描述
  • 欧拉角度 （航向角度保持还可以）
    在这里插入图片描述

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.rhkb.cn/news/270279.html

如若内容造成侵权/违法违规/事实不符，请联系长河编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

制作一个简单的HTML个人网页

制作一个简单的HTML个人网页

制作一个简单的HTML个人网页 1.1 硬件1.1.1 一台电脑1.1.2 配置要求 1.2 系统1.3 软件 二、制作一个简单的HTML个人网页1.创建一个HTML网页1.1 新建文本文档1.2 另存文本文档1.3 命名为index.html 2.编写HTML代码2.1 打开HTML2.2 复制HTML代码2.3 粘贴HTML代码2.4 保存HTML 3.预…
阅读更多...
Map集合体系——遍历，HashMap，TreeMap，LikedHashMap

Map集合体系——遍历，HashMap，TreeMap，LikedHashMap

认识Map集合 Map集合体系特点 方法 代码示例 package com.zz.Map;import java.util.*;public class Test {public static void main(String args[]){Map<String, Integer> map new HashMap <>();//经典代码&#xff0c;按照键 无序 不重复 无索引map.put("…
阅读更多...
阻塞队列、生产者消费者模型、阻塞队列的模拟实现等干货

阻塞队列、生产者消费者模型、阻塞队列的模拟实现等干货

文章目录 &#x1f490;生产者消费者模型&#x1f490;模拟实现阻塞队列&#x1f4a1;注意点一&#x1f4a1;注意点二 阻塞队列是一种“特殊”的数据结构&#xff0c;但是也遵循队列的“先进先出”特性&#xff0c;它的特殊在于&#xff1a; 阻塞队列的两个特性&#xff1a; 1…
阅读更多...
学习基于 JavaScript 语言 的计算机界三大神书”之一 ——SICP

学习基于 JavaScript 语言 的计算机界三大神书”之一 ——SICP

如何阅读“计算机界三大神书”之一 ——SICP 《计算机程序的构造和解释》&#xff08;Structure and Interpretation of Computer Programs&#xff0c;简记为SICP&#xff09;是MIT的基础课教材&#xff0c;出版后引起计算机教育界的广泛关注&#xff0c;对推动全世界大学计算…
阅读更多...
C switch 语句

C switch 语句

一个 switch 语句允许测试一个变量等于多个值时的情况。每个值称为一个 case&#xff0c;且被测试的变量会对每个 switch case 进行检查。 语法 C 语言中 switch 语句的语法&#xff1a; switch(expression){case constant-expression :statement(s);break; /* 可选的 */ca…
阅读更多...
【小白友好】LeetCode 打家劫舍 III

【小白友好】LeetCode 打家劫舍 III

https://leetcode.cn/problems/house-robber-iii/description/ 前言 建议还是先看看动态规划的基础题再看这个。动态规划是不刷题&#xff0c;自己100%想不出来的。 基础题&#xff1a; 最大子数组和乘积最大子数组最长递增子序列 最大升序子数组和 小白想法 现在我们想遍…
阅读更多...
前端语义化标签及实例

前端语义化标签及实例

常用的语义化标签的以下几种&#xff1a; header、nav、article、section、aside、footer、abbr、dfn、address、del、ins、pre、meter、progress <header> 定义文章的页眉信息 <header><h1>我的网站标题</h1><nav><ul><li><a …
阅读更多...
[GYCTF2020]EasyThinking --不会编程的崽

[GYCTF2020]EasyThinking --不会编程的崽

看标题就知道&#xff0c;这大概率是关于thinkphp的题目。先尝试错误目录使其报错查看版本号 thinkphp v6.0.0&#xff0c;在网上搜索一下&#xff0c;这个版本有一个任意文件上传漏洞。参考以下文章。 https://blog.csdn.net/god_zzZ/article/details/104275241 先注册一个账…
阅读更多...
【Android】位置修改相关

【Android】位置修改相关

获取位置服务总开关状态 //获取LOCATION_MODE值&#xff0c;但adb状态下无法获取 //0为关闭&#xff0c;1 gps、2 network、3 高精度等 int state Settings.Secure.getInt(mContext.getContentResolver(),Settings.Secure.LOCATION_MODE,Settings.Secure.LOCATION_MODE_HIGH_…
阅读更多...
three.js可以对3D模型做什么操作和交互，这里告诉你。

three.js可以对3D模型做什么操作和交互，这里告诉你。

Three.js 提供了多种交互功能&#xff0c;可以对 3D 模型进行各种操作和交互。以下是一些常见的交互功能&#xff1a; 鼠标交互 通过鼠标事件&#xff0c;可以实现模型的拖拽、旋转、缩放等操作。例如&#xff0c;可以通过鼠标拖拽来改变模型的位置或角度。 触摸交互 对于支…
阅读更多...
PackagesNotFoundError:学习利用报错信息找到解决方法

PackagesNotFoundError:学习利用报错信息找到解决方法

反思&#xff1a;之前看到报错经常是直接复制报错信息去网上搜&#xff0c;但很多情况下报错信息里其实就给出了解决方案 报错信息&#xff1a; Collecting package metadata (current_repodata.json): done Solving environment: unsuccessful initial attempt using frozen …
阅读更多...
MySQL 篇-深入了解多表设计、多表查询

MySQL 篇-深入了解多表设计、多表查询

&#x1f525;博客主页&#xff1a; 【小扳_-CSDN博客】 ❤感谢大家点赞&#x1f44d;收藏⭐评论✍ 文章目录 1.0 多表设计概述 1.1 多表设计 - 一对多 1.2 多表设计 - 一对一 1.3 多表设计 - 多对多 2.0 多表查询概述 2.1 多表查询 - 内连接 2.2 多表查询 - 外连接 2.3 多表查…
阅读更多...
cetos7 Docker 安装 gitlab

cetos7 Docker 安装 gitlab

一、gitlab 简单介绍和安装要求 官方文档&#xff1a;https://docs.gitlab.cn/jh/install/docker.html 1.1、gitlab 介绍 gitLab 是一个用于代码仓库管理系统的开源项目&#xff0c;使用git作为代码管理工具&#xff0c;并在此基础上搭建起来的Web服务平台&#xff0c;通过该平…
阅读更多...
【C语言】指针初阶2.0版本

【C语言】指针初阶2.0版本

这篇博文我们来继续学习指针的其他内容 指针2.0 传值调用与传址调用传值调用传址调用 一维数组与指针理解数组名使用指针深入理解一维数组 二级指针指针数组二维数组与指针 传值调用与传址调用 在开始之前&#xff0c;我们需要先了解这个概念&#xff0c;后面才能够正常的学习…
阅读更多...
Material UI 5 学习01-按钮组件

Material UI 5 学习01-按钮组件

Material UI 5 学习01-按钮组件 一、安装Material UI二、 组件1、Button组件1、基础按钮2、variant属性3、禁用按钮4、可跳转的按钮5、disableElevation属性6、按钮的点击事件onClick 2、Button按钮的颜色和尺寸1、Button按钮的颜色2、按钮自定义颜色3、Button按钮的尺寸 3、图…
阅读更多...
vulhub中ThinkPHP5 SQL注入漏洞 敏感信息泄露

vulhub中ThinkPHP5 SQL注入漏洞 敏感信息泄露

漏洞原理 传入的某参数在绑定编译指令的时候又没有安全处理&#xff0c;预编译的时候导致SQL异常报错。然而thinkphp5默认开启debug模式&#xff0c;在漏洞环境下构造错误的SQL语法会泄漏数据库账户和密码 启动后&#xff0c;访问http://your-ip/index.php?ids[]1&ids[]2…
阅读更多...
nodejs安装教程（及过程中的易错）

nodejs安装教程（及过程中的易错）

nodejs&#xff1a;Nodejs 是基于 Chrome 的 V8 引擎开发的一个 C 程序&#xff0c;目的是提供一个 JS 的运行环境。 npm&#xff1a;npm 是 Node Package Manager 的缩写&#xff0c;意思是 Node 的包管理系统&#xff0c;是最大的软件包仓库 下载nodejs 首先我们需要在node…
阅读更多...
智慧城市中的数据力量：大数据与AI的应用

智慧城市中的数据力量：大数据与AI的应用

目录 一、引言 二、大数据与AI技术的融合 三、大数据与AI在智慧城市中的应用 1、智慧交通 2、智慧环保 3、智慧公共安全 4、智慧公共服务 四、大数据与AI在智慧城市中的价值 1、提高城市管理的效率和水平 2、优化城市资源的配置和利用 3、提升市民的生活质量和幸福感…
阅读更多...
去除PDF论文行号的完美解决方案

去除PDF论文行号的完美解决方案

去除PDF论文行号的完美解决方案 1. 遇到的问题 我想去除论文的行号&#xff0c;但是使用网上的Adobe Acrobat裁剪保存后 如何去掉pdf的行编号&#xff1f; - 知乎 (zhihu.com) 翻译时依然会出现行号&#xff0c;或者是转成word&#xff0c;这样就大大损失了格式&#xff0c…
阅读更多...
C语言结构体的大小，结构体内存对齐

C语言结构体的大小，结构体内存对齐

1. 结构体的大小 在自己正真了解过之前&#xff0c;一直认为结构体的大小就是结构体内部成员大小的总和。 但当你去尝试打印结构体的大小时&#xff0c;会发现事实并非如此&#xff0c;也不会像你想的那样简单。 #include <stdio.h>struct S1 {char c1;char c2;int i;…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023