CN116476078B - Laser radar-based stable target tracking method for snake-shaped robot - Google Patents
Laser radar-based stable target tracking method for snake-shaped robot Download PDFInfo
- Publication number
- CN116476078B CN116476078B CN202310722790.3A CN202310722790A CN116476078B CN 116476078 B CN116476078 B CN 116476078B CN 202310722790 A CN202310722790 A CN 202310722790A CN 116476078 B CN116476078 B CN 116476078B
- Authority
- CN
- China
- Prior art keywords
- head
- angle
- snake
- shaped robot
- laser radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000005021 gait Effects 0.000 claims abstract description 32
- 230000002572 peristaltic effect Effects 0.000 claims description 24
- 238000004364 calculation method Methods 0.000 claims description 11
- WYTGDNHDOZPMIW-RCBQFDQVSA-N alstonine Natural products C1=CC2=C3C=CC=CC3=NC2=C2N1C[C@H]1[C@H](C)OC=C(C(=O)OC)[C@H]1C2 WYTGDNHDOZPMIW-RCBQFDQVSA-N 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 8
- 238000013459 approach Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 abstract description 3
- 230000006870 function Effects 0.000 description 4
- 239000004973 liquid crystal related substance Substances 0.000 description 4
- 230000008447 perception Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Abstract
The application relates to the technical field of super-redundancy robots, solves the technical problems of low efficiency and easy offset of mobile mapping of the existing snake-shaped robots, in particular to a stable target tracking method of a snake-shaped robot based on a laser radar, which comprises the following steps: s1, acquiring a pitching Euler angle of the gesture of the head of the snake-shaped robot and an expected pitching angle of a relative horizontal plane measured by an IMU sensor in real time, and controlling the advancing movement gait of the snake-shaped robot; s2, calculating the deviation angle of the head according to the actual pitching Euler angle of the head and the expected pitching angle of the head relative to the horizontal plane, which are measured by the IMU sensor. According to the application, the IMU sensor detects the Euler angle change of the head gesture in real time so as to compensate the joint angle, so that the head laser radar can stably scan, and the boundary deviation of the map is avoided.
Description
Technical Field
The application relates to the technical field of super-redundancy robots, in particular to a target stable tracking method of a snake-shaped robot based on a laser radar.
Background
With the wide application of the snake-shaped robot in the field detection and rescue assistance, the environment sensing capability of the snake-shaped robot is particularly important. The snake-shaped robot can sense the surrounding environment by means of sensors such as a laser radar and the like, so that obstacle avoidance and dynamic target recognition tasks are realized. At present, the snake-shaped robot is used for controlling the rotation of the head joint to drive the rotation of the head laser radar to scan after parking, so as to acquire point cloud data. Because of the need of stopping, stable perception mapping can not be performed while advancing, and the environment perception efficiency is relatively low in this way. In addition, head shake has a large influence on stable scanning of the planar laser radar when the snake-shaped robot moves, and map construction is easy to deviate.
Disclosure of Invention
Aiming at the defects of the prior art, the application provides a stable target tracking method of a snake-shaped robot based on a laser radar, which solves the technical problems of low efficiency and easy deviation of the moving mapping of the existing snake-shaped robot.
In order to solve the technical problems, the application provides the following technical scheme: a snake-shaped robot target stable tracking method based on a laser radar comprises the following steps:
s1, acquiring a pitching Euler angle of the head gesture of the snake-shaped robot and an expected pitching angle of a relative horizontal plane measured by an IMU sensor in real time, and controlling the advancing movement gait of the snake-shaped robot;
s2, calculating a deviation angle of the head according to the actual pitching Euler angle of the head and the expected pitching angle of the head relative to the horizontal plane, which are measured by the IMU sensor;
s3, according to the current firstiThe offset angle of the head and the joint angle of the head measured by the position sensor on the joint at the moment are compensated and updated in real time through the sensor dataiExpected joint angle of the head joint at +1;
s4, determining the position of the target object in a local coordinate system of the laser radar according to the point cloud data of the surrounding environment;
s5, calculating the azimuth angle of the target object relative to the axis of the snake-shaped robot according to the position of the target object based on the laser radar local coordinate system, and adjusting the advancing direction of the snake-shaped robot according to the azimuth angle.
Further, in step S1, specifically includes:
the laser radar is horizontally arranged at the head of the snake-shaped robot, the snake-shaped robot is controlled to advance by adopting peristaltic gait, and the motion gait equation of the peristaltic gait control of the snake-shaped robot is as follows:
；
in the above formula, A represents the amplitude of the change of the joint angle,is->Time of daynJoint angle of individual joint angles->To control the frequency of the peristaltic gait over time, control the rate of progression of the peristaltic gait, +.>Spatial frequency for peristaltic gait, +.>To control the snake-shaped robot at +.>The amount of motion turning control at the moment.
Further, in step S2, the calculation formula of the deviation angle of the head is:
；
in the above-mentioned method, the step of,for a desired pitch angle of the head relative to the horizontal, +.>Actually measuring the head pitch Euler angle for IMU sensor, < >>Is the angle of departure of the head.
Further, the desired pitch angle relative to the horizontalSet to 0 degrees.
Further, in step S3, a desired joint angle of the head jointThe updated formula of (2) is:
；
wherein the method comprises the steps ofIs->The joint angle of the head, measured by a position sensor on the moment joint, < >>Is->Deviation angle of time head->Is->Desired joint angle of the head joint at the moment.
Further, in step S4, the specific process includes the following steps:
s41, acquiring a point cloud data set of a single scanning period of the surrounding environment of the head；
S42, setting point cloud data setThere is->Personal point cloud data->And calculating point cloud data from the lidar sensor information +.>，/>Representing the data set +.>The%>Personal point cloud data->；
S43, setting the sliding point length asAnd let->；
S44, slave point cloud dataStart slide continuous taking ∈ ->Personal point cloud data->As a point cloud sequence->Go on->A plurality of fitting errors are determined by sub-matching>；
If it isThen get the point cloud data +.>Performing a round least squares fit and calculating a fitting error +.>Then returns to step S44;
if it isStep S45 is entered;
s45, according to a plurality of fitting errorsDifference of differenceCalculating the minimum match error +.>；
S46, according to the minimum matching errorThe group of matching with the minimum corresponding error is used for determining the position of the target object in the local coordinate system of the laser radar;
if the matching error is minimumStep S5 is entered;
if the matching error is minimumThen return to step S41.
Further, in step S5, azimuth angleThe calculation formula of (2) is as follows:
；
in the above，/>For the unit direction vector of the head axis in the local coordinate system of the lidar +.>For the origin of the local coordinate system of the laser radar to the fitting circle center +.>Is>For the deflection angle of the target object with respect to the head of the serpentine robot, < >>Is a two-parameter arctangent function, det%M) Representation matrixMIs a determinant of (2).
Further, in step S5, the specific process includes the steps of:
s51, calculating the center distance of the center of the target object relative to the laser radar in the head of the robot；
S52, setting the period time of laser radar scanning asFirst->And->The center distance between the center of the target object and the center of the laser radar at the moment is +.>And->And calculate the target->Speed of movement of time relative to lidar；
If it isAnd->Then the stable tracking of the snake-shaped robot to the target is completed；
If it isThe frequency of the peristaltic gait changing with time is controlled to be gradually increased with proper step length +.>Up toLess than 0 and up to the maximum speed of movement of the robot, i.e. the speed of movement of the snake-shaped robot is greater than the speed of the target object, the snake-shaped robot approaches the target object at maximum speed, when +.>At this time, the frequency of controlling the peristaltic gait over time is gradually reduced in appropriate steps +.>Up to->The method is used for completing automatic adjustment of the target tracking speed by the snake-shaped robot.
Further, in step S52, the threshold valueThe value of (2) is 0.2cm/s, threshold +.>The value of (2) is 5cm.
By means of the technical scheme, the application provides a snake-shaped robot target stable tracking method based on a laser radar, which has the following beneficial effects:
1. according to the application, the IMU sensor detects the Euler angle change of the head gesture in real time so as to compensate the joint angle, so that the head laser radar can stably scan, and the boundary deviation of the map is avoided.
2. According to the application, the target object is rapidly identified in a sliding matching mode, and the direction and the speed of the snake-shaped robot are controlled in a closed loop based on the azimuth angle of the local coordinate system of the laser radar and the map movement information, so that the stable tracking of the dynamic target is realized.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application and do not constitute a limitation on the application. In the drawings:
FIG. 1 is a flow chart of a serpentine robotic target stabilization tracking method of the present application;
FIG. 2 is a left side view of a head of the serpentine robotic target tracking of the present application;
FIG. 3 is a top view of a head of the serpentine robotic machine of the present application as it tracks a target.
Detailed Description
In order that the above-recited objects, features and advantages of the present application will become more readily apparent, a more particular description of the application will be rendered by reference to the appended drawings and appended detailed description. Therefore, the realization process of how to apply the technical means to solve the technical problems and achieve the technical effects can be fully understood and implemented.
Those of ordinary skill in the art will appreciate that all or a portion of the steps in a method of implementing an embodiment may be implemented by a program to instruct related hardware and thus that the application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
Referring to fig. 1-3, a specific implementation manner of the present embodiment is shown, where the present embodiment is used to solve the problem that when a surrounding environment map is constructed by using a head laser radar in the existing serpentine robot movement method, the map is offset due to head shake; meanwhile, the existing method has the technical problem that stable tracking is difficult to perform after the object is identified.
Referring to fig. 1, the embodiment provides a target stable tracking method of a snake-shaped robot based on a laser radar, which comprises the following steps:
s1, acquiring a pitching Euler angle of an IMU sensor for measuring head posture of a snake-shaped robot in real timeAnd a desired pitch angle relative to the horizontal>And controls the advancing motion gait of the snake-shaped robot.
In this embodiment, the specific process of step S1 includes:
the laser radar is horizontally arranged at the head of the snake-shaped robot, the head is the head position of the snake-shaped robot, specifically, the part for controlling the snake-shaped robot to advance in peristaltic gait is controlled to advance by adopting peristaltic gait, and the motion gait equation for controlling the peristaltic gait of the snake-shaped robot is as follows:
；
wherein A represents the amplitude of the change in the angle of the joint,the joint angle which is the nth joint angle at the k+1 time,to control the frequency of the peristaltic gait over time, control the rate of progression of the peristaltic gait, +.>Spatial frequency for peristaltic gait, +.>To control the snake-shaped robot at the firstkSports turning control amount at +1.
S2, according to the actual pitching Euler angle of the head measured by the IMU sensorAnd the desired pitch angle of the head relative to the horizontal plane +.>Calculate the deviation angle of the head +.>；
Angle of deviation of headThe calculation formula of (2) is as follows:
；
in the above-mentioned method, the step of,for a desired tilt angle of the head with respect to the horizontal plane, for ensuring that the lidar is scanning in the horizontal plane, a desired tilt angle with respect to the horizontal plane is here + ->Set to 0 degrees, +.>The actual measured head pitch euler angle for the IMU sensor is shown in fig. 2 and 3, which are the left and top views of the head during target tracking for a serpentine robot, respectively.
S3, according to the current firstDeviation angle of time head +.>And the joint angle of the head measured by the position sensor on the joint +.>The update of the +.f can be compensated in real time by the sensor data>Desired joint angle of head joint at momentThe head of the snake-shaped robot is stabilized on the horizontal plane, and the laser radar is guaranteed to acquire point cloud data of the surrounding environment on the horizontal plane.
First, theDesired joint angle of the head joint at moment +.>The updated formula of (2) is:
；
wherein the method comprises the steps ofIs->The joint angle of the head, measured by a position sensor on the moment joint, < >>Is->Deviation angle of time head->Is->Desired joint angle of the head joint at the moment.
S4, determining the position of the target object in a local coordinate system of the laser radar according to the point cloud data of the surrounding environment;
in this embodiment, the specific process of step S4 includes the following steps:
s41, acquiring a point cloud data set Q of a single scanning period of the surrounding environment of the head;
s42, the set point cloud data set Q comprisesPersonal point cloud data->And calculating point cloud data from the lidar sensor information +.>，/>The +.>Personal point cloud data->；
The laser radar sensor information contains parameter information such as the distance and the azimuth of a target object, the rotation minimum angle of the laser radar, the rotation step angle of the laser radar and the like, so that the point cloud dataThe calculation formula of (2) is as follows:
；
in the above-mentioned method, the step of,for the rotation angle of the lidar, +.>Rotate a minimum angle for the lidar, +.>For the laser radar rotation step angle,/->Is->The scanning distance of the scanning points.
S43, setting the sliding point length asAnd let->；
S44, slave point cloud dataStart slide continuous taking ∈ ->Personal point cloud data->As a point cloud sequence->Go on->A plurality of fitting errors are determined by sub-matching>；
If it isThen get the point cloud data +.>Performing a round least squares fit and calculating a fitting error +.>Then returns to step S44;
if it isStep S45 is entered.
Specifically, matching is performed according to the sphere characteristics of the target object, and the sliding selection point cloud sequence is assumed to beThen:
and (3) identifying the moving sphere of the dynamic target object, and carrying out circle fitting on the point cloud as the point scanned by the plane of the sphere is positioned on the circular arc, wherein the formula is as follows:
；
in the above, the center coordinatesAnd radius R is unknown, then let:
；
further comprises the following steps:
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
；
in the above-mentioned method, the step of,kfor the number of laser radar sampling points,representing the transposed matrix of matrix A, ">Representing the inverse of matrix A, < >>Is->T in (2) represents the row vector +.>Is a transpose of (a).
The point cloud sequence is then followedWith equations (10) and (12), the vector +_can be calculated by equations (9) and (11)>X, Y and Z in (2) and then solving the center coordinates +.>Radius of the sameR。
Thus, fitting errorThe calculation formula of (2) is as follows:
；
in the above-mentioned method, the step of,、/>t in (2) represents vector->Transpose of->Represents the transposed matrix of matrix A, k represents the point cloud sequence +.>The number of point clouds.
S45, according to a plurality of fitting errorsCalculating the minimum match error +.>；
Minimum match errorThe calculation formula of (2) is as follows:
；
wherein the Min function is the minimum function and returns all fitting errorsIs the minimum value of (a).
S46, according to the minimum matching errorThe group of matching with the minimum corresponding error is used for determining the position of the target object in the local coordinate system of the laser radar;
if the matching error is minimumStep S5 is entered;
if the matching error is minimumThen return to step S41.
Selecting a group of matching with minimum error to meet the conditionSelecting a group of matching calculation with the smallest error, and further determining the position of the target object in the local coordinate system of the laser radar, wherein +.>Representing the minimum match error,/->Representing the matching error threshold value, and the proper value range in practical application is [2.5e-3,2.25e-2 ]]cm 2 。
S5, calculating the azimuth angle of the target object relative to the axis of the snake-shaped robot according to the position of the target object based on the laser radar local coordinate system, and adjusting the advancing direction of the snake-shaped robot according to the azimuth angle.
Azimuth angleThe calculation formula of (2) is as follows:
based on the local coordinate system of the laser radar, there are:
；
in the aboveFor the unit direction vector of the head axis in the local coordinate system of the lidar +.>For the origin of the local coordinate system of the laser radar to the fitting circle center +.>Is>For the deflection angle of the target object with respect to the head of the serpentine robot, < >>As a two-parameter arctangent function, as shown in fig. 3. />Representation matrixMIs a determinant of (2).
Control amount for serpentine robot steeringBy PThe D control is as follows:
；
wherein, the liquid crystal display device comprises a liquid crystal display device,and->Control parameters of PD, respectively->For turning control variables +.>Upper limit of->Is->Moment of movement turning control amount +.>Is->The first derivative with respect to time.
In this embodiment, the specific process of step S5 includes the following steps:
s51, calculating the center distance of the center of the target object relative to the laser radar in the head of the robot；
Specifically, the motion turning control amount in the equation (1) and the equation (17) of the gait equationControl the automatic adjustment of the movement direction of the snake-shaped robot, +.>To control the frequency of the peristaltic gait over time, the rate of progression of the peristaltic gait is controlled by the center coordinates of equation (7) above>Calculating the center distance of the center of the target object with respect to the laser radar in the robot head>。
；
S52, assuming that the period time of laser radar scanning isFirst->And->The center distance between the center of the target object and the center of the laser radar at the moment is +.>And->Then calculate the target->Speed of movement of time relative to lidar；
Speed of movementThe calculation formula of (2) is as follows:
；
if it isAnd->I.e. the speed of movement of the target object relative to the lidar in the headBelow threshold +.>And center distance relative to the lidar in the head +.>Less than threshold->Threshold +.>The value of (2) is 0.2cm/s, threshold +.>The value of the target is 5cm, and the snake-shaped robot realizes stable tracking of the target.
If it isThe frequency of the control peristaltic gait in equation (1) over time is gradually increased in appropriate steps>Up to->Less than 0 and up to a maximum movement speed, i.e. the speed of movement of the snake-shaped robot is greater than the speed of the target object, the snake-shaped robot approaches the target object at maximum speed, when +.>At this time, the frequency of controlling the peristaltic gait over time is gradually reduced in appropriate steps +.>Up to->Thereby realizing the automatic adjustment of the target tracking speed by the snake-shaped robot.
According to the embodiment, the motion of the head of the snake-shaped robot can be compensated and corrected, the expected joint angle of the head joint is compensated and updated in real time through the sensor data, the motion gait of the snake-shaped robot is adjusted according to the identification direction of the tracking target, the motion direction and the speed of the snake-shaped robot are further controlled, stable tracking of the target object is achieved, and the embodiment is also suitable for the fields of motion planning, navigation and the like of the snake-shaped robot.
In this specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different manner from other embodiments, so that the same or similar parts between the embodiments are referred to each other. For each of the above embodiments, since it is substantially similar to the method embodiment, the description is relatively simple, and reference should be made to the description of the method embodiment for relevant points.
The foregoing embodiments have been presented in a detail description of the application, and are presented herein with a particular application to the understanding of the principles and embodiments of the application, the foregoing embodiments being merely intended to facilitate an understanding of the method of the application and its core concepts; meanwhile, as those skilled in the art will have variations in the specific embodiments and application scope in accordance with the ideas of the present application, the present description should not be construed as limiting the present application in view of the above.
Claims (9)
1. A snake-shaped robot target stable tracking method based on a laser radar is characterized by comprising the following steps:
s1, acquiring a pitching Euler angle of the head gesture of the snake-shaped robot and an expected pitching angle of a relative horizontal plane measured by an IMU sensor in real time, and controlling the advancing movement gait of the snake-shaped robot;
s2, calculating a deviation angle of the head according to the actual pitching Euler angle of the head and the expected pitching angle of the head relative to the horizontal plane, which are measured by the IMU sensor;
s3, according to the deviation angle of the head at the current i time and the joint angle of the head measured by a position sensor on the joint, compensating and updating the expected joint angle of the head joint at the i+1 time in real time through sensor data;
s4, determining the position of the target object in a local coordinate system of the laser radar according to the point cloud data of the surrounding environment;
s5, calculating the azimuth angle of the target object relative to the axis of the snake-shaped robot according to the position of the target object based on the laser radar local coordinate system, and adjusting the advancing direction of the snake-shaped robot according to the azimuth angle.
2. The stable tracking method according to claim 1, characterized in that: in step S1, specifically, the method includes:
the laser radar is horizontally arranged at the head of the snake-shaped robot, the snake-shaped robot is controlled to advance by adopting peristaltic gait, and the motion gait equation of the peristaltic gait control of the snake-shaped robot is as follows:
；
in the above formula, A represents the amplitude of the change of the joint angle,the joint angle of the nth joint angle at time k+1, +.>To control the frequency of the peristaltic gait over time, it is used to control the rate of progression of the peristaltic gait, +.>Air for peristaltic gaitInter-frequency (F)>To control the movement turning control quantity of the snake-shaped robot at the k+1 time.
3. The stable tracking method according to claim 1, characterized in that: in step S2, the calculation formula of the deviation angle of the head is:
；
in the above-mentioned method, the step of,for a desired pitch angle of the head relative to the horizontal, +.>Actually measuring the head pitch Euler angle for IMU sensor, < >>Is the angle of departure of the head.
4. A stable tracking method according to claim 2 or 3, characterized in that: desired pitch angle relative to horizontalSet to 0 degrees.
5. The stable tracking method according to claim 1, characterized in that: in step S3, the desired joint angle of the head jointThe updated formula of (2) is:
；
wherein the method comprises the steps ofIs->The joint angle of the head, measured by a position sensor on the moment joint, < >>Is->Deviation angle of time head->Is->Desired joint angle of the head joint at the moment.
6. The stable tracking method according to claim 1, characterized in that: in step S4, the specific process includes the following steps:
s41, acquiring a point cloud data set Q of a single scanning period of the surrounding environment of the head;
s42, the set point cloud data set Q comprisesPersonal point cloud data->And calculating point cloud data from the lidar sensor information +.>，/>The +.>The point cloud data P;
s43, setting the sliding point length asAnd let->；
S44, slave point cloud dataStart slide continuous taking ∈ ->Personal point cloud data->As a point cloud sequenceGo on->A plurality of fitting errors are determined by sub-matching>；
If it isThen get the point cloud data +.>Performing a round least square fit and calculating a fit errorThen returns to step S44;
if it isStep S45 is entered;
s45, according to a plurality of fitting errorsCalculating the minimum match error +.>；
S46, according to the minimum matching errorThe group of matching with the minimum corresponding error is used for determining the position of the target object in the local coordinate system of the laser radar;
if the matching error is minimumStep S5 is entered;
if the matching error is minimumThen return to step S41.
7. The stable tracking method according to claim 1, characterized in that: in step S5, azimuth angleThe calculation formula of (2) is as follows:
；
in the above，/>Localized laser radar for the head axisUnit direction vector in coordinate system, +.>For the origin of the local coordinate system of the laser radar to the fitting circle center +.>Is>For the deflection angle of the target object with respect to the head of the serpentine robot, < >>Is a two-parameter arctangent function, det%M) Representation matrixMIs a determinant of (2).
8. The stable tracking method according to claim 1, characterized in that: in step S5, the specific process includes the following steps:
s51, calculating the center distance of the center of the target object relative to the laser radar in the head of the robot；
S52, setting the period time of laser radar scanning asFirst->And->The center distance between the center of the target object and the center of the laser radar at the moment is +.>And->And calculate the target->Time of day relative to the speed of movement of the lidar>；
If it isAnd->The stable tracking of the snake-shaped robot to the target is completed;
if it isThe frequency of the peristaltic gait changing with time is controlled to be gradually increased with proper step length +.>Up to->Less than 0 and up to a maximum movement speed, i.e. the speed of movement of the snake-shaped robot is greater than the speed of the target object, the snake-shaped robot approaches the target object at maximum speed, when +.>At the same time, the frequency of controlling the peristaltic gait to change with time is gradually reduced with proper step lengthUp to->For completing the automatic adjustment of the target tracking speed of the snake-shaped robot,/for the control of the target tracking speed of the snake-shaped robot>For the movement speed threshold, ++>Is the center distance threshold.
9. The stable tracking method according to claim 8, characterized in that: in step S52, a threshold valueThe value of (2) is 0.2cm/s, threshold +.>The value of (2) is 5cm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310722790.3A CN116476078B (en) | 2023-06-19 | 2023-06-19 | Laser radar-based stable target tracking method for snake-shaped robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310722790.3A CN116476078B (en) | 2023-06-19 | 2023-06-19 | Laser radar-based stable target tracking method for snake-shaped robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116476078A CN116476078A (en) | 2023-07-25 |
| CN116476078B true CN116476078B (en) | 2023-09-05 |
Family
ID=87219847
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310722790.3A Active CN116476078B (en) | 2023-06-19 | 2023-06-19 | Laser radar-based stable target tracking method for snake-shaped robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116476078B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201703755D0 (en) * | 2017-03-09 | 2017-04-26 | Rolls Royce Plc | Continuum robots |
| CN110103218A (en) * | 2019-05-13 | 2019-08-09 | 中山大学 | The quick self-adapted control method of snake-shaped robot pipeline climbing |
| CN113467249A (en) * | 2021-07-23 | 2021-10-01 | 福州大学 | Self-adaptive path following controller of snake-shaped robot based on tracking error and time-varying coefficient prediction and design method thereof |
| AU2021105416A4 (en) * | 2021-08-13 | 2021-10-14 | Xidian University | Motion data processing method of snake-shaped rope-driven robot arm provided with rope-hole gaps |
| CN114035240A (en) * | 2021-09-29 | 2022-02-11 | 中南大学 | Detection device and method for filling non-roof-connected empty area based on snake-shaped robot |
| CN115256376A (en) * | 2022-07-11 | 2022-11-01 | 北京科技大学 | Variable-diameter climbing gait control method of snake-shaped robot based on Bezier curve |
| CN116061191A (en) * | 2023-03-16 | 2023-05-05 | 上海理工大学 | Model prediction control method for active obstacle avoidance of bionic snake-shaped robot |
| CN116237950A (en) * | 2023-04-03 | 2023-06-09 | 哈尔滨工业大学 | Robot tail end accurate control method and equipment based on sectional motion planning strategy |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11034026B2 (en) * | 2019-01-10 | 2021-06-15 | General Electric Company | Utilizing optical data to dynamically control operation of a snake-arm robot |
-
2023
- 2023-06-19 CN CN202310722790.3A patent/CN116476078B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201703755D0 (en) * | 2017-03-09 | 2017-04-26 | Rolls Royce Plc | Continuum robots |
| CN110103218A (en) * | 2019-05-13 | 2019-08-09 | 中山大学 | The quick self-adapted control method of snake-shaped robot pipeline climbing |
| CN113467249A (en) * | 2021-07-23 | 2021-10-01 | 福州大学 | Self-adaptive path following controller of snake-shaped robot based on tracking error and time-varying coefficient prediction and design method thereof |
| AU2021105416A4 (en) * | 2021-08-13 | 2021-10-14 | Xidian University | Motion data processing method of snake-shaped rope-driven robot arm provided with rope-hole gaps |
| CN114035240A (en) * | 2021-09-29 | 2022-02-11 | 中南大学 | Detection device and method for filling non-roof-connected empty area based on snake-shaped robot |
| CN115256376A (en) * | 2022-07-11 | 2022-11-01 | 北京科技大学 | Variable-diameter climbing gait control method of snake-shaped robot based on Bezier curve |
| CN116061191A (en) * | 2023-03-16 | 2023-05-05 | 上海理工大学 | Model prediction control method for active obstacle avoidance of bionic snake-shaped robot |
| CN116237950A (en) * | 2023-04-03 | 2023-06-09 | 哈尔滨工业大学 | Robot tail end accurate control method and equipment based on sectional motion planning strategy |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116476078A (en) | 2023-07-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | Qualitative vision-based path following | |
| WO2023273107A1 (en) | Robot walking control method and apparatus, robot control device, and storage medium | |
| WO2018209864A1 (en) | Moving control method and device, robot and storage medium | |
| CN110955262B (en) | Control method and system for path planning and tracking of photovoltaic module cleaning robot | |
| CN109960145B (en) | Mobile robot mixed vision trajectory tracking strategy | |
| CN109782810B (en) | Video satellite moving target tracking imaging method and device based on image guidance | |
| CN109189060B (en) | Point stabilization control method and device for mobile robot | |
| US11245841B2 (en) | Method for marking target position of gimbal, gimbal, and photographing device | |
| CN104964683B (en) | A kind of closed-loop corrected method of indoor environment map building | |
| CN112000135B (en) | Three-axis holder visual servo control method based on human face maximum temperature point characteristic feedback | |
| JP6801269B2 (en) | Autonomous mobile device | |
| Mills et al. | Vision based control for fixed wing UAVs inspecting locally linear infrastructure using skid-to-turn maneuvers | |
| CN110824524B (en) | Satellite video transmission system based on airborne Ka wave band | |
| CN116476078B (en) | Laser radar-based stable target tracking method for snake-shaped robot | |
| CN107492717B (en) | Inertial navigation course correction method for communication-in-moving antenna cosine scanning | |
| JP5888115B2 (en) | TRACKING DEVICE AND TRACKING METHOD | |
| CN114019798B (en) | Robot track tracking control method, magnetic medical robot and storage medium | |
| JP2021136009A (en) | Information processing apparatus, information processing method, and program | |
| CN110231823B (en) | Direct control method of two-wheeled robot | |
| Kurdi et al. | Trajectory and Motion for Agricultural Robot | |
| JP6227950B2 (en) | Sensor orientation control method and apparatus | |
| KR101164336B1 (en) | Base plane height compensation method of mobile robot and generating method for elevation map using the same | |
| JP4515884B2 (en) | Camera stabilizer target position correction method | |
| JP6319736B2 (en) | Sensor orientation control method and apparatus | |
| CN114526740A (en) | Monocular automatic driving navigation method, system and device based on natural object |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |