基于激光雷达和叉车内外参联合标定方法、装置及系统与流程

本发明涉及激光雷达标定，尤其是基于激光雷达和叉车内外参联合标定方法、装置及系统。

背景技术：

1、越来越多的货物物流仓储中心使用无人叉车代替人工进行货物搬运，所搬运的货物一般为栈板或者料笼。在无人叉车正常执行搬运任务之前，需要使用装在无人叉车顶部的2d激光雷达数据和同步定位与地图构建技术进行地图创建。在无人叉车正常进行搬运工作时，无人叉车会自动行走到待搬运货物之前，将货物叉起并搬运到仓库中的某个指定位置。整个过程需要2d激光雷达、主舵轮和叉臂末端紧密的配合，无人叉车需要依靠激光雷达的数据实现自身位置在地图中的定位，需要依靠车体底盘的主转向舵轮来控制自身实际的行走与转向，需要准确控制叉臂末端的位置才能够使叉臂准确的伸到栈板或者料笼底部孔位完成叉取动作。2d激光雷达、主舵轮和叉臂末端在无人叉车本体上的安装是通过机械结构进行连接，机械加工存在的加工误差和机械装配存在的装配误差使得激光雷达、主舵轮与叉臂末端三者的位置关系与理论设计存在一定的偏差。

技术实现思路

1、本发明提供基于激光雷达和叉车内外参联合标定方法、装置及系统，能实现不借助标定模板等标定工具的条件下对激光雷达、主舵轮与叉臂末端三者内外参的自动联合标定。

2、为了解决上述技术问题，本发明提供了基于激光雷达和叉车内外参联合标定方法、装置及系统，包括以下步骤：

3、控制叉车按照s型或8型路线进行匀速移动，其中，所述叉车安装有激光雷达，获取叉车移动时舵轮数据和激光雷达实时记录的激光数据；

4、构建叉车坐标系和激光雷达坐标系；

5、根据所述叉车移动时舵轮数据和激光雷达坐标系的变换，建立关于叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法对所述超定线性方程组求解获得叉车内参比值；

6、根据所述叉车移动时舵轮数据和所述激光数据，确定叉车变换关系和激光雷达变换关系，并根据所述叉车变换关系和所述激光雷达变换关系，构建误差函数，继而根据所述叉车内参比值优化误差函数获得叉车内参值和激光雷达的外参值；

7、根据所述叉车内参值和所述激光雷达的外参值标定叉车内参和激光雷达的外参。

8、本发明通过提出基于激光雷达和叉车内外参联合标定方法、装置及系统，通过叉车按照s型或8型路线进行匀速移动，在不借助标定模板等标定工具的条件下，通过叉车移动时舵轮数据和激光数据构建叉车坐标系、激光雷达坐标系、叉车变换关系和激光雷达变换关系，通过叉车移动时舵轮数据和激光雷达坐标系进一步建立了叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法计算叉车内参比值，在较短时间内获得准确性搞得内参比值，通过叉车变换关系和激光雷达变换关系进一步构建误差函数，并结合叉车内参比值计算叉车内参值和外参值，用以标定叉车内参和叉车外参，完成无人叉车上2d激光雷达与叉车本体外参的标定。

9、作为优选例子，所述根据所述叉车移动时舵轮数据和激光雷达坐标系的变换，建立关于叉车舵轮与叉臂轮组的超定线性方程组，具体为：

10、根据所述叉车移动时舵轮数据获取叉车运动时舵轮围绕舵轮中心的角速度和舵轮偏航角，根据激光雷达坐标系的变换获取激光雷达坐标系的转角；

11、根据所述叉车运动时舵轮围绕舵轮中心的角速度、舵轮偏航角和激光雷达坐标系的转角构建第一矩阵，并建立有关第一矩阵的超定线性方程组。

12、本优选例子通过叉车运动时舵轮围绕舵轮中心的角速度、舵轮偏航角和激光雷达坐标系的转角建立超定线性方程组，可以用来解决具有极大复杂度的优化问题、甚至是非线性问题，拥有比一般线性方程多出的变量和约束。可解决复杂的叉车运动轨迹优化问题，更好的适应多变的叉车运动轨迹。

13、作为优选例子，所述超定线性方程组形式为ax＝b；

14、其中，所述超定线性方程组具体为：

15、

16、其中，ωs为叉车运动时舵轮围绕舵轮中心的角速度，t为时间，θs为舵轮偏航角，为激光雷达坐标系相邻帧的转角。

