V2X indoor simulation platform based on mobile robots

Abstract

The present invention discloses a V2X indoor simulation platform based on a mobile robot in the field of intelligent transportation technology, comprising an input end, a V2X vehicle-mounted device end and a display terminal; the input end mainly includes the robot's information processing process; during operation, the robot will broadcast TF transformations between coordinate systems and topics related to transmission speed. By monitoring the TF transformations between the map coordinate system and the base_link coordinate system and subscribing to the / cmd_vel topic, the robot can obtain data information such as real-time coordinates, yaw angles and speeds in the map it has built; the data information is then filtered and calibrated, converted into a GNSS data type, and sent to the V2X device via network communication. Under the premise of realizing V2X safety scenario applications, the complexity of application testing can be reduced, the efficiency of program development can be improved, and the potential safety risks of vehicle road testing can be avoided.

Description

Technical Field

[0001] The present invention relates to the field of intelligent transportation technology, and in particular to a V2X indoor simulation platform based on a mobile robot. Background Art

[0002] In recent years, with the continuous improvement of my country's economic level, the number of cars has also increased rapidly, resulting in the frequent occurrence of traffic accidents. How to effectively prevent traffic accidents has always been a key research area in the transportation field both domestically and internationally. V2X technology, as the core technology of the Internet of Vehicles, can provide users with a safe, comfortable, and efficient driving experience. This technology is based on interconnected communications between V2V (Vehicle to Vehicle), V2P (Vehicle to Pedestrian), V2I (Vehicle to Infrastructure), and V2N (Vehicle to Network). It can share information between vehicles, expand the vehicle's perception range, and issue timely warnings.

[0003] However, applications based on V2X technology typically utilize software simulation platforms to build simulated scenarios for program testing, followed by real-world vehicle testing. However, software simulation platforms are limited by pre-set parameters, cannot guarantee actual program performance, and lack intuitiveness. Furthermore, real-world vehicle verification scenarios are complex to set up, resulting in high testing costs, and the site is susceptible to interference, leading to low testing efficiency. Furthermore, complex scenario testing or testing under extreme operating conditions increases the risk factor.

[0004] Based on this, the present invention designs a V2X indoor simulation platform based on a mobile robot to solve the above problems. Summary of the Invention

[0005] The purpose of the present invention is to provide a V2X indoor simulation platform based on a mobile robot to solve the problems raised in the above background technology.

[0006] To achieve the above objectives, the present invention provides the following technical solutions: a V2X indoor simulation platform based on a mobile robot, comprising an input terminal, a V2X vehicle-mounted device terminal, and a display terminal;

[0007] The input side primarily involves the robot's information processing. During operation, the robot broadcasts topics related to the TF transformation between coordinate systems and speed. By monitoring the TF transformation between the map coordinate system and the base_link coordinate system and subscribing to the / cmd_vel topic, the robot can obtain real-time coordinates, yaw angle, speed, and other data in its own map. This data is then filtered and calibrated, converted to GNSS data, and sent to the V2X device via network communication.

[0008] The V2X vehicle-mounted device is the core unit of the system. It receives information from the robot system through network communication and processes it into its own BSM information. It also receives information from other robot systems through LTE-V and processes it into other robot BSM information. The data processing unit built into the V2X device calculates the positional relationship between the two robots based on their BSM information and executes corresponding danger warnings based on the built-in warning algorithm. If danger occurs, the warning information will be sent to the display terminal.

[0009] On the one hand, the display terminal receives the BSM information of the robot and other robots that are at risk to the robot through Wi-Fi, and displays the positions of the robot and other robots on the map in real time; on the other hand, it displays the warning information sent by the V2X device in real time.

[0010] Preferably, after the xy coordinates and yaw angle information of the robot are acquired, they are converted into GNSS latitude and longitude and direction angle information corresponding to the vehicle respectively.

[0011] Preferably, the XY coordinates are converted to GNSS longitude and latitude

[0012] The haversine equation is used to calculate the distance between two longitude and latitude points. The specific formula is:

[0013]

[0014] in

[0015] Where d is the distance between two points on the great circle, R is the radius of the earth, Δσ is the angle between the two points and the line at the center of the sphere, lat1 and lat2 are the latitudes of the two points, and lon1 and lon2 are the longitudes of the two points. If the longitude and latitude coordinates of any two points on the earth are known, the distance between the two points can be calculated. Assume that the longitude and latitude coordinates of point A are The longitude and latitude coordinates of point B are Since the longitude and latitude in formula (1) are calculated in radians, it is necessary to convert the angles into radians. The specific formula is as follows:

