一种激光无人叉车导航参数自动校准系统及方法与流程

本发明涉及激光无人叉车导航系统，尤其涉及一种激光无人叉车导航参数自动校准系统和方法。

背景技术：

1、激光无人叉车的自主导航运行需要事先配置好和导航相关的各种参数，这些参数通常分为两类：一类是与激光无人叉车的里程计计算相关的参数如激光无人叉车驱动前轮的轮半径、前轮与激光无人叉车运动参考点的距离即轴距；另一类参数（）描述了激光雷达参考坐标系相对于激光无人叉车运动参考坐标系的x、y坐标轴方向的距离偏差和绕z坐标轴方向的角度偏差。

2、在实际应用中，基于差速驱动移动底盘的移动机器人对其导航相关参数进行自动校准的方法已被使用，但是针对单舵轮驱动形式的移动机器人如激光无人叉车，在工厂的实际运行使用环境中，对导航相关的参数如驱动前轮的轮半径、驱动前轮的轴距及激光雷达相关的导航位置参数（），同时定期进行自动校准的系统和方法还没有。

3、随着移动机器人slam导航技术的发展，采用基于工厂的实际环境进行导航的激光无人叉车已逐渐得到大量的应用；因此，针对工业场景应用中的基于工厂实际周围环境导航的激光无人叉车，对其相关导航参数进行自动校准的问题开展研究非常必要。

技术实现思路

1、为了解决现有的激光无人叉车导航相关参数采用手动或半自动方式校准而不是在激光无人叉车实际运行过程中自动进行校准的问题，本发明提供了一种激光无人叉车在生产实际运行过程中导航参数自动校准的系统和方法。

2、一种激光无人叉车导航参数自动校准系统，实施于激光无人叉车，激光无人叉车包括一驱动前轮、两个运动轴共线的后轮，前轮为驱动转向轮，其中：所述激光无人叉车导航参数自动校准系统包括中央控制器，激光无人叉车前轮上设置有用于测量前轮转动角速度的驱动编码器，激光无人叉车前轮上还设置有用于测量前轮绕垂直于地面的轴线旋转角度的转向编码器，在激光无人叉车的顶部设置有导航用单线激光雷达，激光无人叉车进行导航时，需要单线激光雷达执行建图和定位功能；所述驱动编码器的采集信号输出端与用于驱动前轮的电机驱动器的驱动编码器信号输入端相连，再由驱动前轮的电机驱动器的can接口通过can总线将前轮绕中心旋转的角速度值发送给中央控制器；所述转向编码器的采集信号输出端通过can总线将所检测到的前轮绕垂直于地面的轴线所旋转角度的数值发送给中央控制器，单线激光雷达的信号输出端通过以太网接口连接所述中央控制器的雷达信号输入端。

3、本发明还提供了一种激光无人叉车导航参数自动校准的方法，其中：包括如下步骤：

4、1）、预先输入激光无人叉车用导航参数初始值：激光无人叉车前轮的轮半径、前轮的中心与激光无人叉车的运动参考点之间的距离即轴距、导航传感器单线激光雷达坐标系与激光无人叉车的运动参考点坐标系之间的相对位姿，激光无人叉车的运动参考点指的是两个运动轴共线的从动后轮轴的中心位置；

5、2）、激光无人叉车正常运行时，当经过预先设定的路段时，由中央控制器根据激光无人叉车的位置自动启动激光无人叉车的导航参数自动校准系统，并自动将激光无人叉车实时运行时的前轮绕轮中心旋转的角速度值、前轮绕垂直于地面轴线的旋转角度及单线激光雷达的实时扫描数据等相关测量数据存储记录为数据文件，所存储记录的数据文件采用二进制文件的格式，采用直接读取所测量的数据在内存中的存储地址中所存储的相应二进制数值，并存储记录下来；

6、3）、激光无人叉车导航参数的最优化非线性求解过程，在相邻两帧激光扫描点云时间戳时间之间将通过激光雷达的帧间匹配方法所得出的激光雷达在全局参考坐标系下的位姿偏差做为第一位姿偏差，在相同的时间段内将由激光无人叉车的运动学模型所计算得出的激光雷达在全局参考坐标系下的位姿偏差做为第二位姿偏差，最后将第一位姿偏差与第二位姿偏差作差，称为它们之间的位姿偏差残差。此位姿偏差残差最小化问题可以通过非线性最小二乘优化方法求解，这里采用高斯牛顿迭代的非线性优化方法求解。在求取偏差残差最小值的优化迭代过程中，可在预先所输入的激光无人叉车导航参数初始值的基础上，最终得出经过优化校正后的激光无人叉车导航参数的准确数值。

