[发明专利]一种多源辅助导航方法及装置有效
| 申请号： | 202110461932.6 | 申请日： | 2021-04-27 |
| 公开（公告）号： | CN113155124B | 公开（公告）日： | 2022-03-11 |
| 发明（设计）人： | 赵荟文 | 申请（专利权）人： | 涵涡智航科技（玉溪）有限公司 |
| 主分类号： | G01C21/16 | 分类号： | G01C21/16;G01C21/20;G01S19/45 |
| 代理公司： | 北京精金石知识产权代理有限公司 11470 | 代理人： | 尉月丽 |
| 地址： | 653100 云南省*** | 国省代码： | 云南;53 |
| 权利要求书： | 查看更多 | 说明书： | 查看更多 |
| 摘要： | |||
| 搜索关键词： | 一种 辅助 导航 方法 装置 | ||
1.一种多源辅助导航方法，其特征在于：通过至少一个图像获取装置、至少一个惯性传感器采集的数据，计算视觉惯导定位坐标视觉惯导定位权重Wv；
通过至少一个卫星信号接收装置、至少一个惯性传感器采集的数据，计算卫星惯导定位坐标卫星惯导定位权重WG；
通过所述视觉惯导定位坐标视觉惯导定位权重Wv、卫星惯导定位坐标卫星惯导定位权重WG，计算融合定位坐标
通过地图数据、所述融合定位坐标进行路径规划、导航；
所述视觉惯导定位坐标通过如下方式计算：
通过式1计算k时间点的所述视觉惯导定位坐标的状态估计量
式1中，为k-1时间点的视觉惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(v)为预设的视觉惯导状态更新矩阵，B(v)为预设的惯导输入信号变换矩阵；
通过式2、式3计算k时间点的视觉惯导卡尔曼增益K(v)k：
式2中，为k时间点的所述的协方差矩阵P(v)k的估计量，P(v)k-1为k-1时间点的所述的协方差矩阵，Q(v)为预设的视觉惯导噪声矩阵；式3中，H(v)为预设的视觉惯导观测坐标变换矩阵；R(v)为视觉量测噪声矩阵；
通过式4计算k时间点的所述视觉惯导定位坐标
其中，Z(v)k为k时间点的视觉定位坐标；
通过式5计算k时间点的所述视觉惯导定位坐标的协方差矩阵P(v)k，所述P(v)k用于k+1时间点的视觉惯导定位坐标的计算：
；
所述卫星惯导定位坐标通过如下方式计算：
通过式6计算k时间点的所述卫星惯导定位坐标的状态估计量
式6中，为k-1时间点的卫星惯导定位坐标，uk-1为k-1时间点的所述惯性传感器采集的信号向量，A(G)为预设的卫星惯导状态更新矩阵，B(G)为预设的惯导输入信号变换矩阵；
通过式7、式8计算k时间点的卫星惯导卡尔曼增益K(G)k：
式7中，为k时间点的所述的协方差矩阵P(G)k的估计量，P(G)k-1为k-1时间点的所述的协方差矩阵，Q(G)为预设的卫星惯导噪声矩阵；式8中，H(G)为预设的卫星惯导观测坐标变换矩阵；R(G)为卫星量测噪声矩阵；
通过式9计算k时间点的所述卫星惯导定位坐标
其中，Z(G)k为k时间点的卫星定位坐标；
通过式10计算k时间点的所述卫星惯导定位坐标的协方差矩阵P(G)k，所述P(G)k用于k+1时间点的卫星惯导定位坐标的计算：
所述Wv通过式11计算：
Wv＝1-tr(P(v)k)/(tr(P(v)k)+tr(P(G)k)) 式11
所述WG通过式12计算：
WG＝1-tr(P(G)k)/(tr(P(v)k)+tr(P(G)k)) 式12。
2.如权利要求1所述的多源辅助导航方法，特征在于：所述视觉惯导定位权重Wv、卫星惯导定位权重WG满足如下关系：Wv+WG＝1。
3.如权利要求2所述的多源辅助导航方法，特征在于：所述融合定位坐标通过式13计算：
该专利技术资料仅供研究查看技术是否侵权等信息，商用须获得专利权人授权。该专利全部权利属于涵涡智航科技（玉溪）有限公司，未经涵涡智航科技（玉溪）有限公司许可，擅自商用是侵权行为。如果您想购买此专利、获得商业授权和技术合作，请联系【客服】
本文链接：http://www.vipzhuanli.com/pat/books/202110461932.6/1.html，转载请声明来源钻瓜专利网。
