A method, device, equipment and storage medium for tracking and controlling optical paths of a robot arm

Abstract

The present application discloses a method, device, equipment and storage medium for optical path tracking control of a robot arm, and relates to the technical field of robot arm control. The method comprises: determining a reflection position error between an incident laser and a reflector plane; or determining a reflection position error and a receiving position error between an incident laser and a receiver plane; calculating a terminal velocity error of the robot arm according to the reflection position error, or the reflection position error and the receiving position error, and a PID control algorithm; determining a joint angular velocity error of the robot arm according to the terminal velocity error and a Jacobian matrix; determining a joint angular position error and a joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm; determining a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error by using a joint controller; and controlling the robot arm to drive the reflector to move according to the joint angular velocity control signal, so as to track the reflection or track the occlusion of the laser with high precision and high efficiency.

Description

Technical Field

[0001] The present application relates to the field of robotic arm control technology, and in particular to a robotic arm optical path tracking control method, device, equipment and storage medium. Background Art

[0002] With the advancement of science and technology, lasers have played a vital role in various fields, and the acquisition of laser signals is the basis of most applications. In the field of surveying and mapping, laser surveyors use the emission and acquisition of lasers to precisely measure data such as size, distance, and deformation, and are widely used in robotic arm control, intelligent manufacturing, and aerospace. In the field of communications, laser communication technology uses lasers as carrier waves for communication, and has become the mainstream mode of intersatellite communication with its advantages of high communication speed, large capacity, and strong anti-interference ability. In the field of non-destructive testing, by emitting lasers to the target to be tested and collecting and analyzing the reflected lasers, the material properties of the target surface can be obtained non-destructively.

[0003] However, the accuracy and efficiency of related technologies for laser tracking are still low. Summary of the invention

[0004] The main purpose of the embodiments of the present application is to propose a robot arm optical path tracking control method, device, equipment and storage medium to improve the accuracy and efficiency of the robot arm tracking laser.

[0005] To achieve the above object, an embodiment of the present application provides a method for controlling a robot optical path tracking, the method comprising the following steps:

[0006] Determine a reflection position error between the incident laser and a reflector plane; or, determine the reflection position error and determine a reception position error between the incident laser and a receiver plane;

[0007] Calculating the end velocity error of the robot arm according to the reflected position error and the PID control algorithm, or according to the reflected position error, the received position error and the PID control algorithm;

[0008] Determine the joint angular velocity error of the robotic arm according to the terminal velocity error and the Jacobian matrix;

[0009] Determine the joint angular position error and the joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm;

[0010] Determining a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error using a joint controller;

[0011] The robot arm is controlled according to the joint angular velocity control signal to drive the reflector to move so as to track and block or track and reflect the incident laser.

[0012] In some embodiments, determining the reflection position error between the incident laser and the reflector plane comprises the following steps:

[0013] Determine the reflection position error between the incident laser and the reflector plane using a sensor device or a machine vision system;

[0014] Determining the receiving position error between the incident laser and the receiver plane comprises the following steps:

[0015] The receiving position error between the incident laser and the receiver plane is determined using the sensor device or the machine vision system.

[0016] In some embodiments, the determining the reflection position error between the incident laser and the reflector plane using a sensor device or a machine vision system comprises the following steps:

[0017] Establishing a first plane rectangular coordinate system with the center of the reflector plane as a first origin;

[0018] Determine the coordinates of a first intersection point between the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system;

[0019] The reflection position error is obtained by subtracting the coordinates of the first intersection point from the coordinates of the first origin point.

[0020] In some embodiments, determining the receiving position error between the incident laser and the receiver plane using the sensor device or the machine vision system comprises the following steps:

[0021] Establishing a second plane rectangular coordinate system with the center of the receiver plane as a second origin;

[0022] Determine the coordinates of a second intersection point between the incident laser and the receiver plane in a rectangular coordinate system of the second plane according to data collected by the sensor device or the machine vision system;

[0023] The receiving position error is obtained by subtracting the coordinates of the second intersection point from the coordinates of the second origin point.

[0024] In some embodiments, determining the coordinates of the first intersection point of the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system comprises the following steps:

[0025] Establish the world coordinate system;

[0026] Determine a first straight line equation of the incident laser in the world coordinate system and a first plane equation of the reflector plane in the world coordinate system according to data collected by the sensor device or the machine vision system;

[0027] The first straight line equation l1 is:

[0028]

[0029] The first plane equation h1 is:

[0030] m1x+n1y+p1z+d1=0;

[0031] The coordinates of the emission point of the incident laser are (x0, y0, z0); the direction vector relative to the world coordinate system is v0 = (m0, n0, p0); the normal vector relative to the world coordinate system is s1 = (m1, n1, p1); d1 = -m1x1-n1y1-p1z1;

[0032] Determine the coordinates of the intersection of the first straight line equation and the first plane equation as the first world intersection coordinates;

[0033] The coordinates of the first world intersection point are:

[0034]

[0035]

[0036] The coordinates of the first world intersection point are mapped to the first plane rectangular coordinate system to obtain the coordinates of the first intersection point in the first plane rectangular coordinate system.

[0037] In some embodiments, determining the coordinates of the second intersection point of the incident laser light and the receiver plane in the second plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system comprises the following steps:

[0038] Determine, based on the data collected by the sensor device or the machine vision system, a second straight line equation of the reflected light obtained after the incident laser is reflected by the reflector in the world coordinate system, and a second plane equation of the receiver plane in the world coordinate system;

[0039] The second straight line equation l2 is:

[0040]

[0041] Wherein, assuming that the normal vector s1 forms an acute angle with the first plane equation l1, the reflection straight line direction vector v2=(m2, n2, p2) of the first plane equation l1 symmetric with respect to the normal vector s1 is:

[0042]

[0043] v s =v proj -v1;

[0044] v2=v1+2v s ;

[0045] Where, (v1·s1) is the dot product of v1 and s1, v1=-v0;

[0046] Assume that the receiver plane passes through the fixed point (x3, y3, z3), and the normal vector relative to the world coordinate system is s3 = (m3, n3, p3);

[0047] The second plane equation h2 is:

[0048] m3x+n3y+p3z+d3=0;

[0049] Among them, d3 = -m3x3-n3y3-p3z3;

[0050] Determine the coordinates of the intersection of the second straight line equation and the second plane equation as the second world intersection coordinates;

[0051] The coordinates of the second world intersection point are:

[0052]

[0053] The coordinates of the second world intersection point are mapped to the second plane rectangular coordinate system to obtain the coordinates of the second intersection point in the second plane rectangular coordinate system.

[0054] In some embodiments, the calculating the end velocity error of the robot arm according to the reflected position error and the PID control algorithm, or according to the reflected position error, the received position error and the PID control algorithm, comprises the following steps:

[0055] According to the reflected position error and the PID control algorithm, the end velocity error of the robot arm is calculated as follows:

[0056]

[0057] Among them, E i is the error matrix integral term, E d is the differential term of the error matrix, is the terminal velocity error, k p , k i and k d are the proportional coefficient matrix, the integral coefficient matrix and the differential coefficient matrix respectively, and E is the error matrix. The error matrix is:

[0058] E=[Δx iso ,Δy iso ,0] T ;

[0059] (Δx iso ,Δy iso ) is the reflection position error between the incident laser and the isolator plane in the transmitter;

[0060] Alternatively, according to the reflection position error, the receiving position error and the PID control algorithm, the end velocity error of the robot arm is calculated as follows:

[0061]

[0062] Where E i is the error matrix integral term, E d is the differential term of the error matrix, is the terminal velocity error, k p , k i and k d are the proportional coefficient matrix, the integral coefficient matrix and the differential coefficient matrix respectively, and E is the error matrix. The error matrix is:

[0063] E=[Δx ref ,Δy ref ,0,Δx rec ,Δy rec ,0] T ;

[0064] (Δx ref ,Δy ref ) is the reflected position error between the incident laser and the tracker plane in the transmitter; (Δx rec ,Δy rec ) is the receiving position error between the incident laser and the receiver plane.

[0065] To achieve the above-mentioned purpose, another aspect of the embodiment of the present application provides a robot arm optical path tracking control device, the device comprising:

[0066] A position error determination unit, used to determine a reflection position error between the incident laser and a reflector plane; or to determine the reflection position error and determine a reception position error between the incident laser and a receiver plane;

[0067] An end error determination unit, configured to calculate an end velocity error of the robot arm according to the reflection position error and a PID control algorithm, or according to the reflection position error, the receiving position error and the PID control algorithm;

[0068] A joint angular velocity error determination unit, used to determine the joint angular velocity error of the robot arm according to the terminal velocity error and the Jacobian matrix;

[0069] A joint trajectory planning unit, used for determining a joint angular position error and a joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm;

[0070] A control signal determination unit, configured to determine a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error by using a joint controller;

[0071] The robot arm driving unit is used to control the robot arm to drive the reflector to move according to the joint angular velocity control signal, so as to track and block or track and reflect the incident laser.

[0072] To achieve the above objective, another aspect of an embodiment of the present application provides an electronic device, the electronic device comprising a memory and a processor, the memory storing a computer program, and the processor implementing the above method when executing the computer program.

[0073] To achieve the above objective, another aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program implements the above method when executed by a processor.

[0074] The embodiments of the present application include at least the following beneficial effects:

[0075] The present application can determine the reflection position error between the incident laser and the reflector plane; or, determine the reflection position error, and determine the receiving position error between the incident laser and the receiver plane; calculate the terminal velocity error of the robot arm according to the reflection position error and the PID control algorithm, or according to the reflection position error, the receiving position error and the PID control algorithm; determine the joint angular velocity error of the robot arm according to the terminal velocity error and the Jacobian matrix; use the joint trajectory planning control algorithm to determine the joint angular position error and the joint angular acceleration error according to the joint angular velocity error; use the joint controller to determine the joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error; control the robot arm to drive the reflector to move according to the joint angular velocity control signal to track the occlusion or track the reflection of the incident laser. The present application can generate a corresponding joint angular velocity control signal according to the reflection position error, or according to the reflection position error and the receiving position error, and then control the robot arm to drive the reflector to move according to the joint angular velocity control signal to track the occlusion or track the reflection of the incident laser with high precision and high timeliness. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0077] Figure 1 A schematic diagram of the structure of a laser tracking system provided in an embodiment of the present application;

[0078] Figure 2 An example scene diagram of a calibrated laser tracking system provided in an embodiment of the present application;

[0079] Figure 3 A schematic diagram of a process flow of a robot optical path tracking control provided in an embodiment of the present application;

[0080] Figure 4 A block diagram of a light path tracking reflection control based on the Jacobian matrix inversion provided in an embodiment of the present application;

[0081] Figure 5 A block diagram of light path tracking occlusion control based on Jacobian matrix inversion provided in an embodiment of the present application;

[0082] Figure 6 An example diagram of a tracker plane coordinate system provided in an embodiment of the present application;

[0083] 7 to 10 are exemplary diagrams of light path tracking simulation results provided in embodiments of the present application;

[0084] Fig.11A schematic diagram of the structure of a robot arm optical path tracking control device provided in an embodiment of the present application;

[0085] Fig.12 A schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0086] In order to make the purpose, technical solutions and advantages of the present application clearer, the present application is further described in detail below in conjunction with the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the embodiments of the present application. They are only examples of devices and methods consistent with some aspects of the embodiments of the present application as detailed in the attached claims.

[0087] It is understood that the terms "first", "second", etc. used in this application can be used to describe various concepts in this article, but unless otherwise specified, these concepts are not limited by these terms. These terms are only used to distinguish one concept from another concept. For example, without departing from the scope of the embodiment of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the words "if" and "if" as used herein can be interpreted as "at the time of" or "when" or "in response to determination".

[0088] The terms "at least one", "multiple", "each", "any", etc. used in this application, at least one includes one, two or more, multiple includes two or more, each refers to each of the corresponding multiple, and any refers to any one of the multiple.

[0089] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art to which this application belongs. The terms used herein are only for the purpose of describing the embodiments of this application and are not intended to limit this application.

[0090] Before describing the embodiments of the present application in detail, the application scenarios of the embodiments of the present application are described first:

[0091] In order to control the optical path tracking, the embodiment of the present application builds a laser tracking system including a laser transmitter, a reflector, a receiver and a rigid serial mechanical arm, wherein the reflector is arranged at the end of the mechanical arm. The structural diagram of the laser tracking system is as follows: Figure 1As shown. Among them, the laser transmitter is used to emit laser, and the laser transmitter has a three-axis rotating platform and a laser generator, which can emit light in any direction; the receiver has a three-axis rotating platform and a receiving screen, and the receiving screen plane can effectively absorb reflected lasers in all directions; the rigid serial robot arm end load excites the reflector, and the reflector includes a tracker and an isolator, the tracker is used to reflect the laser, and the isolator is used to block the incident laser; in some embodiments, the laser tracking system also includes a visual machine system, the visual machine system is used to obtain images of the reflector and the receiver, and the visual machine system field of view includes the reflector plane and the receiver plane.

[0092] Before the laser tracking system is running, drag the robotic arm and rotate the three-dimensional turntable on the reflector (the three-dimensional turntable can be used to simulate the movement of the light source) so that the laser passes through the center point of the reflector plane in the initial state, and the reflected laser passes through the center point of the receiver plane to achieve initial state calibration. The calibration example scene is as follows Figure 2 shown.

[0093] Figure 3 The embodiment of the present application provides a method for controlling the optical path tracking of a robot arm, wherein a reflector is disposed at the end of the robot arm. The method may include but is not limited to S300 to S350 (the step numbers of the embodiment of the present application are only used to distinguish different steps, and do not limit the order of the steps), as follows:

[0094] S300: determining a reflection position error between an incident laser and a reflector plane; or determining the reflection position error and determining a reception position error between the incident laser and a receiver plane.

[0095] Specifically, when it is necessary to track and block the incident laser, the position error between the incident laser and the isolator plane in the reflector is determined as the reflection position error; when it is necessary to track and reflect the incident laser, the position error between the incident laser and the tracker plane in the reflector is determined as the reflection position error, and the position error between the incident laser and the receiver plane is determined as the receiving position error.

[0096] Furthermore, in this embodiment, the corresponding position error can be determined by a sensor device disposed in the reflector and the receiver, or the corresponding position error can be determined by acquiring images of the reflector plane and the receiver plane through a visual machine system.

[0097] Therefore, S300 may include:

[0098] Determining the reflection position error between the incident laser and the reflector plane includes the following steps:

[0099] S301: Determine the reflection position error between the incident laser and the reflector plane using a sensor device or a machine vision system.

[0100] Furthermore, S301 includes the following steps:

[0101] S3011: Establish a first plane rectangular coordinate system with the center of the reflector plane as a first origin.

[0102] S3012: Determine the coordinates of the first intersection point of the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system.

[0103] More specifically, this embodiment may include:

[0104] Establish the world coordinate system;

[0105] Determine a first straight line equation of the incident laser in the world coordinate system and a first plane equation of the reflector plane in the world coordinate system according to data collected by the sensor device or the machine vision system;

[0106] The first straight line equation l1 is:

[0107]

[0108] The first plane equation h1 is:

[0109] m1x+n1y+p1z+d1=0;

[0110] The coordinates of the emission point of the incident laser are (x0, y0, z0); the direction vector relative to the world coordinate system is v0 = (m0, n0, p0); the normal vector relative to the world coordinate system is s1 = (m1, n1, p1); d1 = -m1x1-n1y1-p1z1;

[0111] Determine the coordinates of the intersection of the first straight line equation and the first plane equation as the first world intersection coordinates;

[0112] The coordinates of the first world intersection point are:

[0113]

[0114]

[0115] The coordinates of the first world intersection point are mapped to the first plane rectangular coordinate system to obtain the coordinates of the first intersection point in the first plane rectangular coordinate system.

[0116] S3013: Subtract the coordinates of the first intersection point from the coordinates of the first origin to obtain the reflection position error.

[0117] Determining the receiving position error between the incident laser and the receiver plane comprises the following steps:

[0118] S302: Determine the receiving position error between the incident laser and the receiver plane using the sensing device or the machine vision system.

[0119] Furthermore, S302 includes the following steps:

[0120] S3021: Establish a second plane rectangular coordinate system with the center of the receiver plane as the second origin.

[0121] S3022: Determine the coordinates of the second intersection point of the incident laser and the receiver plane in the second plane rectangular coordinate system based on the data collected by the sensor device or the machine vision system.

[0122] More specifically, this embodiment may include:

[0123] Determine, based on the data collected by the sensor device or the machine vision system, a second straight line equation of the reflected light obtained after the incident laser is reflected by the reflector in the world coordinate system, and a second plane equation of the receiver plane in the world coordinate system;

[0124] The second straight line equation l2 is:

[0125]

[0126] Wherein, assuming that the normal vector s1 forms an acute angle with the first plane equation l1, the reflection straight line direction vector v2=(m2, n2, p2) of the first plane equation l1 symmetric with respect to the normal vector s1 is:

[0127]

[0128] v s =v proj -v1;

[0129] v2=v1+2v s ;

[0130] Where, (v1·s1) is the dot product of v1 and s1, v1=-v0;

[0131] Assume that the receiver plane passes through the fixed point (x3, y3, z3), and the normal vector relative to the world coordinate system is s3 = (m3, n3, p3);

[0132] The second plane equation h2 is:

[0133] m3x+n3y+p3z+d3=0;

[0134] Among them, d3 = -m3x3-n3y3-p3z3;

[0135] Determine the coordinates of the intersection of the second straight line equation and the second plane equation as the second world intersection coordinates;

[0136] The coordinates of the second world intersection point are:

[0137]

[0138]

[0139] The coordinates of the second world intersection point are mapped to the second plane rectangular coordinate system to obtain the coordinates of the second intersection point in the second plane rectangular coordinate system.

[0140] S3023: Subtract the coordinates of the second intersection point from the coordinates of the second origin to obtain the receiving position error.

[0141] S310: Calculate the end velocity error of the robot arm according to the reflection position error and the PID control algorithm, or according to the reflection position error, the receiving position error and the PID control algorithm.

[0142] It can be understood that when it is necessary to track and block the incident laser, the present embodiment can calculate the end speed error of the robot arm based on the reflection position error and the PID control algorithm; when it is necessary to track and reflect the incident laser, the present embodiment can calculate the end speed error of the robot arm based on the reflection position error, the receiving position error and the PID control algorithm.

[0143] Further, S310 may include:

[0144] According to the reflected position error and the PID control algorithm, the end velocity error of the robot arm is calculated as follows:

[0145]

[0146] Among them, E i is the error matrix integral term, E d is the differential term of the error matrix, is the terminal velocity error, k p , k i and k d are the proportional coefficient matrix, the integral coefficient matrix and the differential coefficient matrix respectively, and E is the error matrix. The error matrix is:

[0147] E=[Δx iso ,Δy iso ,0]T ;

[0148] (Δx iso ,Δy iso ) is the reflection position error between the incident laser and the isolator plane in the transmitter;

[0149] Alternatively, according to the reflection position error, the receiving position error and the PID control algorithm, the end velocity error of the robot arm is calculated as follows:

[0150]

[0151] Where E i is the error matrix integral term, E d is the differential term of the error matrix, is the terminal velocity error, k p , k i and k d are the proportional coefficient matrix, the integral coefficient matrix and the differential coefficient matrix respectively, and E is the error matrix. The error matrix is:

[0152] E=[Δx ref ,Δy ref ,0,Δx rec ,Δy rec ,0] T ;

[0153] (Δx ref ,Δy ref ) is the reflected position error between the incident laser and the tracker plane in the transmitter; (Δx rec ,Δy rec ) is the receiving position error between the incident laser and the receiver plane.

[0154] S320: Determine the joint angular velocity error of the robot arm according to the terminal velocity error and the Jacobian matrix.

[0155] Specifically, the Jacobian matrix of the robot arm's operating space to the joint space is mapped, and then the inverse matrix of the Jacobian matrix is ​​determined. Then, the inverse matrix of the Jacobian matrix is ​​multiplied by the terminal velocity error to obtain the joint angular velocity error of the robot arm.

[0156] S330: Determine the joint angular position error and the joint angular acceleration error according to the joint angular velocity error using a joint trajectory planning control algorithm.

[0157] S340: Determine a joint angular velocity control signal using a joint controller according to the joint angular position error and the joint angular acceleration error.

[0158] S350: Control the robot arm to drive the reflector to move according to the joint angular velocity control signal to track and block or track and reflect the incident laser.

[0159] Next, the solution of the embodiment of the present application will be described in detail and explained in conjunction with specific application examples:

[0160] Reference Figure 4 , this embodiment provides a light path tracking reflection control block diagram based on the Jacobian matrix inverse. Figure 5 , this embodiment provides a light path tracking occlusion control block diagram based on the inverse Jacobian matrix.

[0161] Specifically, Figure 1 The laser tracking system shown in FIG. 1 is used as an example to illustrate the invention, wherein the reflector includes a tracker and an isolator, and the center point O of the tracker plane is rec (x rec0 ,y rec0 ,z rec0 ) is used as the origin to establish the tracker coordinate system, with the center point O of the isolator plane iso (x iso0 ,y iso0 ,z iso0 ) is used as the origin to establish the isolator coordinate system, with the center point O of the receiver plane rec (x rec0 ,y rec0 ,z rec0 ) is used as the origin to establish a receiver coordinate system, with each x-axis and y-axis of the coordinate system located on each plane.

[0162] Use a sensor device or machine vision system to obtain the reflector plane image, and calculate the position error between the light spot at the intersection of the laser and the tracker and the origin of the tracker coordinate system, or calculate the position error between the light spot at the intersection of the laser and the isolator and the origin of the isolator coordinate system according to different requirements for laser reflection or shielding:

[0163] e ref =(x ref0 ,y ref0 )-(x2,y2)=(Δx ref ,Δy ref );

[0164] e iso =(x iso0 ,y iso0 )-(x5,y5)=(Δx iso ,Δy iso );

[0165] Where (x2, y2) is the coordinate of the intersection of the incident laser and the tracker plane relative to the tracker coordinate system, (x5, y5) is the coordinate of the intersection of the incident laser and the isolator plane relative to the isolator coordinate system, (x ref0 ,y ref0 ) is the origin of the reflector coordinate system, (x iso0 ,y iso0 ) is the origin of the isolator coordinate system, e ref is the reflector position error, e iso is the isolator position error.

