基于PNT的轮胎吊自动行走方法与流程

基于pnt的轮胎吊自动行走方法
技术领域
1.本发明涉及一种基于综合定位导航授时(pnt)技术的轮胎吊自动行走方法。


背景技术：

2.目前，要想实现轮胎吊起重机(rubber tyre gantry，缩写rtg)的自动行走，首先需要解决rtg的定位、导航和纠偏，因此rtg在自动行走中需要高精度pnt(positioning、navigation and timing—定位、导航与授时)信息以实现rtg的定位和纠偏。现有的rtg定位和纠偏方法主要有：1)人工纠偏法；2)基于图像定位和纠偏法；3)基于gnss定位的轮胎吊自动行走方法；4)gnss和惯性传感器组合的轮胎吊自动行走方法。其中：
3.1)人工纠偏法，需要轮胎吊司机，在大车行走时，人为观察轮胎吊行驶的偏离情况，并通过纠偏手柄手动进行纠偏调整，这种方法依赖人的主动性，需要司机高度集中精力，敏锐观察并迅速响应，很大程度上增加了司机操作的复杂度，很容易造成司机的疲劳。
4.2)基于图像定位和纠偏法，该方法通过视觉摄像机实时检测用于标记大车位置的标志物，如车道线、条形码等，进而实现大车的位置的定位功能以及行走纠偏功能，实现rtg的自动行走；但该方式极易受到天气、光照和其它物体遮挡的影响，环境适应性相对较差。
5.3)基于gnss定位的轮胎吊自动行走方法，通过安装在起重机顶部的gnss天线并采用rtk(real
–
time kinematic，实时动态)载波相位差分技术可以精确定位到大车的实时位置，并将大车位置与预测的场地gis地图进行比对分析可实现大车的自动行走；但gnss信号很容易受周围高大的建筑物或其它高大物体遮挡，导致gnss定位信号变弱甚至丢失，致使定位功能和纠偏功能失效。
6.4)gnss和惯性传感器组合的轮胎吊自动行走方法，该方法通过利用惯性传感器技术可在gnss信号变弱甚至丢失的短时间内进行辅助定位，保持纠偏功能，但当gnss信号丢失时间较长时，其定位误差也会随着增加，进而导致纠偏精度快速减低甚至功能失效；
7.基于以上情况的分析，本发明将gnss、2d激光扫描仪作为传感器单元，提出一种基于综合定位导航授时(pnt)技术的轮胎吊自动行走方法。


技术实现要素：