7、进一步的，所述的激光无人叉车导航参数自动校准的方法，其中：所述的步骤3）具体步骤如下：

8、3-1）、单线激光雷达360度扫描检测周围环境，当激光雷达按照所设定的扫描频率工作时，则激光雷达每隔一个扫描周期扫描一圈，将所扫描的一圈360度环境点云数据做为完整的一帧数据；当激光无人叉车运行在预先设定的导航参数自动校准路线上时，激光无人叉车的运动速度为400mm/s左右，则在激光雷达获得扫描完整一帧数据期间，激光无人叉车的激光雷达发生了相应的位姿变化，在该扫描周期运行期间，根据激光无人叉车实际运行时的时间戳和对应的位姿，对当前时刻扫描的激光点云使用激光雷达运动畸变去除的方法，由此可得到经过处理后的激光雷达扫描完整一帧360度的二维激光点云数据；以此类推，在每隔相邻的扫描周期内，就分别得到经过去运动畸变校正后的一系列完整激光点云数据；

9、3-2）、经过去运动畸变校正处理后的激光点云数据，采用公开的激光点云的帧间匹配方法即pl-icp方法，计算出每两帧相邻完整激光扫描点云时间戳时间之间，激光雷达在全局参考坐标系下的实际位姿偏差，记为；其中表示第i时刻激光雷达在全局参考坐标系下两帧激光点云时间戳时间之间的位姿偏差，表示激光雷达位姿偏差在全局参考坐标系下x方向的偏差，表示激光雷达位姿偏差在全局参考坐标系下y方向的偏差，表示激光雷达位姿偏差在全局参考坐标系下绕z方向的角度偏差；

10、3-3）、根据激光无人叉车的运动学模型，计算激光无人叉车的运动参考点在相邻两帧激光扫描点云时间戳时间之间，在全局坐标系下的位姿偏差：

11、根据激光无人叉车的运动特点，前轮是驱动转向轮，两个后轮是运动轴共线的固定的从动轮，在一极小时间段内，激光无人叉车的运动参考点由点—坐标为行驶到点—坐标为，其中表示激光无人叉车的运动参考点在第时刻的位姿，表示激光无人叉车的运动参考点在第时刻的位姿，根据激光无人叉车的运动学模型，则可以得到激光无人叉车的运动参考点由点运动到点的位姿，如式（1）所示：

12、（1）

13、式（1）中表示激光无人叉车的运动参考点在第时刻全局坐标系下x方向的位置，表示激光无人叉车的运动参考点在第时刻全局坐标系下y方向的位置，表示激光无人叉车的运动参考点在第时刻全局坐标系下的方向角度；表示激光无人叉车的运动参考点在第时刻全局坐标系下x方向的位置，表示激光无人叉车的运动参考点在第时刻全局坐标系下y方向的位置，表示激光无人叉车的运动参考点在第时刻全局坐标系下的方向角度；表示激光无人叉车驱动前轮的半径，b表示激光无人叉车前轮中心到激光无人叉车运动参考点的距离，表示激光无人叉车前轮的转动角速度，表示激光无人叉车驱动前轮与车体中心轴线的夹角；

14、进行激光无人叉车的运动参考点在相邻两帧激光扫描点云时间戳时间之间位姿偏差的计算：由于根据式（1）所计算得出的激光无人叉车运动参考点的实时位姿所对应的时间戳，与所连续获得的完整一帧激光扫描点云所对应的时间戳不能一一对应，因此需要对激光扫描帧时间戳的前后两个相邻的由式（1）计算得出的激光无人叉车的运动参考点位姿进行线性插值，以计算出在连续获得的完整激光扫描帧时间戳的时刻下激光无人叉车的运动参考点的实时位姿，计算公式如下式（2）所示：

15、（2）

16、式（2）中为插值得到的在激光扫描帧时间戳m的时刻激光无人叉车运动参考点的位姿，为激光扫描帧时间戳之前最近时刻的激光无人叉车里程计位姿，为激光扫描帧时间戳之后最近时刻的激光无人叉车里程计位姿，是根据式（1）计算得出的激光无人叉车的运动参考点分别在第、k时刻的位姿，为对应的时间戳，为对应的时间戳，为m时刻对应的时间戳，最终得出相邻两帧激光扫描点云时间戳时间之间激光无人叉车运动参考点的位姿偏差为，其中表示李群的逆，表示在se(2)空间上的李群运算，表示第m时刻激光无人叉车运动参考点在相邻两帧激光扫描点云时间戳时间之间的位姿偏差，为插值得到的在激光扫描帧时间戳m的时刻激光无人叉车运动参考点的位姿，为插值得到的在激光扫描帧时间戳m-1的时刻激光无人叉车运动参考点的位姿；

