[发明专利]一种自动对准充电桩的控制方法和装置

权利要求书

1.一种自动对准充电桩的控制方法，其特征在于，包括：

获取充电对象位姿和充电桩插口位姿，判断充电对象位姿是否满足预设条件阈值，包括：通过坐标转换得到在以充电桩插口位姿为原点的坐标系中充电对象的相对坐标和充电桩插口的相对坐标，在以充电桩插口位姿为原点的坐标系X正半轴上取参考坐标点，通过坐标转换得到在以参考坐标点为原点的坐标系中充电对象的第二相对坐标，判断充电对象的第二相对坐标的纵向坐标值的绝对值是否大于或等于纵向坐标容忍度；

若不满足则判断充电对象位姿和充电桩插口位姿的横向坐标差值是否大于或等于横向坐标容忍度，若是则控制充电对象以第一线速度和第一旋转速度运动，进而使充电对象对准充电桩插口；否则确定充电对象对准充电桩插口；

若满足则控制充电对象以第二线速度和第二旋转速度运动，进而使充电对象对准充电桩插口。

2.根据权利要求1所述的方法，其特征在于，获取充电对象位姿和充电桩插口位姿，判断充电对象位姿是否满足预设条件阈值，包括：

获取充电对象和充电桩插口在地图坐标系下的位姿，通过坐标转换得到在以充电桩插口位姿为原点的坐标系中充电对象的相对坐标和充电桩插口的相对坐标；

根据充电对象的相对坐标，获取标志位信息，判断所述标志位信息是否满足预设条件阈值。

3.根据权利要求2所述的方法，其特征在于，通过坐标转换得到在以充电桩插口位姿为原点的坐标系中充电对象的相对坐标和充电桩插口的相对坐标之后，包括：

在以充电桩插口位姿为原点的坐标系X正半轴上取参考坐标点，通过坐标转换得到在以参考坐标点为原点的坐标系中充电对象的第二相对坐标；

将充电对象的第二相对坐标的纵向坐标值或者角度作为标志位信息；

判断纵向坐标值的绝对值是否大于或等于纵向坐标容忍度，或者角度的绝对值是否大于或等于角度容忍度，若是则标志位信息满足预设条件阈值，否则不满足预设条件阈值。

4.根据权利要求3所述的方法，其特征在于，所述参考坐标点与充电桩位姿的横向坐标差值为预设数值，其中所述预设数值大于充电对象半径且调整半径为预设数值与预设系数的乘积，预设系数为大于或等于1。

5.根据权利要求3所述的方法，其特征在于，获取充电对象位姿和充电桩插口位姿之前，包括：

当充电对象的第二相对坐标的横向坐标值大于零且角度的绝对值小于二分之π时，设置充电对象的初始线速度为负的预设常数线速度；

根据充电对象的第二相对坐标的纵向坐标值和角度，以及负的预设常数线速度、预设的第一系数和第二系数，设置充电对象的初始旋转速度，即：

其中，w₁为初始旋转速度，v₁为负的预设常数线速度，k₁为第一系数，k₂为第二系数，θ_rs为充电对象的第二相对坐标的角度，y_rs为充电对象的第二相对坐标的纵向坐标值。

6.根据权利要求3所述的方法，其特征在于，控制充电对象的第一线速度和第一旋转速度，包括：

根据充电对象位姿和充电桩插口位姿的横向坐标差值和预设的第三系数，获得充电对象的第一线速度；

根据充电对象的第二相对坐标的纵向坐标值和角度，以及充电对象的第一线速度、预设的第一系数和第二系数，获得充电对象的第一旋转速度，即：

v₂＝-k₃·x_rc

其中，w₂为第一旋转速度，v₂为充电对象的第一线速度，k₁为第一系数，k₂为第二系数，θ_rs为充电对象的第二相对坐标的角度，y_rs为充电对象的第二相对坐标的纵向坐标值，x_rc为充电对象位姿和充电桩插口位姿的横向坐标差值，k₃为第三系数。