[0166] Use a sensor device or machine vision system to obtain the receiver plane image and calculate the error between the light spot at the intersection of the laser and the receiver and the origin of the receiver coordinate system:

[0167] e rec =(x rec0 ,y rec0 )-(x4,y4)=(Δx rec ,Δy rec );

[0168] Where (x4, y4) is the coordinate of the intersection of the reflected laser and the receiver plane relative to the receiver coordinate system, e rec is the receiver position error.

[0169] In some usage scenarios, a sensor device or a machine vision system can also be used to obtain the coordinates of the laser emission point and the laser direction vector, and calculate the coordinates of the intersection of the incident laser and the tracker or isolator plane and the intersection of the reflected laser and the receiver plane.

[0170] Assuming that the coordinates of the laser emission point are (x0, y0, z0), and the direction vector relative to the world coordinate system is v0 = (m0, n0, p0), then its straight line equation l1 can be expressed as:

[0171]

[0172] like Figure 6 As shown, assuming that the tracker plane passes through the fixed point (x1, y1, z1), the normal vector relative to the world coordinate system is s1 = (m1, n1, p1), then its plane equation h1 can be expressed as:

[0173] m1x+n1y+p1z+d1=0;

[0174] In the above formula, d1 = -m1x1-n1y1-p1z1.

[0175] The intersection point of the laser and the tracker, that is, the intersection point of the line l1 and the plane h1, can be expressed as:

[0176]

[0177] Assuming that the normal vector s1 forms an acute angle with the line l1, the direction vector v2 = (m2, n2, p2) of the symmetrical reflection line of the line l1 about s1 can be expressed as:

[0178]

[0179] v s =v proj -v1;

[0180] v2=v1+2v s ;

[0181] Among them, (v1·s1) is the dot product of v1 and s1, and v1=-v0.

[0182] Assuming that after the laser passes through the tracker, the reflected laser passes through the fixed point (x2, y2, z2), then its straight line equation l2 can be expressed as:

[0183]

[0184] Assuming that the receiver plane passes through the fixed point (x3, y3, z3), and the normal vector relative to the world coordinate system is s3 = (m3, n3, p3), then its plane equation h2 can be expressed as:

[0185] m3x+n3y+p3z+d3=0;

[0186] Among them, d3=-m3x3-n3y3-p3z3.

[0187] The intersection equation of the reflected laser and the receiver plane, i.e., the intersection of l2 and h2, can be expressed as:

[0188]

[0189] According to the determined position error, the end velocity error of the robot arm is calculated based on PID control:

[0190]

[0191] In the formula, k p , k i and k d are the proportional coefficient, integral coefficient and differential coefficient matrices respectively, and E is the error matrix. When tracking the reflection of the laser, we have:

[0192] E=[Δx ref ,Δy ref ,0,Δx rec ,Δy rec ,0] T ;

[0193] When tracking and blocking the laser, there are:

[0194] E=[Δx iso ,Δy iso ,0] T ;

[0195] E i is the error matrix integral term, E d is the differential term of the error matrix, is the end velocity error of the robot arm.

[0196] According to the end velocity error of the mirror holding robot and the inverse kinematics of the mirror holding robot, the joint angular velocity error can be calculated as:

[0197]

[0198] Where J is the Jacobian matrix of the mapping from the operation space of the mirror-holding manipulator to the joint space, J -1 Jacobi

[0199] The inverse of a matrix, is the joint angular velocity error of the mirror-holding robot arm.

[0200] In the specific implementation process, according to Figure 4 and Figure 5 The control block diagram shown changes the joint angles of the mirror-holding robot arm so that the tracker or isolator can continuously track the laser and the receiver can continuously receive the reflected light.

[0201] Step 1: When performing light path tracking reflection control:

[0202] The sensor device or machine vision system is used to obtain the coordinates of the intersection of the incident laser and the tracker plane (x2, y2), the coordinates of the intersection of the incident laser and the receiver plane (x4, y4), and the origin coordinates of the tracker coordinate system (x ref0 ,y ref0 ) and the origin coordinates of the receiver coordinate system (x rec0 ,y rec0 ), and then the position error signal e of the tracker can be calculated ref and the receiver's position error signal e rec , and use it as the input of the PID controller, that is:

[0203] e ref =(x ref0 ,y ref0 )-(x2,y2)=(Δx ref ,Δy ref );

[0204] e rec =(x rec0 ,y rec0 )-(x4,y4)=(Δx rec ,Δy rec );

[0205] When performing light path tracking occlusion control:

[0206] The sensor device or machine vision system is used to obtain the coordinates of the intersection of the incident laser and the isolator plane (x5, y5), and then the coordinates of the origin of the isolator's coordinate system (x iso0 ,y iso0 ), and then the position error signal e of the isolator can be calculated iso , and use it as the input of the PID controller. That is:

[0207] e iso =(x iso0 ,y iso0 )-(x5,y5)=(Δx iso ,Δy iso );

[0208] Step 2: Get the end velocity error of the mirror holding robot through the PID controller Taking it as the input of velocity-level inverse kinematics, we have:

[0209]

[0210] Step 3: Through velocity-level inverse kinematics, the joint angular velocity error can be calculated Using it as the input of joint trajectory planning and joint controller, we have:

[0211]

[0212] Step 4: Get the joint angle position error θ through joint trajectory planning d and joint angular acceleration error Used as input for the joint controller.

[0213] Step 5: Get the joint angular velocity control signal through the joint controller Used to control the mirror holding robot arm, namely:

[0214]

[0215] Step 6: When performing light path tracking reflection control:

[0216] By controlling the mirror-holding robot arm, the intersection point of the incident laser and the tracker plane (x2, y2), the intersection point of the incident laser and the receiver plane (x4, y4), and the origin of the tracker coordinate system (x ref0 ,y ref0 ), the origin of the receiver coordinate system (x rec0 ,y rec0 ), based on the changed coordinates, restart the light path tracking reflection control from step 1.

[0217] When performing light path tracking occlusion control:

[0218] By controlling the mirror-holding robot arm, the intersection point of the incident laser and the isolator plane (x5, y5), the origin of the isolator coordinate system (x iso0 ,y iso0 ), based on the changed coordinates, restart the light path tracking isolation control from step 1.

[0219] To verify the solution of this embodiment, an experimental platform was built to verify the laser tracking reflection and laser tracking occlusion scenarios. The cross-sections of the reflector and receiver are both square, with an area of ​​0.01m 2 , the maximum allowable error is 10cm. In the initial state, the joint angles of the mirror-holding manipulator are [-246.4, -93.9, -72.4, 230.2, -259.2, -338.9]°, the coordinates of the center point of the transmitter are [-0.250, -0.058, 0.405]m, and the position of the end of the mirror-holding manipulator is [-0.15, 0.35, 0.48]m. Before each experiment, the arm shape of the mirror-holding manipulator and the angle of the receiver turntable are fine-tuned to make the center of the incident beam approach the center of the tracker or isolator and the center of the reflected beam approach the center of the receiver.

[0220] In the signal acquisition mode, the laser passes through the tracker to generate reflected light, and the receiver receives the reflected light, forming a point-shaped light spot on the receiving plane; in the signal isolation mode, the laser is blocked by the isolator and no reflected light is generated. In the specific experimental process, the movement mode of the laser transmitter is divided into two modes: pitch angle change and yaw angle change, and the simulation results of the two movement modes of signal acquisition mode and signal isolation mode are obtained respectively.

[0221] In the specific experimental process, the signal acquisition mode or signal isolation mode is used respectively, and the joint of the transmitting end turntable rotates to drive the signal transmitter to produce a pitch angle or yaw angle change. Before the simulation starts, adjust the mirror holding mechanical arm so that the beam splitter is facing the incident beam, and adjust the receiving end posture so that the initial transmission beam passes through the center of the receiving end plane. During the simulation process, the angular velocity is set to 2° / s and -2° / s respectively, and the simulation results are shown in Figures 7 to 10. Among them, Figure 7 is the signal acquisition mode-pitch angle change simulation curve, Figure 7 (a) is the 2° / s receiving end plane error curve, and Figure 7 (b) is the -2° / s receiving end plane error curve. Figure 8 is the signal acquisition mode-yaw angle change simulation curve, Figure 8 (a) is the 2° / s receiving end plane error curve, and Figure 8 (b) is the -2° / s receiving end plane error curve. Figure 9 is the simulation curve of signal isolation mode-pitch angle change, Figure 9(a) is the plane error curve of the 2° / s receiving end, and Figure 9(b) is the plane error curve of the -2° / s receiving end. Figure 10 is the simulation curve of signal isolation mode-yaw angle change, Figure 10(a) is the plane error curve of the 2° / s receiving end, and Figure 10(b) is the plane error curve of the -2° / s receiving end.

[0222] From the simulation results of Figures 7 to 10, it can be seen that in the two movement modes, the laser reflection tracking method and the laser occlusion tracking method proposed in this embodiment perform similarly, and the tracking errors both reach their peak values ​​around the initial 1 second, and the errors are all controlled within 0.2 mm, and the errors converge to close to 0 after 8 seconds.

[0223] In summary, the laser reflection and occlusion tracking method proposed in this embodiment achieves good simulation results under low angular velocity conditions.

[0224] This embodiment simulates the optical path change relationship between the laser transmitter-reflector-receiver, obtains the error between the light spot on the tracker, reflector and receiver and the origin of each coordinate system through a sensor device or a machine vision system, and then calculates the position error and the joint angle error of the mirror-holding manipulator in real time, so that the mirror-holding manipulator can complete the tracking task of laser reflection or occlusion through PID control, and builds an experimental platform to simulate and verify this method. The simulation results show that this method can quickly and accurately realize optical path reflection or occlusion tracking under small angular velocity conditions, and can be widely used in application scenarios related to manipulator positioning and navigation.

[0225] Reference Fig.11 The embodiment of the present application further provides a robot light path tracking control device, which can implement the above-mentioned robot light path tracking control method, and the device includes:

[0226] A position error determination unit, used to determine a reflection position error between the incident laser and a reflector plane; or to determine the reflection position error and determine a reception position error between the incident laser and a receiver plane;

[0227] An end error determination unit, configured to calculate an end velocity error of the robot arm according to the reflection position error and a PID control algorithm, or according to the reflection position error, the receiving position error and the PID control algorithm;

[0228] A joint angular velocity error determination unit, used to determine the joint angular velocity error of the robot arm according to the terminal velocity error and the Jacobian matrix;

[0229] A joint trajectory planning unit, used for determining a joint angular position error and a joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm;

[0230] A control signal determination unit, configured to determine a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error by using a joint controller;

[0231] The robot arm driving unit is used to control the robot arm to drive the reflector to move according to the joint angular velocity control signal, so as to track and block or track and reflect the incident laser.

[0232] It can be understood that the contents of the above method embodiments are all applicable to the present device embodiments, the functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0233] The embodiment of the present application also provides an electronic device, the electronic device includes a memory and a processor, the memory stores a computer program, and the processor implements the above-mentioned robot arm optical path tracking control method when executing the computer program. The electronic device can be any intelligent terminal including a tablet computer, a vehicle-mounted computer, etc.

