[发明专利]基于STM32和激光雷达实现汽车姿态测量定位的方法

权利要求书

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

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

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

具体为: 所述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是常数;

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

 S4) 然后将搬运AGV待移动的距离和角度转换成搬运AGV可识别的运动指令，控制AGV进行姿态微调，直至将待搬运汽车中心点和搬运AGV中心点的相对位置关系调整到允许的误差范围内,其特征在于，所述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_Ｂ-L₂cos(θ₂)；

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

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