申请/专利权人：苏州尚同墨方智能科技有限公司

申请日：2023-12-19

公开（公告）日：2024-06-21

公开（公告）号：CN117760417B

主分类号：G01C21/16

分类号：G01C21/16;G01C21/00;G01S13/86;G01S13/88;G06F18/25;G06F17/16

优先权：

专利状态码：有效-授权

法律状态：2024.06.21#授权;2024.04.12#实质审查的生效;2024.03.26#公开

摘要：本发明公开一种基于4D毫米波雷达与IMU的融合定位方法及系统，该方法包括以下步骤：通过4D毫米波雷达获得周围环境的点云信息及每个点对应的速度信息，通过惯性测量单元获得当前机体的加速度信息和角速度信息；对4D毫米波雷达检测的信息进行处理，获取4D毫米波雷达的速度信息；判断是否进行系统初始化并作出响应；状态估计；将位置信息、速度信息和姿态信息输出至机体，同时将局部点云地图输出，作为机体的环境感知信息。本发明利用4D毫米波雷达获取的自速度信息和稀疏的点云信息对IMU预测的状态进行两次更新，可在GNSS缺失的情况下到达厘米级定位精度，可在恶劣环境中保证系统的正常运行，对计算资源要求较少，可低成本实现。

主权项：1.一种基于4D毫米波雷达与IMU的融合定位方法，其特征在于，包括以下步骤：1通过4D毫米波雷达获得周围环境的点云信息以及每个点对应的速度信息，通过惯性测量单元获得当前机体的加速度信息和角速度信息；2对步骤1所得4D毫米波雷达检测的信息进行处理，获取4D毫米波雷达的速度信息；判断是否进行基于4D毫米波雷达与IMU的融合定位系统初始化并作出响应；3状态估计，获取准确的状态信息；4将位置信息、速度信息和姿态信息输出至机体，同时将局部点云地图输出，作为机体的环境感知信息；步骤2中，在基于4D毫米波雷达与IMU的融合定位系统初始化前，需要对基于4D毫米波雷达与IMU的融合定位系统当前的状态进行判断，当基于4D毫米波雷达与IMU的融合定位系统处于运动的临界时刻时才可以对基于4D毫米波雷达与IMU的融合定位系统执行初始化，初始化结束后才能进入步骤3；是否进行基于4D毫米波雷达与IMU的融合定位系统初始化的判断规则为：S1：初始化窗口收集IMU数据，将初始化窗口平均分成A窗口和B窗口两个区域；S2：分别计算A窗口和B窗口内IMU加速度数据的方差；S3：分别判断A窗口内的加速度方差、B窗口内加速度方差和初始化阈值之间的关系，如果A窗口内的加速度方差大于初始化阈值且B窗口内加速度方差小于初始化阈值，则说明基于4D毫米波雷达与IMU的融合定位系统处于临界的运动状态，此时需要对基于4D毫米波雷达与IMU的融合定位系统执行初始化操作，执行S4，否则返回S1重新采集新的IMU数据；S4：使用B窗口的静态IMU数据对基于4D毫米波雷达与IMU的融合定位系统执行初始化；步骤S4中，使用B窗口的静态IMU数据对基于4D毫米波雷达与IMU的融合定位系统执行初始化的步骤包括：S41：计算B窗口内加速度均值amean，则amean为z轴的方向；S42：确定z轴方向后，利用施密特正交化构建单位坐标系，分别得到x轴和y轴方向；利用x、y、z轴方向向量，即可得到初始旋转Rinit；S43：计算IMU的陀螺仪的bias噪声；计算B窗口内角速度均值，作为陀螺仪bias噪声的初始值S44：计算IMU的加速度计的bias噪声；利用式8计算加速度计bias噪声的初始值 其中，g为当地的重力加速度；S45：位置和速度的初始值设为0；步骤3中，状态估计的步骤包括：3.1状态预测，获得误差状态协方差；3.2状态更新；3.3更新局部地图；步骤3.1中，状态预测，获得误差状态协方差的步骤包括：根据式11所示的运动学方程，对基于4D毫米波雷达与IMU的融合定位系统的名义状态进行预测得到名义状态 鉴于ESKF的误差状态在每次更新后都会被重置，因此对于误差状态，只需要通过式12预测误差状态协方差 其中表示预测的姿态信息，状态转移矩阵F为： 噪声矩阵Q为： 步骤3.2中，状态更新的步骤包括：3.2.1ESKF状态更新，步骤为：S3.2.11：利用式13求解卡尔曼滤波增益K： 其中表示式12中预测的协方差，H为观测的状态转移矩阵，V为观测噪声矩阵；S3.2.12：利用式14更新误差状态： 其中zm表示4D毫米波雷达感知的观测信息，表示通过测量方程获取到估计的状态信息；S3.2.13：利用式15更新误差状态的协方差： 其中Pk+1为更新后的误差状态的协方差；S3.2.14：将误差状态更新到名义状态中得到真实状态，最后将误差状态δx设置为0。

全文数据：

权利要求：

百度查询： 苏州尚同墨方智能科技有限公司 一种基于4D毫米波雷达与IMU的融合定位方法及系统

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD