阅读说明：本技术 基于stm32和激光雷达实现汽车姿态测量定位的方法 (Method for realizing automobile attitude measurement positioning based on STM32 and laser radar ) 是由 张丹丹 张志民 王娟 卢扬扬 于 2019-10-29 设计创作，主要内容包括：本发明属于汽车搬运姿态定位领域,涉及一种可适用于室外汽车搬运的基于STM32和激光雷达实现待搬运汽车姿态快速测量定位的方法,该方法利用STM32车载控制器收到第一激光雷达和第二激光雷达的测量信息后,建立在坐标系下的车体点模型,在坐标系中选取n对左右两侧的有效数据点并进行均值处理,所有均值点组成的点线即为激光雷达扫描车体模型的中心线,通过对这些均值点进行拟合处理即可计算出待搬运汽车中心点与搬运AGV中心点的相对位置关系,进而控制搬运AGV进行姿态调整。本发明具有可快速测量出待搬运汽车的姿态,测量精度高,是实现汽车无人搬运较为重要的一个环节,可大大节约人工成本,提高企业的生产节拍,可节约开发成本。(The invention belongs to the field of automobile carrying attitude positioning, and relates to a method for realizing quick measuring and positioning of the attitude of an automobile to be carried based on STM32 and a laser radar, which is suitable for outdoor automobile carrying. The invention can quickly measure the posture of the automobile to be transported, has high measurement precision, is an important link for realizing unmanned transportation of the automobile, can greatly save labor cost, improves the production rhythm of enterprises, and can save development cost.)

基于STM32和激光雷达实现汽车姿态测量定位的方法

技术领域：

本发明属于汽车搬运姿态定位领域，涉及一种可适用于室外汽车搬运的基于STM32和激光雷达实现待搬运汽车姿态快速测量定位的方法。

背景技术：

目前，在未知环境对汽车姿态进行定位与建图最常用的方法为SLAM算法，但是因为SLAM算法实现起来较为复杂，内存消耗较大，故多在工控机上进行开发，因此没办法在STM32板卡上应用SLAM算法实现对待搬运车辆的扫描与建模。在串口通信方式中，数据一位一位地依次传输，数据传送效率低。

发明内容

本发明公开了一种基于STM32和激光雷达实现汽车姿态测量定位的方法，以解决现有技术的上述以及其他潜在问题中任一问题。

为了达到上述目的，本发明的技术方案是：一种基于STM32和激光雷达实现汽车姿态测量定位的方法,该方法利用STM32车载控制器收到第一激光雷达和第二激光雷达的测量信息后，建立在坐标系下的车体点模型，在坐标系中选取n对左右两侧的有效数据点并进行均值处理，所有均值点组成的点线即为激光雷达扫描车体模型的中心线，最后STM32车载控制器通过对这些均值点进行拟合处理即可计算出待搬运汽车中心点与搬运AGV中心点的相对位置关系，进而控制搬运AGV进行姿态调整。

进一步，所述方法的具体步骤为：

S1)STM32车载控制器与第一激光雷达和第二激光雷达建立通信连接；

S2)第一激光雷达和第二激光雷达将采集测量信息发送给STM32车载控制器，STM32车载控制器通过测量信息建立坐标系下的车体点模型，并对数据进行均值处理；

S3)将所有均值点组成的点线即为车体模型的中心线，根据中心线计算出汽车车体中心点与AGV车体中心点之间的相对位置关系△d和△θ，即搬运AGV待移动的距离和角度；

S4)然后将搬运AGV待移动的距离和角度转换成搬运AGV可识别的运动指令，控制AGV进行姿态微调，直至将待搬运汽车中心点和搬运AGV中心点的相对位置关系调整到允许的误差范围内。

进一步，所述S2)的具体步骤为：

S2.1)建立以第一激光雷达中心点O₁和第二激光雷达中心点O₂连线为X轴，其垂直方向为Y轴，O₁和O₂连线的中心点为原点的坐标系下的点模型，然后对跳变数据进行过滤处理；

S2.2)在所述坐标系中选取n对有效数据点A(x,y)和B(x,y)，并对这n对有效相对应数据A(x,y)点和B(x,y)点进行均值处理，其中，n的取值范围为大于0的正整数，得到均值点C(x,y)，并以所有均值点C(x,y)组成的点线即为激光雷达扫描车体模型的中心线；

S2.3)设A(x,y)在第i个数据点为(x_Ai,y_Ai)；B(x,y)点与A(x,y)在第i个数据点对应的点为(x_Bn+1-i,y_Bn+1-i)，i＝1…n，则均值点C(x,y)＝((x_Ai+x_Bn+1-i)/2,(y_Ai+y_Bn+1-i)/2),对均值点C(x,y)按最小二乘法进行拟合处理成线性直线方程aX+bY+c＝0，其中，a是X的系数,b是Y的系数,c是常数。

进一步，所述S2.1)的具体步骤为：

S2.11)以第一激光雷达和第二激光雷达连线为X轴方向，第一激光雷达和第二激光雷达的连线中心为原点O，垂直方向为Y轴方向，第一激光雷达中心点设为O₁点，第二激光雷达中心点设为O₂点；

S2.12)设第一激光雷达中心点O₁至A(x,y)点的距离为L₁，原点O至第一激光雷达中心点O₁之间的距离为L_A；

角度∠AO₁O＝π-θ₁，π＝180°；

点A(x,y)的x坐标为L₁cos(π-θ₁)-L_A；

点A(x,y)的y坐标为L₁sin(π-θ₁)；

即点A(x,y)为A(L₁cos(π-θ₁)-L_A,L₁sin(π-θ₁))；

S2.13)设第二激光雷达中心点O₂至B(x,y)点的距离为L₂，原点O至第二激光雷达中心点O₂之间的距离为L_B；

角度∠BO₂O＝θ₂；

点B(x,y)的x坐标为L_B-L₂cos(θ₂)；

点B(x,y)的y坐标为L₂sin(θ₂)；

即点B(x,y)为B(L_B-L₂cosθ₂,L₂sin(θ₂))。

进一步，所述S3)的具体步骤为：

S3.1)将拟合出的线性直线方程aX+bY+c＝0与Y轴之间的夹角作为搬运AGV与汽车之间的实际角度进行调整，直到线性直线方程与Y轴之间的夹角为0°为止；

S3.2)在车辆进入搬运AGV之前，即车辆前端Y坐标的值＞0,对车辆前端扫描数据进行线性拟合得到的直线a₁X+b₁Y+c₁＝0与方程aX+bY+c＝0的交叉点D为车辆进入搬运AGV之前的定位点，其中，a₁是X的系数,b₁是Y的系数,c₁是常数；

S3.3)当车辆进入搬运AGV后，即车辆前端Y坐标的值≤0，车辆前端越过坐标系X轴后激光雷达无法扫描车辆前端，即无法得到车辆前端扫描数据，只能扫描到车辆从车头到车尾的两侧车体数据，对扫描到的车辆从车头到车尾的两侧车体所有数据在坐标系中进行X方向和Y方向均值处理，得到车辆中心点D在坐标系中的位置；

S3.4)车辆进入搬运AGV后，搬运AGV的定位坐标点D(x,y)的x坐标为：

定位坐标点D(x,y)的y坐标为:

S3.5)STM32车载控制器算出汽车车体中心点与AGV车体中心点之间的相对位置关系△d和△θ，即搬运AGV待移动的距离和角度。

进一步，所述S1)中STM32车载控制器与激光雷达之间通过LWIP协议栈实现通信连接。

进一步，所述S2)第一激光雷达和第二激光雷达可设置的最大扫描范围为-45°至225°，实际应用中激光雷达的扫描范围可根据具体应用场景进行设置，角度分辨率为0.33°。

一种实现上述的基于STM32和激光雷达实现汽车姿态测量定位的方法的计算机程序。

一种实现上述的基于STM32和激光雷达实现汽车姿态测量定位的方法的信息处理终端。

一种计算机可读存储介质，包括指令，当其在计算机上运行时，使得计算机执行上述的基于STM32和激光雷达实现汽车姿态测量定位的方法。

本发明有益效果是：由于采用上述技术方案，本发明的技术方案可快速测量出待搬运汽车的姿态，测量精度高，是实现汽车无人搬运较为重要的一个环节，可大大节约人工成本，提高企业的生产节拍，同时由于STM32板卡成本远远低于工控机成本，可节约开发成本。

附图说明

图1为本发明第一激光雷达和第二激光雷达测量示意图。

图2为本发明一种基于STM32和激光雷达实现汽车姿态测量定位的方法流程图。

图3为实施例中TIM561激光雷达测量定位示意图。

图4为激光雷达扫描车体数据点模型。

图5为车体扫描建模中心线。

图6为均值点C的拟合直线。

图7为车辆进入AGV前定位方案示意图。

图8为车辆进入AGV后定位方案示意图。

图9为待搬运汽车中心点与搬运AGV中心点的相对位置关系示意图。

图中：