[0016]

[0017]

[0018] When A and B are at the same latitude and the longitude angle difference is 0.00001°, it can be seen from formula (2), formula (3) and formula (1) that the distance d between points A and B is approximately equal to 1m. Similarly, the distance between two points with various angle differences under other conditions can also be calculated.

[0019] In a typical robot map, the xy coordinate range is x(-a, a) and y(-b, b), where a and b are both double types. The unit length is 1. After data comparison, the unit length is set to correspond to 0.0001°, and the conversion distance is 10m. Therefore, the first four digits of the coordinate value are sufficient. The specific conversion formula is as follows:

[0020] lon=(base_lon*10 7 +x*1000) / 10 7 (4)

[0021] lat=(base_lat*10 7 +y*1000) / 10 7 (5)

[0022] In formulas (4) and (5), lon represents longitude, lat represents latitude, x represents the robot's x-coordinate, and y represents the robot's y-coordinate. base_lon and base_lat both represent base values, which are the GNSS longitude and latitude coordinates of any point in the area specified for the V2X test.

[0023] Preferably, the azimuth angle information required by the V2X vehicle-mounted device is the angle between the vehicle's driving direction and the north direction parsed from the GNSS data, and increases from 0° to 359.999° in a clockwise direction.

[0024] Preferably, the yaw angle of the robot rotates around the z-axis in radians; when the robot's movement direction coincides with the positive x-axis, the robot's yaw angle at this time is 0 rad; when it coincides with the negative x-axis, the robot's yaw angle is π rad; when it coincides with the positive y-axis, the robot's yaw angle is (π / 2) rad; when it coincides with the negative y-axis, the robot's yaw angle is -(π / 2) rad, and the Y-axis of the yaw angle coordinate system is mapped to the N direction in the azimuth angle coordinate system, and the X-axis is mapped to the E direction in the azimuth angle coordinate system.

[0025] Preferably, the XY coordinates in the yaw angle coordinate system are classified according to quadrants and mathematically deduced to obtain the following formula:

[0026] In the second quadrant:

[0027] in

[0028] In the first, third, and fourth quadrants:

[0029] Where Yaw is other value. (7)

[0030] In formulas (6) and (7), Heading represents the direction angle value, and Yaw represents the yaw angle in radians.

[0031] Preferably, the robot's built-in data processing unit mainly obtains the coordinates, yaw angle, speed and other data information of the mobile robot by monitoring TF transformation and subscribing to speed topics, and performs filtering, calibration, conversion and other processing on it, and then sends it to the V2X vehicle-mounted device through the TCP / IP protocol.

[0032] Preferably, when the program is running, the following steps are performed:

[0033] S1 subscribes to the robot1 / cmd_vel and robot2 / cmd_vel topics of the two mobile robot platforms respectively, and defines listeners and variables for storing coordinate transformations;

[0034] S2 creates a socket service communication and waits for the client to connect;

[0035] S3 can obtain the robot's coordinates and yaw angle in the map in real time by monitoring the coordinate conversion between "map" and "base_link". At the same time, it can obtain the speed by subscribing to the / cmd_vel topic.

[0036] S4 performs filtering and calibration on the acquired data;

[0037] After processing, S5 converts the robot coordinate values ​​into GNSS longitude and latitude, and converts the yaw angle into the direction angle;

[0038] S6 stores the processed data into the BUF buffer as an integer type, waiting to be sent;

[0039] S7 uses TCP / IP to send the data in BUF to the V2X vehicle-mounted device.

[0040] Preferably, a function package of a data processing module is created for the TCP / IP communication server side, and acquires the coordinates, speed, yaw angle and other data of the two mobile robots. After processing, the data is converted into GNSS data type, and then sent to the V2X vehicle-mounted device via TCP / IP through the switch. The specific steps of the test platform are as follows:

[0041] S1 runs the GAZEBO simulation environment and RVIZ to display the robot's perceived environmental information and prepare for subsequent autonomous navigation;

[0042] S2 runs the robot's built-in data processing module software, establishes a socket server, and obtains the robot's coordinates, speed, yaw angle and other information in real time. After processing, it sends it to the V2X vehicle-mounted device via TCP / IP.