17、本优选例子通过叉车运动时舵轮围绕舵轮中心的角速度、叉车坐标系转角、舵轮偏航角和激光雷达坐标系相邻帧的转角建立超定线性方程组，可解决复杂的叉车运动轨迹优化问题，更好的适应多变的叉车运动轨迹。

18、作为优选例子，所述并通过最小二乘法对所述超定线性方程组求解获得叉车内参比值，具体为：

19、通过最小二乘法对所述超定线性方程组进行变形获得变形公式，对所述变形后的公式进行求解，获得叉车内参比值为：

20、

21、其中，δtk为给定时间间隔[tk，tk+1]，为时间间隔[tk，tk+1]内的叉车运动时舵轮围绕舵轮中心的角速度，为时间间隔[tk，tk+1]内的舵轮偏航角，为时间间隔[tk，tk+1]内的激光雷达坐标系相邻帧的转角。

22、本优选例子通过采用最小二乘法对朝顶现行方程组变形并求解，可以达到最小化误差的目的，可以有效求解复杂的非线性方程组是的所有约束条件都能满足，并且能有效求得最优解。

23、作为优选例子，所述变形公式的形式为x＝(ata)-1atb。

24、本优选例子通过采用最小二乘法对朝顶现行方程组变形并求解，可以达到最小化误差的目的，可以有效求解复杂的非线性方程组是的所有约束条件都能满足，并且能有效求得最优解。

25、作为优选例子，所述并根据所述叉车变换关系和所述激光雷达变换关系，构建误差函数，具体为：

26、通过叉车变换关系、激光雷达变换关系构建误差函数；

27、所述误差函数为：

28、

29、其中，为误差值，sk为激光雷达变换关系，rk为叉车变换关系，为激光雷达坐标系在叉车坐标系中外参。

30、本优选例子通过构建误差函数，将内外参标定问题转换为优化问题，求该误差函数的最大值。由于叉车变换关系是基于叉车车体模型和主舵轮角速度积分的理论推导得到的，是包含叉车车体参数的函数，即为叉车车体的内参。在实际应用中，2d激光雷达的数据也会掺杂一定的噪声，对激光雷达变换关系估算也无法获得理想的准确值，因此就构成误差的累积。误差的累积越小，则对应的传感器外参和就越逼近真值。

31、作为优选例子，所述继而根据所述叉车内参比值优化误差函数获得叉车内参值和外参值，具体为：

32、根据所述叉车内参比值对所述误差函数进行优化，转换为对应二次型问题，构造激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数，并从中提取出对应齐次线性方程组；

33、对所述对应齐次线性方程组求解，获得第一解向量和第二解向量；

34、将所述对应齐次线性方程组与所述二次型问题结合，将所述第一解向量和第二解向量分别代入误差函数中，取函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值；

35、根据函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算得叉车内参值和激光雷达的外参值。

36、本优选例子通过构建齐次线性方程，可以用于求解采集叉车运动数据的稀疏矩阵中与特定目标量值最接近的未知变量，也可以用于确定任意叉车运动数据点之间的关系，还可以用来推导任何未知变量的最优解，从而提供了方便和有效的条件。

37、作为优选例子，所述根据所述叉车内参比值对所述误差函数进行优化，转换为对应二次型问题，构造激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数，并从中提取出对应齐次线性方程组，具体为：

38、结合所述叉车内参比将所述误差函数优化为：

39、

40、转换为对应二次型问题其中，所述对应二次型问题为：

41、

42、

43、

44、利用拉格朗日乘法构造激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数，其中，所述激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数为：

45、

46、对所述拉格朗日函数进行变形，并根据的物理含义提取出对应的齐次线性方程组，其中，所述齐次线性方程组为：

47、

48、其中，qk为k时刻二次型问题的构造矩阵，为叉车舵轮到叉臂轮组距离与激光雷达坐标系在叉车坐标系位姿组合的向量，c为与优化变量无关的常数，lx为激光雷达坐标系在车体坐标系x轴下的位置，ly为激光雷达坐标系在车体坐标系y轴下的位置，lθ为激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角；w为矩阵

49、本优选例子通过构建齐次线性方程，可以用于求解采集叉车运动数据的稀疏矩阵中与特定目标量值最接近的未知变量，也可以用于确定任意叉车运动数据点之间的关系，还可以用来推导任何未知变量的最优解，从而提供了方便和有效的条件。