8.为了克服现有技术的上述缺点，本发明提出了一种基于综合定位导航授时(pnt)技术的轮胎吊自动行走方法。
9.本发明解决其技术问题所采用的技术方案是：一种基于pnt的轮胎吊自动行走方法，包括pnt传感器单元、数据采集单元、信息融合单元和rtg自动行走控制单元，所述pnt传感器单元与数据采集单元相连接，用于获取rtg的定位、导航与授时pnt信息；所述数据采集单元与信息融合单元相连接，用于将rtg的信息输出至信息融合单元；所述信息融合单元与rtg自动行走控制单元相连接，用于将融合之后的数据用于rtg的实时定位、导航；所述rtg自动行走控制单元用于实现rtg的实时定位、导航、纠偏、行进监控；包括如下步骤：
10.步骤1、gnss数据、点云数据的采集与预处理及堆场gis地图测量和绘制；
11.步骤2、对gnss天线装置与2d激光扫描仪进行外部参数标定；
12.步骤3、采集gnss数据与2d激光数据并进行融合，构建slam地图；
13.步骤4、对rtg进行自动行走控制。
14.与现有技术相比，本发明的积极效果是：
15.本发明提出在综合pnt系统的框架下，采用gnss和2d激光为主传感器，通过将rtk载波相位差分技术获得的gnss数据与2d激光扫描仪的数据融合，将2d激光slam技术输出的地图与预先绘制的高精gis地图校验和纠正，可有效保持轮胎吊定位位置精度的可靠性和稳定性，同时在gnss信号变弱甚至丢失情况下，仍可根据2d激光slam构建的地图进行定位、导航，保证rtg的自动行走。该方法有效的克服了单gnss技术信号变弱甚至丢失情况下无法工作的问题，同时相较gnss与惯性传感器组合的技术可以更长时间的实现定位、导航和纠偏功能的保持。
附图说明
16.本发明将通过例子并参照附图的方式说明，其中：
17.图1是本发明的整体结构的俯视图；
18.图2是本发明的轮胎吊行驶的gis地图；
19.图3是本发明的轮胎吊航向角示意图；
20.图4是本发明gnss数据与2d激光数据融合因子图。
具体实施方式
21.为了能充分利用gnss、2d激光扫描仪作为传感器单元的优点，本发明提出了基于综合定位导航授时(pnt)技术的轮胎吊自动行走方法。具体流程如下：
22.1)传感器单元，主要是通过将rtk载波相位差分技术获得的gnss定位数据和2d激光点云数据，并基于机器人操作系统ros(robot operating system)，对上述两种数据及对应时间戳进行打包预处理；同时对轮胎吊作业集装箱堆场通过人工测量的方式进行场地地图数据采集，并绘制为gis地图文件，存储在计算机内。
23.2)数据采集单元，主要是对gnss数据和2d激光扫描数据进行采集。在数据采集之前，需要对两传感器的安装位置关系进行标定。通过对gnss天线装置与2d激光扫描仪装置的外部参数标定，获取两种装置安装的位置关系，求取两者坐标系对齐的位姿数据(r,t)，即旋转矩阵r和平移矩阵t。
24.3)信息融合单元，对gnss数据和2d激光数据进行融合，通过2d激光slam算法进行地图构建，并将其与预先测量的高精gis地图进行校验和纠正；旨在获取与gis地图一致的slam地图，并且在gnss信号变弱甚至丢失时，仍可进行轮胎吊定位。
25.4)rtg自动行走控制单元，基于轮胎吊的位置，估算轮胎吊的行驶姿态，并将姿态与gis地图或slam地图进行实时比对校验，计算输出偏离位置和纠偏量，并输出给起重机电控系统plc进行纠偏控制。
26.以下将结合附图对本发明方法进行详细描述：
27.本发明的整体场景结构如图1所示，包括：轮胎吊1、电气房2、轮胎吊电控系统plc 3、堆场集装箱4、轮胎吊车道线5、gnss天线装置6、2d激光扫描仪7、2d激光扫描仪线8、计算
机9、gnss接收器10、gnss电台11、报警器12。轮胎吊司机驾驶轮胎吊1在堆场内行驶时，位于轮胎吊电气房2内的gnss接收器10通过安装在轮胎吊4个门腿上的4个gnss天线装置6接收gnss数据，并通过gnss电台11与gnss基准站通信，通过rtk技术进行差分定位计算，并将定位结果通过gnss电台传输给电气房内的计算机9；同时计算机通过ros工具采集分别安装于轮胎吊海陆侧的两台2d激光扫描仪7的激光数据；接着计算机内系统将两者数据融合并将slam输出的地图与预先存储在计算机内的gis地图校验和纠正；最后系统将定位结果转换为轮胎吊的定位数据并进行处理和分析，同时实时与gis地图和slam地图进行比对和分析，输出纠偏量给起重机电控系统plc，最后由plc完成纠偏动作的执行，进而实现轮胎吊的纠偏功能；轮胎吊行驶过程中，当存在轮胎吊行驶姿态偏离过大，纠偏量过大，plc无法快速响应执行纠偏动作的情况时，计算机控制报警器12进行鸣笛报警提示轮胎吊司机注意。
28.基于综合定位导航授时(pnt)技术的轮胎吊自动行走方法具体流程如下：
29.1.gnss数据、点云数据的采集与预处理及场地gis地图测量和绘制
30.1.1gnss数据、点云数据的采集与预处理
31.本发明采用了多模gnss，包括bds、gps、glonass等卫星导航定位系统。同时系统采用4台gnss天线装置6融合定位，可获得高精度的位置、航向、姿态等信息，且4天线互为校验、互相融合，系统可稳定连续工作。在gnss信号没有遮挡和中断的情况下，利用多模gnss系统采集gnss安装位置的经度、纬度和高度、时间戳数据(用于pnt的授时)，使用激光扫描仪采集点云数据和时间戳。对于激光数据的接收与转换包括：ros驱动获取激光数据，通过sick官方提供的ros驱动直接连接激光的一个端口，直接获取ros格式发布的激光数据。
32.1.2gis地图测量和绘制
33.在进行轮胎吊自动行走之前，需要提前获取到所在堆场的高精度地图数据：外业主要通过rtk或者三维激光扫描仪来采集数据，内业用cass 9.0、arcgis10.0、contextcapture等gis软件或者点云处理软件，做成电子地图的样式，保存在控制器中。在外业采集时，对rtg行车线进行重点采集，以保证后续的rtg定位和纠偏功能的需求。如图2中的gis地图所示。
34.2.gnss数据与激光点云数据坐标系的标定
35.gnss坐标系与激光扫描仪坐标系位姿变换矩阵t2×1,可以通过人为量测得到。如图1所示，4台gnss天线装置安装在轮胎吊的四个门腿的上方接收gnss信号。两台激光扫描仪分别安装在rtg的海、陆两侧获得激光点云数据。两传感器安装完毕之后，人为的在水平面上量测即可得到矩阵t2×1。
36.在gnss数据和激光点云数据融合之前，需要通过时间戳对齐的方式实现数据匹配，以满足pnt中统一授时的要求：gnss采集的数据的时间戳time
gnss
是周秒，激光雷达采集的数据的时间time
lidar
是距离最近的整点的秒数；此外，time
gnss
和time
lidar
之间存在着一个差值为t的闰秒差(待测定)。为了实现时间戳格式的统一，对time
gnss
做预处理，记预处理后的gnss的时间戳为time
gnssl
：
37.time
gnssl
＝time
gnss
％3600
–
t
ꢀꢀꢀꢀ
(1)
38.在完成激光扫描仪与gnss的位置的标定与时间戳的标定之后，还需进一步的在数据级层面上进行融合。本发明所涉的gnss数据、激光数据融合到slam坐标系的对齐方法如下：在一定的短距离下，gnss的精度良好，激光扫描仪的位置在gnss坐标系表示精确获取。
假设xi,yi—为2d激光扫描仪i在gnss坐标系下的水平坐标，ai,bi—为2d激光扫描仪在激光坐标系下的水平表示(激光里程计中获得)。其中：为2d激光扫描仪相对gnss坐标系下的航向偏转角。则有：
[0039][0040]
数据的处理流程如下：
[0041]
通过接收gnss数据(经度、纬度、高度、dop值、时间戳)，把第一帧设为本地坐标系xyz的原点，后续gnss数据转换到本地坐标系下的相对位置，并保存转换后的数据至缓存。
[0042]
通过接收的激光点云数据，经由激光里程计算法计算出2d激光扫描仪位置在激光扫描仪坐标系下运动轨迹并保存至缓存。
[0043]
加载gnss缓存数据p和激光里程计数据q(本发明中指的是cartographer算法计算得到的里程计数据q)缓存数据，根据每个数据的时间戳对齐gnss数据值和激光里程计数据，并保存对齐好的数据点对。
[0044]
根据两点之间的运动距离阈值，挑选出合适的数据点对。结合公式(1)采用奇异值分解(singular value decomposition，简称svd)方法计算求解航向角yaw，具体步骤如下：
[0045]
1》计算两组点pi＝[x
i yi]
t
，qi＝[a
i bi]
t
的质心位置p,q。其中：
[0046][0047]
两组点分别去质心坐标：
[0048]
pi＝p
i-p，qi＝q
i-q
ꢀꢀꢀꢀꢀ
(4)
[0049]
2》可转化为优化问题：
[0050][0051]
r—2
×
2旋转变换矩阵。
[0052]
3》计算航向夹角：
[0053]
yaw＝atan2(r
(1,0)
，r
(0,0)
)
ꢀꢀ
(6)
[0054]
式(6)中，yaw即为航向角，r(1,0)为r的第二行第一列的值，r(0,0)为r的第一行第一列的值。以上即为激光扫描仪相对于gnss的航向夹角的计算过程。
[0055]
3.gnss数据、激光点云数据融合slam
[0056]
当gnss信号失锁的情况下，本发明在cartographer的算法基础之上，通过因子图的方式融合激光点云数据和gnss数据。二维激光扫描仪以10hz的频率发布点云数据。
[0057]
雷达里程计节点对采集的点云数据采用最近迭代点(icp)方法进行点云配准得到雷达的位姿估计；lidar mapping(激光雷达构图)节点以2hz的频率将当前帧的点云与整个点云地图再次进行配准以优化里程计的位姿估计结果，同时将失效前的gnss数据添加到因子图中进行优化，并以2hz的频率发布优化后的位姿估计结果；transform integration(数据转换模块)节点将lidarodometry(雷达里程计)与lidarmapping(雷达构图)两个节点的位姿估计结果进行融合，以10hz的频率发布最终准确的位姿估计结果。
[0058]
本发明融合激光雷达与gnss的因子图，如图4所示，其中：黑点表示位姿，f
lidar
表示点云数据参与位姿计算，f
gnss
表示gnss数据参与位姿计算。因子图中用x1，x2，xi等表示随时
间变化的位姿状态量；s
lidar
表示激光雷达通过点云配准得到的位姿因子：
[0059]slidar
＝d(x
k+1-h(xk))(7)
[0060]
式(7)中d表示k+1时刻估计值与k时刻的位姿差值。x
k+1
表示k+1时刻的估计值，h(xk)表示由k时刻递推的状态值。如图4所示，每个因子节点可以理解为对与其相连的状态量的约束和测量，对于任何因子图寻找最大后验概率估计值推断可以归结为最大化所有因子势能的乘积，并通过gtsam求解得到全局最优解：
[0061][0062]
由公式(8)即可得到融合后的激光扫描仪的位姿值。
[0063]
利用激光雷达获取点云数据，使用高精度的雷达里程计cartographer算法获取位姿数据rpy角和相对位置(x,y,z)。在gnss数据不可靠的路段，先将该路段起点的由gnss得到的位姿数据传递给cartographer算法作为初值，使用cartographer算法获得该行程段每一帧点云的位姿t1，由rpy角和坐标点(x,y,z)信息计算得到的变换矩阵，然后将上述位姿t1作为输入，输入到elch算法中，获得每一帧点云位姿的累积误差t2。最后使用t2优化来自雷达里程计的位姿，记优化后的位姿为t3：
[0064]
t3＝t2*t1ꢀꢀꢀꢀꢀ
(9)
[0065]
使用(9)得到的融合后的位姿t3转换激光雷达的点云数据到世界坐标系下，获取最终的全局地图；记点云中某个点在雷达坐标系下的坐标为p1，转换到世界坐标系下的坐标为p2：
[0066]
p2＝t3*p1ꢀꢀꢀꢀ
(10)
[0067]
4.rtg自动行走控制
[0068]
本发明的方法结合高精度gis地图信息、gnss数据、激光lidar里程计数据的定位信息，可以实时获得高精度、可靠的大车位置、航向、姿态等信息。
[0069]
系统通过rtg本机接收rtg远控系统大车自动运行命令后，控制器匹配当前大车位置和目标作业位置，向plc输出控制运动命令，自动控制rtg大车向目标位置运动，并自动停车到准确的作业贝位上。当rtg大车行走时，控制器可以获得大车的实时位置，通过plc可以获取大车行走的速度和方向并发送给控制器，控制器会实时检测大车位置及行走的方向，并计算大车轮胎与地面上基准线的偏差值并发送给plc，当预判rtg有跑偏趋势或偏离达到阀值时，可以通过plc分别控制增加或减少rtg两侧的轮胎速度，形成前后轮胎速度差从而实现纠偏作用，将rtg行走方向纠正过来。如图2和图3所示，当gnss信号受到干扰，此时rtg功能停止。假设此时点a(x1,y1)为gnss信号中断时测得的坐标值，根据公式(5)和公式(11)，计算得到的激光扫描仪在世界坐标系下的坐标，a(x1,y1)坐标融合到激光lidar slam里程计数据中。通过里程计累加得到点b(x2,y2)，根据纠偏阈值得知此时轮胎吊已经发生了偏移，在点b(x2,y2)处进行纠偏操作，c(x3,y3)处为rtg行车线关键点。根据余弦定理：
[0070]
cosα＝(ab2+bc
2-ac2)/2*ab*bc
ꢀꢀ
(11)
[0071]
cosγ＝(cd2+de
2-ce2)/2*cd*de (12)
[0072]
计算可得α,γ的弧度值：arctanα,arctanγ，如果α,γ的值小于一定的阈值l，则认为出现偏移，需要进行纠偏操作，反之，则不需进行纠偏。然后根据rtg前进方向，判断其
位于导航线哪一侧的容限范围内，在弧度绝对值前加上相应的符号(α)为正号的情况，(γ)为负号的情况。将计算的弧度值和纠偏指令通过信息发送模块发送到plc控制系统的寄存器中，由寄存器通过运算，将纠偏值转化为指令发送到编程控制器，再由控制器发送指令到rtg的轮胎电机，调整左右轮胎的角度(或调整左右轮的转速，要根据转向控制方式而定)，实现调整rtg前进姿态。