[0043] S3 runs the relevant programs in the V2X vehicle-mounted device, establishes a connection with the robot simulation platform socket server, and receives data sent by the robot simulation platform;

[0044] S4 runs the display terminal program and receives the robot's BSM information sent by the V2X vehicle terminal via Wi-Fi. The display terminal will display the robot's location on the road test map;

[0045] S5 runs the robot motion control software, causing the two robots to navigate to multiple target points, one in front of the other, and move in a rectangular clockwise direction around the map, simulating a collision scenario;

[0046] The two S6 V2X onboard devices exchange information via the built-in LTE-V module, receiving each other's BSM information from the other robot. When Robot2 is directly in front of Robo1, Robot1's speed is greater than Robot2's, and the calculated collision time is less than 5 seconds, Robot1 will trigger a forward collision warning. As the collision time decreases to less than 3 seconds, a severe forward collision risk will be triggered.

[0047] The S7 warning information is sent to Robotl's display terminal via the Wi-Fi module built into the V2X vehicle-mounted device.

[0048] Compared with the existing technology, the beneficial effects of the present invention are: it can reduce the complexity of application testing, improve the efficiency of program development, and avoid the potential safety risks of vehicle road testing under the premise of realizing V2X safety scenario application.

[0049] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for describing the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0051] Figure 1 This is a block diagram of the overall architecture of the V2X system based on a mobile robot in the present invention;

[0052] Figure 2 Schematic diagram of the distance between two points A and B of the great circle of the present invention;

[0053] Figure 3 It is the robot operation circuit diagram in the simulation experimental environment of the present invention;

[0054] Figure 4 Schematic diagram of the coordinate conversion effect of the present invention;

[0055] Figure 5 Schematic diagram of the vehicle driving direction angle of the present invention;

[0056] Figure 6 Schematic diagram of the coordinate mapping relationship of the present invention;

[0057] Figure 7 This is a flowchart of the built-in data processing module program of the robot of the present invention;

[0058] Figure 8 This is a topological diagram of the test platform of the present invention;

[0059] Figure 9 This is a principle block diagram of the V2X forward collision scenario of the present invention. DETAILED DESCRIPTION

[0060] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative efforts shall fall within the scope of protection of the present invention.

[0061] See also Figures 1 to 9 The present invention provides a technical solution for a V2X indoor simulation platform based on a mobile robot. The system mainly includes three parts: an input terminal, a V2X vehicle-mounted device terminal, and a display terminal. The specific functions are as follows:

[0062] (1) The input end mainly includes the robot's information processing process. During operation, the robot will broadcast the TF transformation between coordinate systems and send speed-related topics. By monitoring the TF transformation between the map coordinate system and the base link coordinate system and subscribing to the / cmd vel topic, the robot can obtain real-time coordinates, yaw angle, speed and other data information in the map it has built. The data information is then filtered and calibrated, converted into a GNSS data type, and sent to the V2X device through network communication.

[0063] (2) The V2X device, as the core unit of the system, receives information sent by the robot system through network communication and processes it into the BSM information of the robot. At the same time, it receives information from other robot systems through LTE-V and processes it into the BSM information of other robots. The data processing unit built into the V2X device will calculate the positional relationship between the robots based on the BSM information of the robot and other robots, and execute the corresponding danger warning according to the built-in warning algorithm. If a danger occurs, a warning message will be sent to the display terminal;

[0064] (3) The display terminal receives the BSM information of the robot and other robots that pose a risk to the robot through Wi-Fi, and displays the positions of the robot and other robots on the map in real time. On the other hand, it also displays the warning information sent by the V2X device in real time.

[0065] During actual operation, V2X devices require real-time access to key information such as the vehicle's latitude, longitude, heading, and speed. The mobile robot's position information is stored as xy coordinates in a map coordinate system, while its heading information is stored as a yaw angle, using the map coordinate system as a reference. Due to the V2X device's input data requirements, the robot's xy coordinates and yaw angle information must be acquired and then converted into the vehicle's corresponding GNSS latitude, longitude, and heading information, respectively.

[0066] Therefore, in order to realize the mobile robot replacing both the vehicle and the GNSS module, the key technology lies in converting the XY coordinates into GNSS longitude and latitude and converting the yaw angle into the heading angle;

[0067] The haversine equation is used to calculate the distance between two longitude and latitude points. The specific formula is:

[0068]

[0069] in

[0070] Where d is the distance between two points on the great circle, R is the radius of the earth, and Δσ is the angle between the two points and the line to the center of the sphere. lat1, lat2 are the latitudes of the two points, and lon1, lon2 are the longitudes of the two points. Figure 9 As shown, this is a schematic diagram of the distance between two points A and B in the great circle;

[0071] Therefore, if we know the longitude and latitude coordinates of any two points on the earth, we can find the distance between the two points. The longitude and latitude coordinates of point B are Since the longitude and latitude in formula (1) are calculated in radians, it is necessary to convert the angles into radians. The specific formula is as follows:

[0072]

[0073]

[0074] When A and B are at the same latitude and their longitude angles differ by 0.00001°, it can be seen from formula (2), formula (3) and formula (1) that the distance d between points A and B is approximately 1m.

[0075] Similarly, the distance between two points with various angle differences under other conditions can be calculated;

[0076] Therefore, the corresponding relationship between the longitude and latitude differences in Table 1 can be obtained from the above:

[0077] Table 1 Correspondence between longitude and latitude differences

[0078]

[0079]

[0080] In the general map where the robot is located, the xy coordinate value range is x(-a, a) and y(-b, b), and a and b are both double types. For example, in the simulation experiment environment of this article, the coordinate range of the robot's set route is as follows Figure 3 As shown;

[0081] The distance between the coordinates of the two vertices in route 1 is 12. After data comparison, the second set of data in the table above is taken, that is, the coordinate degree difference is set to 0.0001, then the distance between the coordinates of the two vertices is 0.0012. Assuming that the x coordinate is longitude and the y coordinate is latitude, the distance between the coordinates of the two vertices in route 1 is 120m. Other routes are analogous to route 1. Route 3 is 120m, and route 2 and route 4 are both 55m. Therefore, the rectangular route of the robot in the map is converted to the standard map used in the v2x map, that is, a rectangular route of 120m*55m. In summary, this article takes the first 4 digits of the coordinate value to meet the requirements. The specific conversion formula is as follows:

[0082] lon=(base_lon*10 7 +x*1000) / 10 7 (4)

[0083] lat=(base_lat*10 7 +y*1000) / 10 7 (5)

[0084] In formulas (4) and (5), lon represents longitude, lat represents latitude, x represents the robot's x-coordinate, and y represents the robot's y-coordinate. base_lon and base_lat both represent base values, i.e., any GNSS longitude and latitude coordinate point in the area set in the V2X test. Figure 4 The figure shows the coordinate transformation effect.

[0085] The azimuth angle information required by V2X vehicle-mounted equipment

[11] is the angle between the vehicle's driving direction and the North direction, which is parsed from the GNSS data, and increases from 0° to 359.999° in a clockwise direction. Figure 5 The figure shows the direction angle of the vehicle, where N represents the north direction and E represents the east direction;

[0086] The robot's yaw angle is a rotation around the z-axis, measured in radians. When the robot's motion coincides with the positive x-axis, the robot's yaw angle is 0 rad. When it coincides with the negative x-axis, the robot's yaw angle is π rad. When it coincides with the positive y-axis, the robot's yaw angle is (π / 2) rad. When it coincides with the negative y-axis, the robot's yaw angle is -(π / 2) rad.

[0087] In order to convert the yaw angle of the mobile robot into the heading angle of the vehicle, it is necessary to map the robot's yaw angle coordinates to the heading angle coordinates. Set the Y axis of the yaw angle coordinate system to the N direction in the heading angle coordinate system, and the X axis to the E direction in the heading angle coordinate system. Figure 6 Shown is the mapping relationship between yaw angle and azimuth angle coordinates;

[0088] According to the coordinate mapping relationship, the XY coordinates in the yaw angle coordinate system are classified according to quadrants and mathematically deduced to obtain the following formula:

[0089] In the second quadrant:

[0090] in

[0091] In the first, third, and fourth quadrants:

[0092] Where Yaw is other value. (7)

[0093] In formulas (6) and (7), Heading represents the direction angle value, and Yaw represents the yaw angle in radians;

