一种AGV相对导航方法及系统

本发明涉及agv导航，更具体的说是涉及一种agv相对导航方法及系统。

背景技术：

1、agv已在制造业车间、仓储物流、集装箱码头等领域广泛部署。目前主流导航方式包括：磁条/磁钉导引、二维码视觉定位、激光slam、视觉slam以及惯性/卫星组合导航等。这些定位方案存在一个共同的技术前提：agv行驶的地面（厂房地面、仓库地面、码头地面）相对地球近似静止，世界坐标系中的地图不随时间变化，定位输出的绝对位姿与运动控制量直接对应。然而，在船舶及浮式海洋平台场景中，甲板与舱室地面随波浪持续做六自由度运动，目前该领域甲板/舱室的物资转运仍主要依赖人工作业与普通叉车，海上平台方向的工作集中于固定式导管架平台的巡检机器人；船舶方向的工作集中于造船厂岸侧车间内的预制舱室移载机器人与机舱救援机器人路径规划研究；而现有的agv导航方案在浮式载体上，存在以下问题：

2、1.现有slam地图定义于地球静止坐标系，平台摇摆使世界持续运动，导致地图畸变、闭环失败、定位漂移，路径跟踪发散；2.单帧激光扫描周期（如100ms）内平台横摇角速度可达数度每秒，扫描点云产生显著运动畸变，匹配退化甚至丢定位；3.车体惯性测量单元同时感知平台牵连运动与车体自身运动，二者耦合，惯性数据无法直接用于车体相对甲板的位姿推算；4.大横摇条件下agv存在侧滑与倾覆风险，现有导航系统未将平台运动状态纳入速度与路径约束。因此，如何提供一种agv相对导航方法及系统是本领域技术人员亟需解决的问题。

技术实现思路

1、有鉴于此，本发明提供了一种agv相对导航方法及系统，将导航参考系从静止坐标系切换为与浮式载体固连的坐标系，在动基座条件下输出车体相对甲板的高精度相对位姿，据此进行agv导航。

2、为了实现上述目的，本发明提供如下技术方案：

3、一种agv相对导航方法，包括以下步骤：

4、s1、获取浮式载体上的平台惯性测量单元输出的平台惯性数据，基于波浪运动学模型约束的状态估计器对平台惯性数据进行滤波估计，得到浮式载体的平台运动状态；

5、s2、获取安装于agv车体的车体惯性测量单元输出的车体惯性数据以及轮速计输出的轮速数据；将车体惯性数据与平台惯性数据在统一坐标系下做差分处理，并以误差状态卡尔曼滤波器结合轮速数据估计车体相对浮式载体的相对运动状态；

6、s3、获取车体激光雷达输出的原始点云帧，依据平台运动状态与相对运动状态确定原始点云帧内各激光点对应扫描时刻的车体相对位姿，按扫描时刻对各激光点进行运动补偿，得到平台坐标系下的无畸变点云；

7、s4、将无畸变点云与预先构建的平台坐标系下的相对地图进行匹配，将匹配得到的位姿测量值作为观测量对误差状态卡尔曼滤波器进行更新，得到车体的相对位姿；

8、s5、获取浮式载体上的信标网络中多个信标与agv车体上安装的信标标签之间的测距观测量，基于各信标在相对地图中的标定坐标，对相对位姿进行校正，得到校正后的相对位姿；

9、s6、根据平台运动状态与校正后的相对位姿，在动基座安全约束下生成导航控制指令，以控制agv在浮式载体上行驶。

10、可选的，s1中的波浪运动学模型为自回归滑动平均模型，模型参数由平台惯性数据的频谱特性辨识得到；状态估计器为扩展卡尔曼滤波器，状态向量包括横摇角、横摇角速度、纵摇角、纵摇角速度、艏摇角、艏摇角速度、升沉位移与升沉速度。

11、可选的，s2具体为：

12、获取安装于agv车体的车体惯性测量单元输出的车体惯性数据以及轮速计输出的轮速数据；将车体惯性测量单元输出的车体坐标系角速度变换至平台坐标系：

13、；

14、式中，为车体坐标系角速度，为平台坐标系角速度，为车体坐标系到平台坐标系的旋转矩阵，为车体相对平台的角速度；

15、将车体惯性测量单元输出的比力扣除平台牵连加速度、科氏加速度、杠杆臂补偿项与重力分量：

16、；

17、式中，为平台惯性测量单元输出的比力，为杠杆臂矢量，为平台角速度矢量，为平台角加速度矢量，为车体相对平台的相对速度，为车体相对平台的相对线加速度；对进行积分得到相对速度估计；以误差状态卡尔曼滤波器融合相对速度估计与轮速计里程增量，估计相对运动状态。

18、可选的，s3具体为：

19、获取车体激光雷达输出的原始点云帧，设第帧的原始点云帧的基准时刻为，帧内第个激光点的扫描时刻为，运动补偿公式为：

20、；