1.第一激光雷达数据点线，2第二激光雷达数据点线，3.拟合直线，4.建模中心线，5.竖直拟合直线，6水平拟合直线，7.第一激光雷达，8.第二激光雷达。

具体实施方式

下面结合附图和具体实施例对本发明的技术方案做进一步说明。

如图2所示，本发明一种基于STM32和激光雷达实现汽车姿态测量定位的方法,该方法利用STM32车载控制器收到第一激光雷达和第二激光雷达的测量信息后，建立在坐标系下的车体点模型，在坐标系中选取n对左右两侧的有效数据点并进行均值处理，所有均值点组成的点线即为激光雷达扫描车体模型的中心线，最后STM32车载控制器通过对这些均值点进行拟合处理即可计算出待搬运汽车中心点与搬运AGV中心点的相对位置关系，进而控制搬运AGV进行姿态调整。

所述方法的具体步骤为：

S1)STM32车载控制器与第一激光雷达和第二激光雷达建立通信连接；

S2)第一激光雷达和第二激光雷达将采集测量信息发送给STM32车载控制器，STM32车载控制器通过测量信息建立坐标系下的车体点模型，并对数据进行均值处理；

S3)将所有均值点组成的点线即为车体模型中心线，根据中心线计算出汽车车体中心点与AGV车体中心点之间的相对位置关系△d和△θ，即搬运AGV待移动的距离和角度；

S4)然后将搬运AGV待移动的距离和角度转换成搬运AGV可识别的运动指令，控制AGV进行姿态微调，直至将待搬运汽车中心点和搬运AGV中心点的相对位置关系调整到允许的误差范围内。

所述S2)的具体步骤为：

S2.1)建立以第一激光雷达中心点O₁和第二激光雷达中心点O₂连线为X轴，其垂直方向为Y轴，O₁和O₂连线的中心点为原点的坐标系下的点模型，然后对跳变数据进行过滤处理；

S2.2)在所述坐标系中选取n对有效数据点A(x,y)和B(x,y)，并对这n对有效数据A(x,y)点和B(x,y)点进行均值处理，其中，n的取值范围为大于0的正整数，得到均值点C(x,y)，并以所有均值点C(x,y)组成的点线即为激光雷达扫描车体模型的中心线；

S2.3)设A(x,y)在第i个数据点为(x_Ai,y_Ai)；B(x,y)点与A(x,y)在第i个数据点为(x_Bn+1-i,y_Bn+1-i)，i＝1…n，则均值点C(x,y)＝((x_Ai+x_Bn+1-i)/2,(y_Ai+y_Bn+1-i)/2),对均值点C(x,y)按最小二乘法进行拟合处理成线性直线方程aX+bY+c＝0，其中，a是X的系数,b是Y的系数,c是常数。

所述S2.1)的具体步骤为：

S2.11)以第一激光雷达和第二激光雷达连线为X轴方向，第一激光雷达和第二激光雷达的连线中心为原点O，垂直方向为Y轴方向，第一激光雷达中心点设为O₁点，第二激光雷达中心点设为O₂点；

S2.12)设第一激光雷达中心点O₁至A(x,y)点的距离为L₁，原点O至第一激光雷达中心点O₁之间的距离为L_A；

角度∠AO₁O＝π-θ₁，π＝180°，θ₁为第一激光雷达的扫描角度；

点A(x,y)的x坐标为L₁cos(π-θ₁)-L_A；

点A(x,y)的y坐标为L₁sin(π-θ₁)；

即点A(x,y)为A(L₁cos(π-θ₁)-L_A,L₁sin(π-θ₁))；

S2.13)设第二激光雷达中心点O₂至B(x,y)点的距离L₂，原点O至第二激光雷达中心点O₂之间的距离为L_B；

角度∠BO₂O＝θ₂；

点B(x,y)的x坐标为L_B-L₂cos(θ₂)；

点B(x,y)的y坐标为L₂sin(θ₂)；

即点B(x,y)为B(L_B-L₂cosθ₂,L₂sin(θ₂))。

所述S3)的具体步骤为：

S3.1)将拟合出的线性直线方程aX+bY+c＝0与Y轴之间的夹角作为搬运AGV与汽车之间的实际角度进行调整，直到线性直线方程与Y轴之间的夹角为0°为止；

S3.2)在车辆进入搬运AGV之前，即车辆前端Y坐标的值＞0,对车辆前端扫描数据进行线性拟合得到的直线a₁X+b₁Y+c₁＝0与方程aX+bY+c＝0的交叉点D为车辆进入搬运AGV之前的定位点，其中，a₁是X的系数,b₁是Y的系数,c₁是常数；