[0094] The robot's built-in data processing module mainly obtains the mobile robot's coordinates, yaw angle, speed and other data information by monitoring TF transformation and subscribing to speed topics, and performs filtering, calibration, conversion and other processing on it, and then sends it to the V2X vehicle-mounted device through the TCP / IP protocol. Figure 7 Shown is a logic diagram of the data processing module program flow;

[0095] To verify the effectiveness of the simulation platform, this paper designed a hardware-in-the-loop simulation test platform for the V2X forward collision scenario. The test platform includes a ROS-based indoor mobile robot simulation platform, a switch, two V2X vehicle-mounted devices, and a display terminal. The switch is used to build the experimental platform network, in which the ROS-based indoor mobile robot simulation platform serves as the server and the V2X device serves as the client. The server sends the real-time position information, azimuth information, and speed information of the two robots. The two V2X devices receive their corresponding relevant information respectively and exchange information through LTE-V. The V2X-1 device uploads the position information of the two robots to the display terminal via WIFI.

[0096] To verify the effectiveness of the simulation platform, this paper designed a hardware-in-the-loop simulation test platform for the V2X forward collision scenario. The test platform includes a ROS-based indoor mobile robot simulation platform, a switch, two V2X vehicle-mounted devices, and a display terminal. The switch is used to build the experimental platform network, in which the ROS-based indoor mobile robot simulation platform serves as the server and the V2X device serves as the client. The server sends the real-time position information, azimuth information, and speed information of the two robots. The two V2X devices receive their corresponding relevant information respectively and exchange information through LTE-V. The V2X-1 device uploads the position information of the two robots to the display terminal via WIFI.

[0097] Two mobile robots Robot1 and Robot2 are used to replace HV and RV respectively to simulate vehicles driving on the road. During the operation of the robot, ROS itself will broadcast the TF transformation between map and base_link and publish the " / cmdvel" topic. The robot's built-in data processing module obtains the coordinates, yaw angle, speed and other information of the two robots by listening to TF transformation and subscribing to the " / cmd vel" topic, and then filters and calibrates the data, converts it into a GNSS data type, and sends it to their respective V2X on-board devices. After the V2X on-board device obtains the relevant data information of the two robots, it executes the forward collision application scenario algorithm. If there is a risk of forward collision, an early warning message will be sent to the display terminal. If Figure 9 The following is a schematic diagram of the application scenario design;

[0098] The ROS-based indoor mobile robot simulation platform runs on Ubuntu and includes a dual-robot simulation environment based on the GAZEBO platform, with autonomous navigation for both robots enabled through RVIZ. Within this workspace directory, a robot control package is created, with multiple target points set for each robot, controlling the robots to move in a clockwise rectangular path. When Robot1, the front robot, slows down or stops, Robot2, the rear robot, is at risk of a forward collision, simulating a rear-end collision scenario.

[0099] In addition, a data processing module function package was created for the TCP / IP communication server. This module acquires the coordinates, speed, yaw angle, and other data of the two mobile robots, converts the data into GNSS data type, and then sends it to the V2X vehicle-mounted device via TCP / IP and the switch. The specific steps of this test platform are as follows:

[0100] Run the GAZEBO simulation environment and RVIZ to display the robot's perceived environmental information and prepare for subsequent autonomous navigation;

[0101] Run the robot's built-in data processing module software, establish a socket server, and obtain the robot's coordinates, speed, yaw angle and other information in real time. After processing, it is sent to the V2X vehicle-mounted device via TCP / IP.

[0102] Run the relevant programs in the V2X vehicle-mounted device, establish a connection with the robot simulation platform socket server, and receive data sent by the robot simulation platform;

[0103] Run the display terminal program to receive the robot's BSM information sent by the V2X vehicle terminal via Wi-Fi. The display terminal will display the robot's location on the road test map.

[0104] Run the robot motion control software to make the two robots navigate to multiple target points one after the other, and run in a rectangular clockwise direction around the map to simulate a collision scenario.

[0105] In the description of this specification, the reference terms "one embodiment", "example", "specific example", etc. mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any appropriate manner in any one or more embodiments or examples;

[0106] The two V2X onboard devices exchange information via their built-in LTE-V modules, receiving each other's BSM information. When Robot2 is directly in front of Robo1, Robot1's speed is greater than Robot2's, and the calculated collision time is less than 5 seconds, Robot1 triggers a forward collision warning. As the collision time decreases to less than 3 seconds, a severe forward collision risk is triggered.