21、式中，为位姿变换复合运算，为时刻激光点在车体坐标系下的坐标，为时刻激光点在车体坐标系下的坐标，为时刻的预测相对位姿，为时刻的预测相对位姿，各扫描时刻的预测相对位姿由平台运动状态与相对运动状态经时间插值得到。

22、可选的，s4具体为：

23、以原始点云帧基准时刻的预测相对位姿作为初始位姿，将无畸变点云与预先构建的平台坐标系下的相对地图进行正态分布变换匹配，得到基准时刻的位姿测量值；将位姿测量值作为观测量反馈至s2中估计车体相对浮式载体的相对运动状态，将结果与基准时刻的预测相对位姿融合，得到车体的相对位姿。

24、可选的，s5基于各信标在相对地图中的标定坐标，对相对位姿进行校正具体为：

25、第个信标的测距观测方程为：

26、；

27、式中，为信标标签与第个信标之间的测距值，为车体在平台坐标系下的位置，为车体相对平台的航向角，为航向角对应的旋转矩阵，为信标标签在车体坐标系下的安装偏置矢量，为第个信标在相对地图中的标定坐标，为测距噪声；以加权最小二乘法解算相对位姿校正量；校正权重根据各信标测距观测量的方差与非视距检测结果确定，非视距检测根据超宽带信号信道脉冲响应的首径能量占比判别。

28、可选的，s6中根据平台运动状态与校正后的相对位姿，在动基座安全约束下生成导航控制指令具体为：

29、根据平台运动状态中的横摇角幅值执行分级限速约束：当横摇角幅值不超过第一阈值时，控制agv按第一车速限值行驶；当横摇角幅值超过第一阈值且不超过第二阈值时，控制agv按低于第一车速限值的第二车速限值行驶，并禁止沿船宽方向的横向路径；当横摇角幅值超过第二阈值且不超过第三阈值时，控制agv按低于第二车速限值的第三车速限值沿船长方向行驶；当横摇角幅值超过第三阈值时，控制agv制动驻车并锁止驻车机构；

30、根据平台运动状态中的横摇角、甲板摩擦系数在线估计值与agv的载货状态，按侧向受力平衡约束计算允许的最大横向加速度、最大允许车速与最小转弯半径，并写入局部路径规划；其中，侧向受力平衡约束满足：，其中，为车体横向加速度，为甲板摩擦系数在线估计值，为重力加速度，为横摇角。

31、可选的，s6还包括：根据平台运动状态预测未来预设时段内浮式载体的运动包络；将关键作业任务安排于运动包络平稳的波浪窗口内执行，巡航类任务按当前横摇角幅值所在的分级限速规则执行。

32、一种agv相对导航系统，应用上述的一种agv相对导航方法，包括：

33、平台惯性测量单元，安装在浮式载体上，用于获取浮式载体的平台惯性数据；

34、平台状态估计模块，与平台惯性测量单元连接，基于波浪运动学模型约束的状态估计器对平台惯性数据进行滤波估计，得到浮式载体的平台运动状态；

35、车体感知模块，包括安装在agv车体的激光雷达、车体惯性测量单元和轮速计，分别用于获取原始点云帧、车体惯性数据和轮速数据；

36、信标模块，包括安装在浮式载体上的多个信标，以及安装于agv车体、用于与各信标进行测距的信标标签；

37、车载计算模块，与平台惯性测量单元、平台状态估计模块、车体感知模块、信标模块连接，用于生成agv车体的导航控制指令；执行模块，与车载计算模块连接，用于接收车载计算模块输出的导航控制指令，驱动agv在浮式载体上行驶。

38、可选的，车载计算模块包括相对状态估计单元、点云补偿单元、相对位姿匹配单元、相对位姿校正单元和导航控制单元；相对状态估计单元基于车体惯性数据、平台惯性数据和轮速数据估计车体相对浮式载体的相对运动状态，点云补偿单元确定原始点云帧内各激光点对应扫描时刻的车体相对位姿，按扫描时刻对各激光点进行运动补偿，得到平台坐标系下的无畸变点云，相对位姿匹配单元将无畸变点云与预先构建的平台坐标系下的相对地图进行匹配，得到车体的相对位姿，相对位姿校正单元基于各信标在相对地图中的标定坐标，对相对位姿进行校正，得到校正后的相对位姿；导航控制单元根据平台运动状态与校正后的相对位姿，在动基座安全约束下生成导航控制指令。

39、经由上述的技术方案可知，与现有技术相比，本发明提供了一种agv相对导航方法及系统，具有以下有益效果：本发明将地图与定位基准定义于浮式载体运动的坐标系，从根本上规避了动态环境导致的slam失效问题；通过逐点相对位姿补偿消除点云畸变导致的匹配退化；通过差分惯性、轮速计与固连信标网络的多源融合，在金属舱室完全遮挡卫星信号的条件下仍保持长期稳定的相对定位精度；将平台横摇角、甲板摩擦系数与载货状态纳入速度、转弯与驻车约束，有效降低侧滑与倾覆风险；基于平台运动包络预测的波浪窗口调度，使与固定设备对接等关键作业的成功率与平均等待时间显著改善。