The problem is that after uploading a complete map to the indoor positioning robot, the car cannot identify its position in the currently uploaded map.
Solution (four steps):
1. Record the robot's current position (Pre_Position) and two significant target positions (Pre_GoalPosition1 and Pre_GoalPosition2) in the original map;
2. Record the two significant target positions (Current_GoalPosition1 and Current_GoalPosition2) corresponding to the first step in the current map;
3. Calculate the translation (Map_Translation) and rotation angle (Map_AngleRotation) between the original map and the current map.
(1) Calculation method of translation amount (Map_Translation): Map_Translation = Current_GoalPosition1 - Pre_GoalPosition1;
(2) Calculate the rotation angle
First find the two vector values of the two maps: Pre_vector and Current_vector (Pre_vector = Pre_GoalPosition2 -Pre_GoalPosition1; Current_vector = Current_GoalPosition2 -Current_GoalPosition1)
Then calculate the angle between the two vectors Pre_vector and Current_vector, using the formula
cos=a*b/[|a|*|b|]
=(x1x2+y1y2)/[√[x1^2+y1^2]*√[x2^2+y2^2]]
4. Coordinate system transformation
(1) Principle of coordinate system transformation
We start from the plane rectangular coordinate system. The relationship between the coordinate system O-XY and the coordinate system O'-X'Y' is shown in the following figure:
For the above coordinate relationship, we can describe it as follows: the coordinate system O-XY rotates theta counterclockwise (or clockwise -theta) and coincides with the coordinate system O'-X'Y', or the coordinate system O'-X'Y 'Rotate theta clockwise (or -theta counterclockwise) to coincide with the coordinate system O-XY. Using the simple cosine theorem and the Zhengxuan theorem of a simple triangle, the following formula can be obtained (the format editor is not very familiar, so it is replaced by an image):
(2), coordinate transformation method
The position vector Current_Position_vector of the current map can be calculated by Pre_Position_vector, where Pre_Position_vector = Pre_Position - Pre_GoalPosition1; Current_Position_vector = Current_Position - Current_GoalPosition1.