[0107] The warning information is sent to the display terminal of Robot1 via the Wi-Fi module built into the V2X vehicle-mounted device;

[0108] Test results

[0109] Multiple tests were conducted using a hardware-in-the-loop simulation platform. When the robot motion control software was running, the two robots began moving toward their respective target points. Under normal circumstances, the mobile robots display images in the RVIZ. At this time, the warning algorithm determines the safety level to be 2, and the display terminal displays the following images. When Robot2 suddenly decelerates and Robot1 moves to Position 1, the two robots are positioned in the RVIZ. At this point, Robot2 is at risk of a forward collision with Robot1. The warning algorithm detects that the estimated collision time is within 5 seconds, resulting in a safety level of 1. A warning message is output to the display terminal, and Robot2's icon on the display terminal turns orange and flashes, accompanied by an alarm sound. The display terminal displays the image at safety level 1. As the distance between the two robots decreases further, when Robot1 enters Position 2, the warning algorithm detects that the estimated collision time is less than or equal to 3 seconds, declaring a severe risk of forward collision, and the safety level is 0. Robot2's icon on the display terminal turns dark red and flashes, accompanied by an alarm sound. The display terminal displays the image at safety level 0.

[0110] The preferred embodiments of the present invention disclosed above are intended only to help illustrate the present invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the present invention to the specific embodiments described. Obviously, many modifications and variations are possible based on the content of this specification. These embodiments are selected and described in detail in this specification to better explain the principles and practical applications of the present invention, thereby enabling those skilled in the art to better understand and utilize the present invention. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A V2X indoor simulation platform based on mobile robots, characterized by: It includes input terminal, V2X vehicle-mounted equipment terminal and display terminal; The input side primarily involves the robot's information processing. During operation, the robot broadcasts topics related to the TF transformation between coordinate systems and speed. By monitoring the TF transformation between the map coordinate system and the base_link coordinate system and subscribing to the / cmd_vel topic, the robot obtains real-time coordinates, yaw angle, and speed data from its own map. This data is then filtered and calibrated, converted to GNSS data, and sent to the V2X device via network communication. The V2X vehicle-mounted device is the core unit of the system. It receives information from the robot system through network communication and processes it into its own BSM information. It also receives information from other robot systems through LTE-V and processes it into other robot BSM information. The data processing unit built into the V2X device calculates the positional relationship between the two robots based on their BSM information and executes corresponding danger warnings based on the built-in warning algorithm. If danger occurs, the warning information will be sent to the display terminal. The display terminal receives BSM information of the robot and other robots that pose a risk to the robot via Wi-Fi, and displays the positions of the robot and other robots on a map in real time. It also displays warning information sent by the V2X device in real time. After obtaining the robot's XY coordinates and yaw angle information, they are converted into the GNSS latitude and longitude and direction angle information corresponding to the vehicle; XY coordinates are converted to GNSS longitude and latitude; the distance between two longitude and latitude points is calculated according to the haversine formula. The specific formula is: in Where d is the distance between two points on the great circle, R is the radius of the Earth, Δσ is the angle between the two points and the line at the center of the sphere, lat1 and lat2 are the latitudes of the two points, and lon1 and lon2 are the longitudes of the two points. Given the longitude and latitude coordinates of any two points on Earth, the distance between them can be calculated.

2. The mobile robot-based V2X indoor simulation platform according to claim 1, characterized in that: Assume that the latitude and longitude coordinates of point A are The longitude and latitude coordinates of point B are Since the longitude and latitude in formula (1) are calculated in radians, it is necessary to convert the angles into radians. The specific formula is as follows: Then, when A and B are at the same latitude and the longitude angle difference is 0.00001°, the distance between points A and B can be calculated by formula (2), formula (3) and formula (1); similarly, the distance between two points with various angle differences under other conditions can also be calculated; Assume that the XY coordinates of the robot in the map are in the range of x(-a,a) and y(-b,b), where a and b are both double types. The unit length is 1. After data comparison, the unit length is set to correspond to 0.0001°, and the conversion distance is 10m. The first 4 digits of the coordinate value can meet the requirements. The specific conversion formula is as follows: lon=(base_lon*10 7 +x*1000) / 10 7 (4) lat=(base_lat*10 7 +y*1000) / 10 7 (5) In formulas (4) and (5), lon represents longitude, lat represents latitude, x represents the robot's x-coordinate, and y represents the robot's y-coordinate. Both base_lon and base_lat represent base values, i.e., any GNSS longitude and latitude coordinate point in the area set in the V2X test.