S3.3)当车辆进入搬运AGV后，即车辆前端Y坐标的值≤0，车辆前端越过坐标系X轴后激光雷达无法扫描车辆前端，即无法得到车辆前端扫描数据，只能扫描到车辆从车头到车尾的两侧车体数据，对扫描到的车辆从车头到车尾的两侧车体所有数据在坐标系中进行X方向和Y方向均值处理，得到车辆中心点D在坐标系中的位置；

S3.4)车辆进入搬运AGV后，搬运AGV的定位坐标点D(x,y)的x坐标为：

定位坐标点D(x,y)的y坐标为:

S3.5)STM32车载控制器算出汽车车体中心点与AGV车体中心点之间的相对位置关系△d和△θ，即搬运AGV待移动的距离和角度。

所述S1)中STM32车载控制器与激光雷达之间通过LWIP协议栈实现通信连接。

所述S2)第一激光雷达和第二激光雷达可设置的最大扫描范围为-45°至225°，实际应用中激光雷达的扫描范围可根据具体应用场景进行设置，角度分辨率为0.33°。

实施例：

激光雷达Tim561配置

激光雷达本身自带网口，与其他外设通信基于TCP协议，且为Server端，端口号为2111。Tim561的最大扫描角度为270度，在其配置的上位机软件SOPAS中可以设置实际输出的角度范围。

激光雷达扫描指令：

(1)单次测量指令

ASCII：sRN LMDscandata

HEX:02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 03

Binary:02 02 02 02 00 00 00 0F 73 52 4E 20 4C 4D 44 73 63 61 6E 64 6174 61 05

(2)连续测量指令

ASCII：sEN LMDscandata 1

HEX:02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 31 03

Binary:02 02 02 02 00 00 00 11 73 45 4E 20 4C 4D 44 73 63 61 6E 64 6174 61 20 01 33

(3)停止测量指令

ASCII：sEN LMDscandata 0

HEX:02 73 45 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 30 03

Binary:02 02 02 02 00 00 00 11 73 45 4E 20 4C 4D 44 73 63 61 6E 64 6174 61 20 00 33

激光雷达单次测量数据解析：

sRA LMDscandata 1(设备版本号)1(设备ID)DCA50C(设备序列号)0(DeviceStatus)0 2F49(Telegram Count)2F53(Scan Count)3102CF4B(time since start up,单位us)3103DE91 0 0 1 0 0 5DC(扫描频率)36 0 1 DIST1 3F800000 00000000 C3500(起始角度)2710(角度分辨率)15(测量数据个数)5F8(实际测量点，总共21个点)5F4 5EC 5E75E8 5E8 5DE 5E1 5E3 5E6 5E4 5DC 5DE 5E3 5EC 6C1 6B5 5EA 5F0 60E 60F 0 0 0 0 00

扫描频率：0x5DC＝1500,15Hz

起始角度：0Xc3500＝800000,从80度开始测量

角度分辨率：0x2710＝10000,表示角度分辨率为1度(TIM561默认角度分辨率为0.33度,可在SOPAS软件中进行设置)

测量点数量：0x15＝21(总共21个测量点)

测量距离值(单位mm)：第1个点(80度)0x5F8＝1528(十进制)，表示该距离为1528mm

第2个点(81度)0x5F4＝1524(十进制)，表示该距离为1524mm之后依次类推，本次测量总共21个点

第21个点(100度)0x60F＝1551mm

1. STM32车载控制器端实现

因激光雷达在与其他外设通信的过程中为Server端，故STM32车载控制器为Client端，需要发起与激光雷达建立连接的请求。待激光雷达监听到并接收该连接请求，双方才可完成通信握手。连接建立后，STM32车载控制器发送单次测量指令，激光雷达收到指令并返回测量数据。STM32车载控制器通过对测量数据进行过滤、融合与拟合定位处理，计算出汽车中心点与AGV中心点的相对位置关系，然后转换为AGV的运动控制指令，进而控制AGV进行姿态调整，直至待搬运汽车中心点与搬运AGV中心点的相对位置关系调整到可接受的误差范围内。STM32车载控制器端实现流程如图2所示。

2. TIM561激光测量建模

以搬运AGV上的2个TIM561激光雷达建立坐标系，2个激光雷达连线为X轴方向，2个激光雷达连线中心为原点O，垂直方向为Y轴方向，第一激光雷达中心点设为O₁点，第二激光雷达中心点设为O₂点，如图3所示。

