航迹推算定位方法及自动泊车系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN110307850A(43)申请公布日2019.10.08(21)申请号201910711364.3(22)申请日2019.08.02(71)申请人湖南海迅自动化技术有限公司地址410000湖南省长沙市开福区青竹湖街道青竹湖路118号金卓产业园8号小型仪表装配厂房第三层301房(72)发明人罗世彬韩松来胡茂青(51)Int.Cl.G01C21/20(2006.01)G01C21/34(2006.01)G01C1/00(2006.01)G01S15/08(2006.01)B60W30/06(2006.01)权利要求书4页说明书10页附图3页(54)发明名称航迹推算定位方法及自动泊车系统(57)摘要本发明提供了一种基于超声波传感器和轮速传感器的航迹推算定位方法及自动泊车系统，以解决室内移动机器人、辅助泊车系统在车辆低速运动状态下，采用惯性导航设备测量车辆行驶状态比较困难，利用超声波传感器确定泊车空间及泊车空间坐标系，使用轮速传感器的数据，采用航迹推算算法，定位车辆在泊车空间坐标系中的初始位姿和泊车过程中的车辆位姿；在移动机器人领域当中，可用于移动机器人自主对充电桩位置的探索。CN110307850ACN110307850A权利要求书1/4页1.基于超声波传感器和轮速传感器的航迹推算定位方法，其特征在于，包括步骤：S1.启动泊车系统，获取超声波传感器数据和轮速传感器数据，建立泊车坐标系；S2.进行泊车位空间尺寸计算，并判断是否满足平行泊车位或垂直泊车位空间尺寸标准，倘若是，进入步骤S3，否则重新进行泊车位空间尺寸计算；S3.进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，或者获取左右前轮轮速传感器及方向盘转角信息，并计算后轮轴中心点运动轨迹。2.根据权利要求1所述的航迹推算定位方法，其特征在于，步骤S2中平行泊车位空间尺寸标准的确定方法如下：根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的平行泊车最小泊车位空间尺寸为：a×b，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：c×d；为了提高道路两侧车位的利用率，当车位满足a×(b-0.2)或c×(d-0.2)时，认为是有效车位；设定目标车位长度为：lco，宽度为：lko，实测车位长度为：Ic；根据车位长度确定路径规划方法，如果实测车位长度lc≥a时，泊车系统采用双圆弧叠加进行轨迹规划，即lco＝a，lko＝b；如果实测车位长度c≤lc≤a时，泊车系统采用B样条进行轨迹规划，即lco＝c，lko＝d；以上两种泊车方式都采用以前车为车辆停放基准；步骤S2中垂直泊车位空间尺寸标准的确定方法如下：设定目标车位长度为：lco，宽度为：lko；根据自动泊车系统的路径规划分析与推算，采用双圆弧叠加进行轨迹规划的垂直泊车最小泊车位空间尺寸为：e×f，采用B样条进行轨迹规划的平行泊车最小泊车位空间尺寸为：g×h；为了提高道路两侧车位的利用率，当车位满足(e-0.2)×f或(g-0.2)×h时，认为是有效车位。3.根据权利要求1所述的航迹推算定位方法，其特征在于，步骤S3包括如下步骤：进行泊车车辆初始位置计算，根据数据异常检测算法检测前轮数据，若前轮数据正常，则用前轮数据计算后轮轴中心点运动轨迹；若前轮数据异常，则用后轮数据计算后轮轴中心点运动轨迹。4.根据权利要求3所述的航迹推算定位方法，其特征在于，步骤S3中采用后轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右后轮轮速传感器数据，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。5.根据权利要求4所述的航迹推算定位方法，其特征在于，由CAN总线得到后轮左右两轮的轮速传感器给出的车轮转动距离ΔsrL和ΔsrR，对于后轮中心Mr，其瞬时速度为vr，满足如下公式：其中航向角θ为：为了简化计算，设后轮中心处Mr的速度vr为左右后轮瞬时速度的平均值，即：2CN110307850A权利要求书2/4页将(3)式代入(1)式，合并(2)式，整理得：式中w为后轮轴轮距。6.根据权利要求3所述的航迹推算定位方法，其特征在于，步骤S3中采用前轮数据计算后轮轴中心点运动轨迹的具体方法是：进行泊车车辆初始位置计算，获取左右前轮轮速传感器及方向盘转角信息，计算前轮中心转向角计算前轮轴中心点速度vf，推算后轮轴中心点速度vr，推算车辆航向角θ，计算后轮轴中心点运动轨迹(xr(t),yr(t))。7.根据权利要求6所述的航迹推算定位方法，其特征在于，由CAN总线得到前轮左右两轮的轮速传感器给出的车轮转动距离ΔsfL和ΔsfR，方向盘转向角为φ可知，由方向盘转向角为φ得前轮中心转向角其中k为方向盘转角