没有合适的资源?快使用搜索试试~ 我知道了~
首页px4-position_estimator
资源详情
资源评论
资源推荐
导航位置估计内容
2016.5.26 第 0.8 版本(除了位置估计工具数据更新部分,其他流程都已完成)
2016.5.31 第 1.1 版本(第一部分基本完成,开始第二部分更新,暂停更新,将先处理传感器部分的内容)
2016.6.21 第 1.2 版本(第一部分完成流图形式的分析,附在最后)
关于位置估计的内容更新:
(高级查找,倾斜字体,为个人观点,用于后期认知足够以后修改)
内容概述:
位置估计有两个部分,position_estimator_inav 和 local_position_estimator。其中默认使用前者,两者都使用了外
部位置估计工具,包括 GPS,光流,激光雷达等。前者代码中没有对声纳的支持,如果需要声纳则需自己添加(貌
似被删除了,看到一些注释内容是有关声纳的)。后者有声纳支持,官方说法更合适用于室内,使用卡尔曼滤波。
第一版本基本完成对前者的分析,第二版本完成后者的分析。
实际飞行有 GPS 使用前者很稳定,后者未进行测试。大概观察发现后者使用了卡尔曼,是因为计算量妥协了吗?
目录
导航位置估计内容 ...............................................................................................................................................1
第一部分(position_estimator_inav): .....................................................................................................................2
文件结构: ............................................................................................................................................................2
滤波器: .............................................................................................................................................................2
位置估计参数: ..................................................................................................................................................3
位置估计主函数: ...............................................................................................................................................4
1.相关数据更新............................................................................................................................................8
2.检查相关主题的超时 ...............................................................................................................................16
3.检查位置估计工具的有效性 ....................................................................................................................17
4. 使用位置估计工具更新传感器补偿 .......................................................................................................17
5.对高度和位置进行滤波处理 ....................................................................................................................20
6.地形估计和发布数据 ...............................................................................................................................23
第二部分(local_position_estimator): ...................................................................................................................28
文件结构 ...........................................................................................................................................................28
参数:param.c ..................................................................................................................................................28
估计器: ...........................................................................................................................................................30
第一部分(position_estimator_inav):
文件结构:
首先我们看下文件结构:
包含位置估计主函数,相关参数和惯性滤波器三个源文件和两个头文件。
滤波器:
首先从工具入手,这个 inertial_filter 到底有什么呢,在头文件中声明了两个函数:
void inertial_filter_predict(float dt, float x[2], float acc);
void inertial_filter_correct(float e, float dt, float x[2], int i, float w);
一个是预先滤波器,另一个是校正滤波器。
预先滤波器是这样子的:
if (isfinite(dt)) {
if (!isfinite(acc)) {
acc = 0.0f;
}
x[0] += x[1] * dt + acc * dt * dt / 2.0f;
x[1] += acc * dt;
}
之后在得到位置状态中会使用(XYZ 位置和速度求解)
函数的使用范围是单位时间有限的情况下,距离 x[0]加上单位增量,包括速度 x[1]和加速度 acc 两个部分;而速度
x[1]也加上单位增量,即加速度 acc*dt。当然 acc 非有限的情况则忽略 acc 作用,给 acc 置 0,即无加速度情况。
校正滤波器的函数如下:
if (isfinite(e) && isfinite(w) && isfinite(dt)) {
float ewdt = e * w * dt;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
}
}
这里依旧对 x[0]和 x[1]作用,这里 ewdt 应该是一个误差校正系数,对 x[i]作用,当对距离 x[0]作用的情况下,
x[1]增加增量 w*ewdt。大前提就是 e,w,dt 都是有限的,【补偿中可以将 e 看作是误差,w 是权重,位置增量由 e,w,dt
补偿,速度增量由 e,w
2
,dt 补偿】。
位置估计参数:
接下来我们就进入参数相关函数的分析,两个部分:
在头文件中,定义了两个结构体,分别用于参数和参数句柄,两者一一对应:
position_estimator_inav_params
position_estimator_inav_param_handles
其中包含位置估计中使用的传感器的相关参数,传感器如气压计 baro,GPS,激光雷达 lidar,视觉 vision,光流
flow,动作捕捉 mocap 等。
另外还定义一个常量 CBRK_NO_VISION_KEY 为 328754(用于禁止视觉输入),并声明了两个函数分别用于参数句柄
与数值的初始化和参数的更新:
int inav_parameters_init(
struct position_estimator_inav_param_handles *h);
int inav_parameters_update(
const struct position_estimator_inav_param_handles *h,
struct position_estimator_inav_params *p);
在源文件中,主要就是上面两个函数的定义,另一个部分就是权重参数数值定义:
PARAM_DEFINE_FLOAT(_name, _default),这个将用于参数初始化函数,给每个参数句柄赋值。如下面这个例
子:
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
顶一个INAV_W_Z_BARO为0.5f,用于气压计中高度测量数值的权重(截止频率)。
接下来看下初始化函数(这里就举其中一个例子):
h->w_z_baro = param_find("INAV_W_Z_BARO");
这里h是参数句柄,其中的w_z_baro变量被初始化为INAV_W_Z_BARO对应的值,在权重参数数值定义中定义的那样;
对于未定义的权重参数则赋值0xffffffff,表明未找到。
if (notification) {
param_set_used_internal(param);
}
这里暂未明确作用
对于更新函数,同样举一个例子:
param_get(h->w_z_baro, &(p->w_z_baro));
将参数句柄中的值覆给参数变量(h赋值给p),使用内存拷贝的方式。
位置估计主函数:
代码内首先是头文件包含,接着定义常量用于超时,线程状态等,最后是几个函数:
extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
int position_estimator_inav_thread_main(int argc, char *argv[]);
static void usage(const char *reason);用于输出当前任务
static inline int min(int val1, int val2)用于获得两数最小
{
return (val1 < val2) ? val1 : val2;
}
static inline int max(int val1, int val2)用于获得两数最大
{
return (val1 > val2) ? val1 : val2;
}
除了上面的还有一个
debug
任务,功能是先读取
sd
卡中
log
文件,然后进行打印,这里暂时不详细介绍。
接下来分析两个函数:
position_estimator_inav_main(int argc, char *argv[])
position_estimator_inav_thread_main(int argc, char *argv[])
对于第一个函数:
position_estimator_inav_main(int argc, char *argv[])
这个函数内主要是三个命令操作,总共三个命令:start,stop和status,之外的就用usage()函数报错”missing
command”或者”unrecognized command”。
那么先分析后面两者stop和status,
对于stop命令,如果thread_running常量为1则输出”stop”,并给常量thread_should_exit置1,之后用于停
止线程(不进行之后的数据处理);否则输出”not started”。
对于status命令,如果thread_running常量为1则输出”is running”;否则输出”not started”。
最重要的还是start命令,当然也需要对thread_running进行判断,为1则输出”already running”;不是得话就
要开启这么一个线程,这里有一个哆嗦模式verbose,对于使用了 ”-v”命令的就开启这个模式,对
inav_verbose_mode置1。接着对thread_should_exit置0,避免线程终止。然后通过任务句柄的设定来开启线
程,如下:
position_estimator_inav_task = /* 这个是守护线程句柄*/
px4_task_spawn_cmd("position_estimator_inav",
SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300,
position_estimator_inav_thread_main,
(argv && argc > 2) ? (char *const *) &argv[2] : (char *const *)
NULL);
这里最主要的就是px4_task_spawn_cmd()这个函数,会使用task_create()来创建任务,这里就主要是操作系统
nuttx的事情了:
int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size,
main_t entry, char *const argv[])
{
int pid;
剩余30页未读,继续阅读
xajh_wk
- 粉丝: 0
- 资源: 9
上传资源 快速赚钱
- 我的内容管理 收起
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
会员权益专享
最新资源
- RTL8188FU-Linux-v5.7.4.2-36687.20200602.tar(20765).gz
- c++校园超市商品信息管理系统课程设计说明书(含源代码) (2).pdf
- 建筑供配电系统相关课件.pptx
- 企业管理规章制度及管理模式.doc
- vb打开摄像头.doc
- 云计算-可信计算中认证协议改进方案.pdf
- [详细完整版]单片机编程4.ppt
- c语言常用算法.pdf
- c++经典程序代码大全.pdf
- 单片机数字时钟资料.doc
- 11项目管理前沿1.0.pptx
- 基于ssm的“魅力”繁峙宣传网站的设计与实现论文.doc
- 智慧交通综合解决方案.pptx
- 建筑防潮设计-PowerPointPresentati.pptx
- SPC统计过程控制程序.pptx
- SPC统计方法基础知识.pptx
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0