激光雷达从设定的起始角度开始发出激光线束，并以设定的分辨角度为激光间隔，当扫描到车体时，激光雷达会返回具体数值，当没有扫到车体时(超出扫描范围)返回的数值为0，到设定的终止角度时激光雷达扫描停止。

(1)激光雷达的扫描数据及建模

以搬运AGV左侧的第一激光雷达为例，第一激光雷达扫描到车体时，返回的数据为第一激光雷达中心点O₁至A(x,y)点的距离L₁，第一激光雷达扫描角度为θ₁，则车体扫描数据点A(x,y)的坐标：

设第一激光雷达中心点O₁至A(x,y)点的距离L₁，原点O至第一激光雷达中心点O₁之间的距离为L_A；

角度∠AO₁O＝π-θ₁；

点A(x,y)的x坐标为L₁cos(π-θ₁)-L_A；

点A(x,y)的y坐标为L₁sin(π-θ₁)；

即点A(x,y)为A(L₁cos(π-θ₁)-L_A,L₁sin(π-θ₁))。

搬运AGV右侧第二激光雷达扫描到车体时，返回的数据为第二激光雷达中心点O₂至B(x,y)点的距离L₂，第二激光雷达扫描角度为θ₂，原点O至第二激光雷达中心点O₂之间的距离为L_B。

同理，车体扫描数据点B(x,y)的坐标为：B(L_B-L₂cosθ₂,L₂sin(θ₂))。

第一激光雷达扫描的A(x,y)点轨迹和第二激光雷达扫描的B(x,y)点轨迹如图4所示，将第一激光雷达和第二激光雷达每一次扫描的数据点在坐标系中即可建立起车体的轮廓模型。

(2)激光雷达数据点处理

在坐标系中选取n对有效数据点A(x,y)和B(x,y)，并对这n对有效相对应数据A(x,y)点和B(x,y)点进行均值处理，得到均值点C(x,y)，所有均值点C(x,y)组成的点线为激光雷达扫描车体模型的中心线。

设A(x,y)在第i个数据点为(x_Ai,y_Ai)；B(x,y)点与A(x,y)在第i个数据点为(x_Bn+1-i,y_Bn+1-i)，则均值点C(x,y)＝((x_Ai+x_Bn+1-i)/2,(y_Ai+y_Bn+1-i)/2),如图5所示，图5中的中间深红色线为均值点C(x,y)在坐标系中组成的点线。

对均值点C(x,y)按最小二乘法进行拟合处理成线性直线方程aX+bY+c＝0，其中a，b，c为常数，线性直线方程如图6所示。

(3)定位方案

图6中所示的利用最小二乘法拟合出的线性直线方程aX+bY+c＝0，与Y轴之间的夹角为搬运AGV与汽车之间的实际角度，搬运AGV根据线性直线方程与Y轴之间的夹角进行旋转，直到线性直线方程与Y轴之间的夹角为0°为止。

在车辆进入搬运AGV之前(即车辆前端Y坐标的值>0),对车辆前端扫描数据进行线性拟合得到的直线a₁X+b₁Y+c₁＝0(其中a₁，b₁，c₁为常数)与方程aX+bY+c＝0的交叉点D为车辆进入搬运AGV之前的定位点，如图7所示。

当车辆进入搬运AGV后(即车辆前端Y坐标的值≤0)，车辆前端越过坐标系X轴后激光雷达无法扫描车辆前端(即无法得到车辆前端扫描数据)，只能扫描到车辆从车头到车尾的两侧车体数据，如图8所示，红线和蓝线为车辆两侧数据，对扫描到的车辆从车头到车尾的两侧车体所有数据在坐标系中进行X方向和Y方向均值处理，得到车辆中心点D在坐标系中的位置。

车辆进入搬运AGV后，与搬运AGV的定位坐标点D(x,y)的x坐标为：

定位坐标点D(x,y)的y坐标为:

3. AGV姿态调节

STM32车载控制器根据上述算法计算出汽车车体中心点与AGV车体中心点之间的相对位置关系△d和△θ，即搬运AGV待移动的距离(左右移动距离和前后移动距离)和角度，然后将其转换成AGV可识别的运动指令，控制AGV进行姿态微调，直至将两者的相对位置关系调整到允许的误差范围内。待搬运汽车中心点与搬运AGV中心点两者的相对位置关系如图9所示。

本文虽然已经给出了本发明的最优实施例，但是本领域的技术人员应当理解，在不脱离本发明精神的情况下，可以对本文的实施例进行改变。上述实施例只是示例性的，不应以本文的实施例作为本发明权利范围的限定。