17、3-4）、激光无人叉车导航参数的最优化非线性求解过程，在相邻两帧激光扫描点云时间戳时间之间将通过激光雷达的帧间匹配方法所得出的激光雷达在全局参考坐标系下的位姿偏差做为第一位姿偏差，在相同的时间段内将由激光无人叉车的运动学模型所计算得出的激光雷达在全局参考坐标系下的位姿偏差做为第二位姿偏差，最后将第一位姿偏差与第二位姿偏差作差，称为它们之间的位姿偏差残差。此位姿偏差残差最小化问题可以通过非线性最小二乘优化方法求解；

18、3-4-1）所述非线性最小二乘求解的目标函数如下：由步骤3-2）所得到的通过激光雷达的帧间匹配得出的位姿偏差为，由激光无人叉车的运动学模型计算得出的激光雷达的位姿偏差为，设所述位姿偏差与所述位姿偏差之间的误差服从高斯分布，设所述位姿偏差与所述位姿偏差的协方差矩阵为，如式（3）所示：

19、（3）

20、其中、、分别表示激光无人叉车的激光雷达的位姿对应在全局坐标系下x坐标、y坐标及方向角度上的误差所得出的标准差；

21、非线性问题求解的目标函数如下式（4）所示：

22、（4）

23、其中表示激光无人叉车激光雷达的位姿偏差与位姿偏差的残差，，其中表示激光雷达的导航位置参数（）， 、 、分别表示激光雷达坐标系参考位置相对于激光无人叉车运动参考坐标系的x、y坐标轴方向的距离偏差和绕z坐标轴方向的角度偏差，表示相邻两帧激光扫描点云时间戳时间之间激光无人叉车的运动参考点的位姿偏差，将式（4）可简化为下式（5）：

24、（5）

25、其中（6）

26、3-4-2）采用高斯牛顿迭代法求解目标函数的最小值，如式（7）所示：

27、 （7）

28、根据最小二乘法定义：（8）

29、（9）

30、其中为的雅克比矩阵，则

31、（10）

32、因此求取时，需要的一阶导数为0，即

33、（11）

34、由式(11)可知（12）

35、从而最终得出的迭代公式如下式（13）所示：

36、（13）

37、表示迭代步长的系数，采用经验值，取值范围为（0.5~5.0），为第i次迭代的修正量，表示第i次迭代的状态向量，此向量表示为，由式（12）可知，表示第i次迭代的雅可比矩阵，表示第i次迭代的雅可比矩阵的转置，如式（6）所示为误差向量；

38、3-4-3）迭代公式（13）的具体求解计算过程如下所示：

39、①的误差公式具体求解如下：

40、（14）

41、其中 、 、如下式（15）所示，可根据式（1）所推导的激光无人叉车的运动学模型可得出：

42、（15）

43、其中（16）

44、②修正量的雅可比矩阵的具体计算如下式（17）所示：

45、（17）

46、式（17）中的各个参数，说明如下：

47、其中（18）

48、（19）

49、  （20）

50、（21）

51、式（21）中的 、 、求解如下式（22）所示：

52、（22）

53、式（22）中的（23）

54、（24）

55、式（24）中的 、 、求解如下式（25）所示：

56、（25）

57、式（25）中的（26）

58、（27）

59、式（27）中的 、 、求解如下式（28）所示：

60、（28）

61、式（28）中的（29）

62、经过上述的迭代公式（13）的多次迭代计算，在迭代计算过程中，当迭代计算的次数达到所设定的最大的迭代次数，或当前的计算结果和上次的计算结果的差值在设定的很小的数值范围内，则迭代过程结束，将迭代结束时的向量中各参数的数值做为最终的激光无人叉车导航参数最终的校准值。

63、本发明提供的一种激光无人叉车的导航参数自动校准系统及方法，解决了现有的激光无人叉车导航相关参数采用手动或半自动方式校准而不是在实际运行过程中自动进行校准的问题，实现了完全的自动化校准，节省了人力和时间，校准结果更加精确。