50、作为优选例子，所述对所述拉格朗日函数进行变形，具体为：

51、对所述拉格朗日函数进行求导变形，获得公式：

52、

53、其中，为差值，λ为需求解的根。

54、本优选例子通过对所述拉格朗日函数进行求导变形用以获得对应齐次线性方程组。

55、作为优选例子，所述将所述对应齐次线性方程组与所述二次型问题结合，将所述第一解向量和第二解向量分别代入误差函数中，取函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值，具体为：

56、将所述对应齐次线性方程组与所述二次型问题结合，获得对应公式：

57、

58、将所述第一解向量和第二解向量分别代入误差函数中，取函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值，所述最小的激光雷达坐标系在叉车坐标系下的位置矩阵值为：

59、

60、根据误差函数可获得：

61、

62、其中，为激光雷达坐标系在叉车坐标系下的各相关位置，l为舵轮到叉臂轮组距离，lx是激光雷达坐标系在车体坐标系下的x轴位置，ly是激光雷达坐标系在车体坐标系下的y轴位置，lθ是激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角。

63、本优选例子通过求解误差函数获得激光雷达坐标系在叉车坐标系下的位置矩阵值，以进一步推算叉车内参和外参。

64、作为优选例子，所述根据函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算得叉车内参值和激光雷达的外参值，具体为：

65、根据函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算得叉车内参值和激光雷达的外参值，所述内参值和外参值如下：

66、

67、

68、

69、

70、

71、其中，内参值为舵轮半径rs和舵轮到叉臂轮组距离l，外参值为2d激光雷达在叉车车体坐标系下的位姿其中，lx是激光雷达坐标系在车体坐标系下的x轴位置，ly是激光雷达坐标系在车体坐标系下的y轴位置，lθ是激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角。

72、本优选例子通过最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算叉车内参和外参。

73、本发明还提出了基于激光雷达和叉车内外参联合标定装置，包括获取模块、构建模块、求解模块、计算模块和标定模块；

74、所述获取模块用于控制叉车按照s型或8型路线进行匀速移动，其中，所述叉车安装有激光雷达，获取叉车移动时舵轮数据和激光雷达实时记录的激光数据；

75、所述构建模块用于构建叉车坐标系和激光雷达坐标系；

76、所述求解模块由于根据所述叉车移动时舵轮数据和激光雷达坐标系的变换，建立关于叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法对所述超定线性方程组求解获得叉车内参比值；

77、所述计算模块用于根据所述叉车移动时舵轮数据和所述激光数据，确定叉车变换关系和激光雷达变换关系，并根据所述叉车变换关系和所述激光雷达变换关系，构建误差函数，继而根据所述叉车内参比值优化误差函数获得叉车内参值和激光雷达的外参值；

78、所述标定模块用于根据所述叉车内参值和所述激光雷达的外参值标定叉车内参和激光雷达的外参。

79、本发明通过获取模块令叉车按照s型或8型路线进行匀速移动，在不借助标定模板等标定工具的条件下，获取通过叉车移动时舵轮数据和激光数据，并通过构建模块构建叉车坐标系、激光雷达坐标系、叉车变换关系和激光雷达变换关系，通过叉车移动时舵轮数据和激光雷达坐标系进一步建立了叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法计算叉车内参比值，在较短时间内获得准确性搞得内参比值，通过计算模块计算叉车变换关系和激光雷达变换关系进一步构建误差函数，并结合叉车内参比值计算叉车内参值和外参值，用以标定叉车内参和叉车外参，最终通过标定模块完成无人叉车上2d激光雷达与叉车本体外参的标定。

80、作为优选例子，所述求解模块包括第一单元和第二单元；

81、所述第一单元用于根据所述叉车移动时舵轮数据获取叉车运动时舵轮围绕舵轮中心的角速度和舵轮偏航角，根据激光雷达坐标系的变换获取激光雷达坐标系的转角；

82、所述第二单元用于根据所述叉车运动时舵轮围绕舵轮中心的角速度、叉车坐标系转角、舵轮偏航角和激光雷达坐标系的转角构建第一矩阵，并建立有关第一矩阵的超定线性方程组。