[0234] It can be understood that the contents of the above method embodiments are all applicable to the present device embodiments, the functions specifically implemented by the present device embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0235] See also Fig.12 , Fig.12 The hardware structure of an electronic device of another embodiment is illustrated, and the electronic device includes:

[0236] The processor 1201 may be implemented by a general-purpose CPU (Central Processing Unit), a microprocessor, an application-specific integrated circuit (Application Specific Integrated Circuit, ASIC), or one or more integrated circuits, and is used to execute relevant programs to implement the technical solutions provided in the embodiments of the present application;

[0237] The memory 1202 can be implemented in the form of a read-only memory (ROM), a static storage device, a dynamic storage device, or a random access memory (RAM). The memory 1202 can store an operating system and other application programs. When the technical solution provided in the embodiment of this specification is implemented by software or firmware, the relevant program code is stored in the memory 1202, and the processor 1201 calls and executes the robot arm optical path tracking control method of the embodiment of this application;

[0238] Input / output interface 1203, used to implement information input and output;

[0239] The communication interface 1204 is used to realize the communication interaction between the device and other devices. The communication can be realized through a wired manner (such as USB, network cable, etc.) or a wireless manner (such as mobile network, WIFI, Bluetooth, etc.);

[0240] A bus 1205 that transmits information between various components of the device (e.g., the processor 1201, the memory 1202, the input / output interface 1203, and the communication interface 1204);

[0241] The processor 1201 , the memory 1202 , the input / output interface 1203 and the communication interface 1204 are connected to each other in communication within the device via the bus 1205 .

[0242] An embodiment of the present application also provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, the above-mentioned robot arm optical path tracking control method is implemented.

[0243] It can be understood that the contents of the above method embodiments are all applicable to the present storage medium embodiments, the functions specifically implemented by the present storage medium embodiments are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those achieved by the above method embodiments.

[0244] The memory, as a non-transient computer-readable storage medium, can be used to store non-transient software programs and non-transient computer executable programs. In addition, the memory may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some embodiments, the memory may optionally include a memory remotely disposed relative to the processor, and these remote memories may be connected to the processor via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0245] The embodiments described in the embodiments of the present application are intended to more clearly illustrate the technical solutions of the embodiments of the present application and do not constitute a limitation on the technical solutions provided in the embodiments of the present application. Those skilled in the art will appreciate that with the evolution of technology and the emergence of new application scenarios, the technical solutions provided in the embodiments of the present application are also applicable to similar technical problems.

[0246] Those skilled in the art will appreciate that the technical solutions shown in the figures do not constitute a limitation on the embodiments of the present application, and may include more or fewer steps than shown in the figures, or a combination of certain steps, or different steps.

[0247] The device embodiments described above are merely illustrative, and the units described as separate components may or may not be physically separated, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0248] Those skilled in the art will appreciate that all or some of the steps in the methods disclosed above, and the functional modules / units in the systems and devices may be implemented as software, firmware, hardware, or a suitable combination thereof.

[0249] The terms "first", "second", "third", "fourth", etc. (if any) in the specification of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0250] It should be understood that in the present application, "at least one (item)" means one or more, and "plurality" means two or more. "And / or" is used to describe the association relationship of associated objects, indicating that three relationships may exist. For example, "A and / or B" can mean: only A exists, only B exists, and A and B exist at the same time, where A and B can be singular or plural. The character " / " generally indicates that the objects associated before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b or c can mean: a, b, c, "a and b", "a and c", "b and c", or "a and b and c", where a, b, c can be single or multiple.

[0251] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the above units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0252] The units described above as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0253] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0254] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including multiple instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, referred to as ROM), random access memory (Random Access Memory, referred to as RAM), disk or optical disk and other media that can store programs.

[0255] The preferred embodiments of the present invention are described above with reference to the accompanying drawings, but the scope of the rights of the present invention is not limited thereto. Any modification, equivalent substitution and improvement made by a person skilled in the art without departing from the scope and essence of the present invention should be within the scope of the rights of the present invention.

Claims

1. A method for controlling the optical path tracking of a robot arm, characterized in that: The reflector is arranged at the end of the mechanical arm, and the method comprises the following steps: Determine a reflection position error between the incident laser and a reflector plane; or, determine the reflection position error and determine a reception position error between the incident laser and a receiver plane; Calculating the end velocity error of the robotic arm according to the reflected position error and the PID control algorithm, or according to the reflected position error, the received position error and the PID control algorithm; Determine the joint angular velocity error of the robotic arm according to the terminal velocity error and the Jacobian matrix; Determine the joint angular position error and the joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm; Determining a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error using a joint controller; Controlling the robot arm to drive the reflector to move according to the joint angular velocity control signal to track, block or track and reflect the incident laser; The method of determining the reflection position error between the incident laser and the reflector plane comprises the following steps: Determine the reflection position error between the incident laser and the reflector plane using a sensor device or a machine vision system; Determining the receiving position error between the incident laser and the receiver plane comprises the following steps: Determining the receiving position error between the incident laser and the receiver plane using the sensing device or the machine vision system; The method of determining the reflection position error between the incident laser and the reflector plane by using a sensor device or a machine vision system comprises the following steps: Establishing a first plane rectangular coordinate system with the center of the reflector plane as a first origin; Determine the coordinates of a first intersection point between the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system; Subtracting the coordinates of the first intersection point from the coordinates of the first origin point to obtain the reflection position error; Determining the coordinates of the first intersection point of the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system comprises the following steps: Establish the world coordinate system; Determine a first straight line equation of the incident laser in the world coordinate system and a first plane equation of the reflector plane in the world coordinate system according to data collected by the sensor device or the machine vision system; The first straight line equation for: ； The first plane equation for: ； Among them, the emission point coordinates of the incident laser are ; The direction vector relative to the world coordinate system is ; The normal vector relative to the world coordinate system is ; ; Determine the coordinates of the intersection of the first straight line equation and the first plane equation as the first world intersection coordinates; The coordinates of the first world intersection point are: ； ； ； The coordinates of the first world intersection point are mapped to the first plane rectangular coordinate system to obtain the coordinates of the first intersection point in the first plane rectangular coordinate system.