3. The mobile robot-based V2X indoor simulation platform according to claim 1, characterized in that: The azimuth angle information required by V2X vehicle-mounted equipment is the angle between the vehicle's driving direction and the north direction, which is parsed from GNSS data, and increases from 0° to 359.999° in a clockwise direction.

4. The mobile robot-based V2X indoor simulation platform according to claim 1, characterized in that: The robot's yaw angle rotates around the z-axis in radians. When the robot's movement direction coincides with the positive x-axis, the robot's yaw angle is 0 rad. When it coincides with the negative x-axis, the robot's yaw angle is π rad. When it coincides with the positive y-axis, the robot's yaw angle is (π / 2) rad. When it coincides with the negative y-axis, the robot's yaw angle is -(π / 2) rad. The Y-axis of the yaw angle coordinate system is mapped to the N direction in the azimuth angle coordinate system, and the X-axis is mapped to the E direction in the azimuth angle coordinate system.

5. The mobile robot-based V2X indoor simulation platform according to claim 1, characterized in that: The XY coordinates in the yaw angle coordinate system are classified according to quadrants and mathematically deduced to obtain the following formula: In the second quadrant: In the first, third, and fourth quadrants: In formulas (6) and (7), Heading represents the direction angle value, and Yaw represents the yaw angle in radians.

6. The mobile robot-based V2X indoor simulation platform according to claim 1, characterized in that: The robot's built-in data processing unit mainly obtains the mobile robot's coordinates, yaw angle and speed data information by monitoring TF transformation and subscribing to speed topics, and filters, calibrates and converts them, and then sends them to the V2X vehicle-mounted device through the TCP / IP protocol.

7. The mobile robot-based V2X indoor simulation platform according to claim 6, characterized in that: When the program runs, it executes the following steps: S1, subscribe to the robot1 / cmd_vel and robot2 / cmd_vel topics of the two mobile robot platforms respectively, and define the listener and the variables for storing coordinate transformations; S2. Create a socket service communication and wait for the client to connect; S3. By monitoring the coordinate conversion between "map" and "base_link", the robot's coordinates and yaw angle in the map are obtained in real time. At the same time, the speed is obtained by subscribing to the / cmd_vel topic. S4, filtering and calibrating the acquired data; S5. After processing, the robot coordinate values ​​are converted into GNSS longitude and latitude, and the yaw angle is converted into the heading angle; S6. Store the processed data into the BUF buffer as an integer type by byte, waiting to be sent; S7. Use TCP / IP to send the data in BUF to the V2X vehicle-mounted device.

8. The mobile robot-based V2X indoor simulation platform according to claim 6, characterized in that: In addition, a data processing module function package was created for the TCP / IP communication server. This module acquires the coordinates, speed, and yaw angle data of the two mobile robots, converts the data into GNSS data, and then sends it to the V2X vehicle-mounted device via TCP / IP and a switch. The specific steps of the test platform are as follows: S1. Run the GAZEBO simulation environment and RVIZ to display the robot's perceived environmental information and prepare for subsequent autonomous navigation. S2. Run the robot's built-in data processing module software, establish a socket server, and obtain the robot's coordinates, speed, and yaw angle information in real time. After processing, it is sent to the V2X vehicle-mounted device via TCP / IP. S3. Run the relevant program in the V2X vehicle-mounted device, establish a connection with the socket server of the robot simulation platform, and receive data sent by the robot simulation platform; S4. Run the display terminal program to receive the robot's BSM information sent by the V2X vehicle terminal via Wi-Fi. The display terminal will display the robot's location on the road test map. S5. Run the robot motion control software to make the two robots navigate to multiple target points, one in front of the other, and move in a rectangular clockwise direction around the map to simulate a collision scenario. S6. Two V2X vehicle-mounted devices exchange information via the built-in LTE-V module, receiving each other's BSM information. When Robot2 is directly in front of Robo1, Robot1's speed is greater than Robot2's, and the calculated collision time is less than 5 seconds, Robot1 triggers a forward collision warning. As the collision time decreases to less than 3 seconds, a severe forward collision risk is triggered. S7. The warning information is sent to the display terminal of Robot1 through the WiFi module built into the V2X vehicle-mounted device.

Images