83、本优选例子通过叉车运动时舵轮围绕舵轮中心的角速度、叉车坐标系转角、舵轮偏航角和激光雷达坐标系的转角建立超定线性方程组，可以用来解决具有极大复杂度的优化问题、甚至是非线性问题，拥有比一般线性方程多出的变量和约束。可解决复杂的叉车运动轨迹优化问题，更好的适应多变的叉车运动轨迹。

84、作为优选例子，所述超定线性方程组形式为ax＝b；

85、其中，所述超定线性方程组具体为：

86、

87、其中，ωs为叉车运动时舵轮围绕舵轮中心的角速度，t为时间，θs为舵轮偏航角，为激光雷达坐标系相邻帧的转角。

88、本优选例子通过叉车运动时舵轮围绕舵轮中心的角速度、叉车坐标系转角、舵轮偏航角和激光雷达坐标系的转角建立超定线性方程组，可解决复杂的叉车运动轨迹优化问题，更好的适应多变的叉车运动轨迹。

89、作为优选例子，所述求解模块还包括第三单元和第四单元；

90、所述第三单元用于通过最小二乘法对所述超定线性方程组进行变形获得变形公式，对所述变形后的公式进行求解，获得叉车内参比值为：

91、

92、其中，δtk为给定时间间隔[tk，tk+1]，为时间间隔[tk，tk+1]内的叉车运动时舵轮围绕舵轮中心的角速度，为时间间隔[tk，tk+1]内的舵轮偏航角，为时间间隔[tk，tk+1]内的激光雷达坐标系相邻帧的转角。

93、本优选例子通过采用最小二乘法对朝顶现行方程组变形并求解，可以达到最小化误差的目的，可以有效求解复杂的非线性方程组是的所有约束条件都能满足，并且能有效求得最优解。

94、作为优选例子，所述变形公式的形式为x＝(ata)-1atb。

95、本优选例子通过采用最小二乘法对朝顶现行方程组变形并求解，可以达到最小化误差的目的，可以有效求解复杂的非线性方程组是的所有约束条件都能满足，并且能有效求得最优解。

96、作为优选例子，所述计算模块包括第五单元；

97、所述第五单元用于通过叉车变换关系、激光雷达变换关系构建误差函数；

98、所述误差函数为：

99、

100、其中，为误差值，sk为激光雷达变换关系，rk为叉车变换关系，为激光雷达坐标系在叉车坐标系中外参。

101、本优选例子通过构建误差函数，将内外参标定问题转换为优化问题，求该误差函数的最大值。由于叉车变换关系是基于叉车车体模型和主舵轮角速度积分的理论推导得到的，是包含叉车车体参数的函数，即为叉车车体的内参。在实际应用中，2d激光雷达的数据也会掺杂一定的噪声，对激光雷达变换关系估算也无法获得理想的准确值，因此就构成误差的累积。误差的累积越小，则对应的传感器外参和就越逼近真值。

102、作为优选例子，所述计算模块还包括第六单元、第七单元、第八单元和第九单元；

103、所述第六单元用于根据所述叉车内参比值对所述误差函数进行优化，转换为对应二次型问题，构造激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数，并从中提取出对应齐次线性方程组；

104、所述第七单元用于对所述对应齐次线性方程组求解，获得第一解向量和第二解向量；

105、所述第八单元用于将所述对应齐次线性方程组与所述二次型问题结合，将所述第一解向量和第二解向量分别代入误差函数中，取函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值；

106、所述第九单元用于根据函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算得叉车内参值和激光雷达的外参值。

107、本优选例子通过构建齐次线性方程，可以用于求解采集叉车运动数据的稀疏矩阵中与特定目标量值最接近的未知变量，也可以用于确定任意叉车运动数据点之间的关系，还可以用来推导任何未知变量的最优解，从而提供了方便和有效的条件。

108、作为优选例子，所述第六单元包括第一子单元、第二子单元、第三子单元和第四子单元；

109、所述第一子单元用于结合所述叉车内参比将所述误差函数优化为：

110、

111、所述第二子单元用于转换为对应二次型问题其中，所述对应二次型问题为：

112、

113、

114、

115、所述第三子单元用于利用拉格朗日乘法构造激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数，其中，所述激光雷达坐标系在叉车坐标系下的位置矩阵的拉格朗日函数为：

116、

117、所述第四子单元用于对所述拉格朗日函数进行变形，并根据的物理含义提取出对应的齐次线性方程组，其中，所述齐次线性方程组为：

118、