2. A robot arm optical path tracking control method according to claim 1, characterized in that: The method of using the sensor device or the machine vision system to determine the receiving position error between the incident laser and the receiver plane comprises the following steps: Establishing a second plane rectangular coordinate system with the center of the receiver plane as a second origin; Determine the coordinates of a second intersection point between the incident laser and the receiver plane in a rectangular coordinate system of the second plane according to data collected by the sensor device or the machine vision system; The receiving position error is obtained by subtracting the coordinates of the second intersection point from the coordinates of the second origin point.

3. A robot arm optical path tracking control method according to claim 2, characterized in that: Determining the coordinates of the second intersection point of the incident laser and the receiver plane in the second plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system comprises the following steps: Determine, based on the data collected by the sensor device or the machine vision system, a second straight line equation of the reflected light obtained after the incident laser is reflected by the reflector in the world coordinate system, and a second plane equation of the receiver plane in the world coordinate system; The second straight line equation for: ； Wherein, let the normal vector With the first plane equation At an acute angle, the equation of the first plane is About the normal vector Symmetrical reflection line direction vector for: ； ； ； in, for and The dot product of ; Assume that the receiver plane passes through a fixed point , the normal vector relative to the world coordinate system is ; The second plane equation for: ； in, ; Determine the coordinates of the intersection of the second straight line equation and the second plane equation as the second world intersection coordinates; The coordinates of the second world intersection point are: ； ； ； The coordinates of the second world intersection point are mapped to the second plane rectangular coordinate system to obtain the coordinates of the second intersection point in the second plane rectangular coordinate system.

4. A method for controlling the optical path tracking of a robot arm according to any one of claims 1 to 3, characterized in that: The step of calculating the end velocity error of the robot arm according to the reflected position error and the PID control algorithm, or according to the reflected position error, the receiving position error and the PID control algorithm, comprises the following steps: According to the reflected position error and the PID control algorithm, the end velocity error of the robotic arm is calculated as follows: ； in, is the error matrix integral term, is the differential term of the error matrix, , is the terminal velocity error, , and They are the proportional coefficient matrix, integral coefficient matrix and differential coefficient matrix respectively. is the error matrix, and the error matrix is: ； is the reflection position error between the incident laser and the isolator plane in the reflector; Alternatively, according to the reflection position error, the receiving position error and the PID control algorithm, the end velocity error of the robotic arm is calculated as follows: ； in, is the error matrix integral term, is the differential term of the error matrix, , is the terminal velocity error, , and They are the proportional coefficient matrix, integral coefficient matrix and differential coefficient matrix respectively. is the error matrix, and the error matrix is: ； is the reflection position error between the incident laser and the tracker plane in the reflector; is the receiving position error between the incident laser and the receiver plane.

5. A robot arm optical path tracking control device, characterized in that: The device comprises: A position error determination unit, used to determine a reflection position error between the incident laser and a reflector plane; or to determine the reflection position error and determine a reception position error between the incident laser and a receiver plane; An end error determination unit, configured to calculate an end velocity error of the robotic arm according to the reflected position error and a PID control algorithm, or according to the reflected position error, the receiving position error and the PID control algorithm; A joint angular velocity error determination unit, used to determine the joint angular velocity error of the robotic arm according to the terminal velocity error and the Jacobian matrix; A joint trajectory planning unit, used for determining a joint angular position error and a joint angular acceleration error according to the joint angular velocity error by using a joint trajectory planning control algorithm; A control signal determination unit, configured to determine a joint angular velocity control signal according to the joint angular position error and the joint angular acceleration error by using a joint controller; A mechanical arm driving unit, used for controlling the mechanical arm to drive the reflector to move according to the joint angular velocity control signal, so as to track and block or track and reflect the incident laser; The method of determining the reflection position error between the incident laser and the reflector plane comprises the following steps: Determine the reflection position error between the incident laser and the reflector plane using a sensor device or a machine vision system; Determining the receiving position error between the incident laser and the receiver plane comprises the following steps: Determining the receiving position error between the incident laser and the receiver plane using the sensing device or the machine vision system; The method of determining the reflection position error between the incident laser and the reflector plane by using a sensor device or a machine vision system comprises the following steps: Establishing a first plane rectangular coordinate system with the center of the reflector plane as a first origin; Determine the coordinates of a first intersection point between the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system; Subtracting the coordinates of the first intersection point from the coordinates of the first origin point to obtain the reflection position error; Determining the coordinates of the first intersection point of the incident laser and the reflector plane in the first plane rectangular coordinate system according to the data collected by the sensor device or the machine vision system comprises the following steps: Establish the world coordinate system; Determine a first straight line equation of the incident laser in the world coordinate system and a first plane equation of the reflector plane in the world coordinate system according to data collected by the sensor device or the machine vision system; The first straight line equation for: ； The first plane equation for: ； Among them, the emission point coordinates of the incident laser are ; The direction vector relative to the world coordinate system is ; The normal vector relative to the world coordinate system is ; ; Determine the coordinates of the intersection of the first straight line equation and the first plane equation as the first world intersection coordinates; The coordinates of the first world intersection point are: ； ； ； The coordinates of the first world intersection point are mapped to the first plane rectangular coordinate system to obtain the coordinates of the first intersection point in the first plane rectangular coordinate system.

6. An electronic device, characterized in that: The electronic device comprises a memory and a processor, the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 4 when executing the computer program.

7. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 4 is implemented.

Images