119、其中，qk为k时刻二次型问题的构造矩阵，为叉车舵轮到叉臂轮组距离与激光雷达坐标系在叉车坐标系位姿组合的向量，c为与优化变量无关的常数，lx为激光雷达坐标系在车体坐标系x轴下的位置，ly为激光雷达坐标系在车体坐标系y轴下的位置，lθ为激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角；w为矩阵

120、本优选例子通过构建齐次线性方程，可以用于求解采集叉车运动数据的稀疏矩阵中与特定目标量值最接近的未知变量，也可以用于确定任意叉车运动数据点之间的关系，还可以用来推导任何未知变量的最优解，从而提供了方便和有效的条件。

121、作为优选例子，所述第四子单元包括第一子模块；

122、所述第一子模块用于对所述拉格朗日函数进行求导变形，获得公式：

123、

124、其中，为差值，λ为需求解的根。

125、本优选例子通过对所述拉格朗日函数进行求导变形用以获得对应齐次线性方程组。

126、作为优选例子，所述第八单元包括第五子单元、第六子单元和第七子单元；

127、所述第五子单元用于将所述对应齐次线性方程组与所述二次型问题结合，获得对应公式：

128、

129、所述第六子单元用于将所述第一解向量和第二解向量分别代入误差函数中，取函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值，所述最小的激光雷达坐标系在叉车坐标系下的位置矩阵值为：

130、

131、第七子单元用于根据误差函数可获得：

132、

133、其中，为激光雷达坐标系在叉车坐标系下的各相关位置，l为舵轮到叉臂轮组距离，lx是激光雷达坐标系在车体坐标系下的x轴位置，ly是激光雷达坐标系在车体坐标系下的y轴位置，lθ是激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角。

134、本优选例子通过求解误差函数获得激光雷达坐标系在叉车坐标系下的位置矩阵值，以进一步推算叉车内参和外参。

135、作为优选例子，所述第九单元包括第八子单元；

136、所述第八子单元用于根据函数值最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算得叉车内参值和激光雷达的外参值，所述内参值和外参值如下：

137、

138、

139、

140、

141、

142、其中，内参值为舵轮半径rs和舵轮到叉臂轮组距离l，外参值为2d激光雷达在叉车车体坐标系下的位姿l，其中，lx是激光雷达坐标系在车体坐标系下的x轴位置，ly是激光雷达坐标系在车体坐标系下的y轴位置，lθ是激光雷达坐标系x轴与叉车坐标系x轴以逆时针旋转为正的夹角。

143、本优选例子通过最小的激光雷达坐标系在叉车坐标系下的位置矩阵值计算叉车内参和外参。

144、本发明还提出了基于激光雷达和叉车内外参联合标定系统，包括叉车、激光雷达和计算设备；

145、所述叉车用于承载激光雷达并按照s型或8型路线进行匀速移动；

146、所述激光雷达用于发射激光并实时记录激光数据；

147、所述计算设备用于执行上述基于激光雷达和叉车内外参联合标定方法。

148、本优选例子通过叉车按照s型或8型路线进行匀速移动；激光雷达发射激光并实时记录激光数据；在不借助标定模板等标定工具的条件下，计算设备通过叉车移动时舵轮数据和激光数据构建叉车坐标系、激光雷达坐标系、叉车变换关系和激光雷达变换关系，通过叉车坐标系和激光雷达坐标系进一步建立了叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法计算叉车内参比值，在较短时间内获得准确性搞得内参比值，通过叉车变换关系和激光雷达变换关系进一步构建误差函数，并结合叉车内参比值计算叉车内参值和外参值，用以标定叉车内参和叉车外参，完成无人叉车上2d激光雷达与叉车本体外参的标定。

149、本发明提出基于激光雷达和叉车内外参联合标定方法、装置及系统，通过令叉车按照s型或8型路线进行匀速移动，在不借助标定模板等标定工具的条件下，获取通过叉车移动时舵轮数据和激光数据，并构建叉车坐标系、激光雷达坐标系、叉车变换关系和激光雷达变换关系，对叉车坐标系和激光雷达坐标系进一步建立了叉车舵轮与叉臂轮组的超定线性方程组，并通过最小二乘法计算叉车内参比值，在较短时间内获得准确性搞得内参比值，计算叉车变换关系和激光雷达变换关系进一步构建误差函数，并结合叉车内参比值计算叉车内参值和外参值，用以标定叉车内参和叉车外参，最终完成无人叉车上2d激光雷达与叉车本体外参的标定。