Disclosure of Invention
The invention aims to provide a method for planning a movement track and avoiding an obstacle of a wheeled double-arm humanoid robot, which aims to solve the problems in the background technology.
In order to achieve the purpose, the invention provides the following technical scheme that the method for planning the movement track and avoiding the obstacle of the wheeled double-arm humanoid robot comprises the following steps:
S01, starting a robot system for self-checking;
S02, acquiring environmental information by a first sensor, and constructing a real-time three-dimensional point cloud obstacle information graph;
s03, a second sensor acquires chassis state information and state information of the mechanical arm;
S04, setting a target task, and planning a moving path of a chassis and a moving path of a mechanical arm based on state information of the target task;
S05, controlling the chassis to move according to the planned moving path through a cooperative control model, and enabling the mechanical arm to move according to the planned moving track;
S06, the second sensor detects the environment data and the robot state in real time, and updates a second planning working path through a D-Lite algorithm based on the real-time environment data and additional constraint conditions to generate a real-time moving path of the chassis;
S07 reaches the target task position, the chassis is adjusted based on the object state information, and the object is grabbed from the initial position by matching with the mechanical arm.
Preferably, the first sensor acquires environmental information, constructs a real-time three-dimensional point cloud obstacle information map, wherein,
The first sensor is a laser radar;
The laser radar emits laser beams, analyzes the reflected signals, and draws a three-dimensional point cloud image of the surrounding environment, wherein each point represents a specific position in space;
identifying and tracking the obstacle by using a point cloud segmentation algorithm, extracting the position information of the obstacle from the point cloud, marking in a navigation system, and generating a three-dimensional point cloud obstacle information map.
Preferably, the second sensor acquiring chassis state information and state information of the mechanical arm includes:
the second sensor is an accelerometer and a gyroscope;
The state information of the chassis specifically comprises position, speed and posture information of the chassis;
The state information of the mechanical arm specifically comprises the position of the double arms and the movement data of the double arms.
Preferably, the setting the target task plans a moving path of the chassis based on state information of the target task, wherein the state information of the target task includes position information and space information.
Preferably, the setting the target task, planning the movement path of the chassis and the movement path of the mechanical arm based on the state information of the target task includes:
s401, planning a target path required to move by the chassis based on the position information of the target task, and obtaining a first planning working path by using a path planning algorithm;
S402, planning a motion track of the mechanical arm based on the space information of the target task, and calculating the maximum contour range of the mechanical arm in the motion process through inverse kinematics;
s403, inputting the obtained mechanical arm movement profile into an environment model to generate an expansion barrier;
S404, optimizing the first planning working path by using a path planning algorithm based on the generated expansion obstacle to obtain a second planning working path.
Preferably, the path planning algorithm is an a-algorithm.
Preferably, the system controls the chassis to move according to the planned moving path through a cooperative control model, and the mechanical arm moves according to the planned moving track,
The synchronous motion information of the chassis comprises speed, direction and motion trail
The synchronous motion information of the robotic arm includes arm extension, rotation and any possible swing.
Preferably, the second sensor detects the environmental data and the robot state in real time, updates the second planning working path through a d×lite algorithm based on the real-time environmental data and an additional constraint condition, and generates an extended obstacle calculated in real time in the real-time moving path of the chassis.
Preferably, the robot further comprises a memory, a processor and a path planning and obstacle avoidance program of the robot stored on the memory and executable on the processor, the path planning and obstacle avoidance program of the robot being configured to implement the steps of the obstacle avoidance method of the industrial robot according to any one of claims 1 to 8.
Compared with the prior art, the invention has the beneficial effects that:
According to the invention, through the construction and real-time updating of the real-time three-dimensional point cloud obstacle information graph, the robot can effectively identify and avoid obstacles, reduce collision risks with surrounding environments, adapt to dynamically-changed environments, and rapidly update path planning through a D-Lite algorithm, so that the paths can be timely adjusted when the obstacles move or new obstacles appear;
According to the invention, independent control of the robot chassis and the mechanical arm is realized through the cooperative control model, cooperative work is realized through a communication protocol, the task execution efficiency is improved, meanwhile, the robot can accurately control the movement of the robot by utilizing the chassis and the mechanical arm state information acquired by the sensor, and the end effector of the mechanical arm can accurately reach the position of a target object;
According to the invention, through data fusion of various sensors and systems, such as a laser radar, a camera, an infrared sensor and the like, the robot can obtain more complete and vivid three-dimensional environment representation, and the robustness of the system is enhanced;
According to the invention, the working path is optimized through the path planning algorithm, the robot can reach the target position by the shortest path, time and energy are saved, and meanwhile, the parameters of the actuator are adjusted according to the state information of the target object, so that the object can safely and accurately move from the initial position to the target position;
the robot design comprises a processor, a communication bus, a user interface and a network interface, so that the robot can be compatible with various external equipment and network environments, expansion and integration are convenient, the robot can store and execute programs through the arrangement of the memory, and parameters for the industrial robot are stored at the same time, so that the efficiency and stability of data processing are improved.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Example 1
The obstacle avoidance method in the embodiment is applied to an industrial robot, wherein the industrial robot can be a multi-joint manipulator or a humanoid robot, the humanoid robot is provided with a chassis and a double-arm manipulator, and the chassis moves through executing a navigation module to drive the whole robot to move. Specifically, the motion trail planning and obstacle avoidance method of the robot in the embodiment includes:
S01, starting a robot system for self-checking;
S02, acquiring environmental information by a first sensor, and constructing a real-time three-dimensional point cloud obstacle information graph;
s03, a second sensor acquires chassis state information and state information of the mechanical arm;
S04, setting a target task, and planning a moving path of a chassis and a moving path of a mechanical arm based on state information of the target task;
S05, controlling the chassis to move according to the planned moving path through a cooperative control model, and enabling the mechanical arm to move according to the planned moving track;
S06, the second sensor detects the environment data and the robot state in real time, and updates a second planning path through a D-Lite algorithm based on the real-time environment data and additional constraint conditions to generate a real-time moving path of the chassis;
S07 reaches the target task position, the chassis is adjusted based on the object state information, and the object is grabbed from the initial position by matching with the mechanical arm.
In one embodiment, the step S01 starts the robot system for self-checking, and specifically comprises the following steps:
In the system start-up phase, hardware and software in each module are required to be self-checked, and it can be understood that the system specifically includes power supply checking, whether the motion joint parts of the mechanical arm and the chassis are flexible, no jamming or damage, calibration of each sensor installed on the system, ensuring that the acquired data are accurate, whether the navigation system of the chassis and the motion system of the mechanical arm in the robot are all running normally, and the like.
In one embodiment, the S02 first sensor acquires environmental information and constructs a real-time three-dimensional point cloud obstacle information graph, and the method specifically comprises the following steps:
The first sensor is a laser radar;
The laser radar emits laser beams, analyzes the reflected signals, and draws a three-dimensional point cloud image of the surrounding environment, wherein each point represents a specific position in space;
identifying and tracking the obstacle by using a point cloud segmentation algorithm, extracting the position information of the obstacle from the point cloud, marking in a navigation system, and generating a three-dimensional point cloud obstacle information map.
It can be understood that, after the robot in this embodiment starts, from the initial position, the robot navigates to move to the target position, drives the two arms to grasp the object, moves to form a point-to-point predicted track, and in the predicted track, people and objects in the obstacle avoidance range of the robot, collectively referred to as obstacles, the system needs to re-plan the working path based on the data acquisition of the obstacles.
The first sensor may be any sensor that can be used to collect data of surrounding environment, such as a laser radar, a camera, and an infrared sensor, and in this embodiment, the first sensor is defined as a laser radar, and by emitting a series of rapidly rotating laser beams to the surrounding environment, a laser radar receiver captures signals reflected back by the laser beams when encountering obstacles, and by analyzing the intensity, time, and direction of these reflected signals, three-dimensional coordinates of each reflection point relative to the sensor are accurately calculated, so that a high-precision three-dimensional point cloud image of the surrounding environment is drawn, where each point represents a specific position in space.
Identifying and tracking the obstacle from the known acquired point cloud data by using a point cloud segmentation algorithm, wherein the obstacle is divided into an animal and a still, the still is kept motionless in an image, the still can be used as basic data in the primary path planning, the animal needs to mark in the image, and the animal keeps tracking the obstacle after moving, so that effective obstacle avoidance is realized; the point cloud segmentation algorithm can segment the point cloud data into different clusters or objects based on the geometric features of the points, the spatial distribution, the reflection intensity of the laser beams and the like, each cluster represents a potential obstacle, the position, the shape, the moving speed and the like of the obstacle can be extracted in real time by continuously tracking the change of the clusters, the obstacle information is input into a navigation system for marking and positioning, and a three-dimensional point cloud obstacle information graph can be generated for providing data support and reference for path planning of the robot.
In addition, the three-dimensional point cloud obstacle information graph can be built through a stereoscopic vision system, the same scene is shot through two or more cameras from different angles, three-dimensional coordinates of each point in the scene are calculated through the parallax principle, the three-dimensional point cloud obstacle information graph can be built through the installation and use of the laser radar based on the installation and use of the laser radar, the laser radar in the structure light system projects light beams with known patterns to the surrounding environment, the deformation condition of the shooting patterns of the cameras is utilized, the three-dimensional shape of the scene is restored through calculating the difference between the deformation patterns and the original patterns, and the three-dimensional shape of the scene can be understood.
In one embodiment, the second sensor in step S03 obtains chassis status information and status information of the mechanical arm, and specifically includes:
The state information of the chassis specifically comprises position, speed and posture information of the chassis;
The state information of the mechanical arm specifically includes the position of the two arms and the movement data of the two arms.
The second sensor is any sensor capable of measuring the angular velocity and acceleration of an object, such as an accelerometer, a gyroscope and the like, the gyroscope is mainly arranged in the chassis, the attitude change of the chassis can be obtained through integrating the angular velocity data, the current attitude of the chassis is determined by combining initial attitude information, and the linear velocity and position information of the chassis can be calculated indirectly by combining the data obtained by the accelerometer. The estimating step can refer to the following specific steps that under the static or uniform linear motion state, the measured value of the accelerometer is a component of the gravity acceleration g, and under the dynamic state, the measured value of the accelerometer is a combination of the gravity acceleration and the chassis motion acceleration, so that in order to calculate the linear velocity and the position information of the chassis from the acceleration data, the influence of the gravity acceleration needs to be removed firstly to obtain the motion acceleration of the chassis relative to the ground, then the motion acceleration is integrated to obtain the linear velocity of the chassis, and then the linear velocity is integrated to obtain the position information of the chassis.
The related formulas are:
a= [ ax, ay, az ] (unit: m/s 2), v ̇ = a.
Wherein a is a motion acceleration vector of the chassis, and v is a linear velocity vector.
V= [ vx, vy, vz ] (unit: m/s), then p ̇ = v
Where p is the position vector of the chassis.
Secondly, the accelerometer or gyroscope can be used for acquiring state information of the mechanical arm, the state information specifically comprises initial positions of the two arms and motion data of the two arms, the system acquires initial motion data of the two arms from a database, the initial motion data comprise but not limited to joint angle, speed and strength information of the mechanical arm, after the mechanical arm is started, sensors arranged on all joints of the mechanical arm can be used for measuring the angular speed of the joints, and the rotation angle of the joints and the overall posture change of the mechanical arm are obtained through integrating the angular speed data. Besides, the moment sensor can be loaded for measuring the moment born by the joint, and the load and stress conditions born by the mechanical arm in the motion process can be known after the measurement data are analyzed, so that more accurate control and adjustment are realized.
In one embodiment, the step S04 is to set a target task and plan a moving path of a chassis and a moving path of a mechanical arm based on state information of the target task, and specifically comprises the following steps:
s401, planning a target path required to move by the chassis based on the position information of the target task, and obtaining a first planning working path by using a path planning algorithm;
S402, planning a motion track of the mechanical arm based on the space information of the target task, and calculating the maximum contour range of the mechanical arm in the motion process through inverse kinematics;
s403, inputting the obtained mechanical arm movement profile into an environment model to generate an expansion barrier;
S404, optimizing the first planning working path by using a path planning algorithm based on the generated expansion obstacle to obtain a second planning working path.
Firstly, a specific task to be completed by the robot is defined, in this embodiment, a target task position to be moved by the robot, namely, a position of the required object to be moved, is preset, a relative position of the target task position relative to a current position of the robot is obtained based on construction of a three-dimensional point cloud obstacle information map, and an optimal movement path of the chassis, namely, a first planning working path, is calculated according to map information, the obstacle position and the target position. And secondly, determining the optimal motion track of the mechanical arm when the target task is completed based on the spatial information of the target task, acquiring the spatial information of the target task in advance, including the size and shape of an object to be carried, required grabbing operation and the like, determining the joint angle and the joint speed of the mechanical arm when the task is executed through inverse kinematics calculation, planning the motion track of the mechanical arm, calculating the maximum contour range of the mechanical arm in the motion process, evaluating the space possibly occupied by the mechanical arm in the motion process, and avoiding collision with obstacles.
For the n degrees of freedom (DOF) serial mechanical arms in this embodiment, the joint angle of the mechanical arm needs to be calculated based on Denavit-Hartenberg (DH) parameters, so as to determine that the joint angle of the mechanical arm reaches a specific end position, that is, the joint angle data of the mechanical arm reaches the maximum contour range, and then calculate the maximum contour range of the mechanical arm.
It can be understood that the calculated maximum outline range of the mechanical arm is taken as an expansion obstacle in consideration of the space possibly occupied by the mechanical arm in the motion process, a known three-dimensional point cloud obstacle information map is input, the known three-dimensional point cloud obstacle information map comprises all possible positions of the mechanical arm when the mechanical arm executes a task, the three-dimensional point cloud obstacle information map is updated to comprise the mechanical arm serving as an expansion obstacle, and the first planning working path is optimized again by using a path planning algorithm based on the update of the three-dimensional point cloud obstacle information map, so that a second planning working path is generated. The second planning working path reflects the actual motion requirement of the robot when the robot executes the task, and the optimized path can ensure that the chassis cannot collide with the expansion barrier in the moving process, so that the safety when the planning path is executed is kept.
It should be noted that, the path planning algorithm of the present invention mainly relies on an a-type algorithm, and finds the shortest path from the initial position to the target task position on the three-dimensional point cloud obstacle information map through heuristic search.
In one embodiment, the S05 system controls the chassis to move according to the planned moving path and the mechanical arm to move according to the planned moving track through a cooperative control model, and specifically comprises the following steps:
The embodiment uses a cooperative control model to separate the controllers of the chassis and the mechanical arm, and realizes cooperative control through communication protocols such as a CAN bus, and the like, and the embodiment comprises the steps of sending motion instructions to the chassis and the mechanical arm, executing a pre-planned moving path, and adjusting the chassis and the mechanical arm in real time according to feedback of the sensors in the motion process so as to cope with environmental changes.
In cooperative control, the robot moves and moves according to a second planning working path, and meanwhile, the motion information of the chassis and the mechanical arm is integrated through development of a sensor fusion algorithm, so that the overall accuracy and reliability of the system are improved. The synchronous motion information of the chassis comprises speed, direction and motion track, and the synchronous motion information of the mechanical arm comprises extension, rotation and any possible swing of the double arms.
The CAN bus communication protocol CAN specifically plan a motion path of the chassis and the mechanical arm according to the service requirement, send motion instructions to controllers of the chassis and the mechanical arm through the CAN bus, detect environmental changes in real time through respective sensors in the motion process of the chassis and the mechanical arm, feed detection information back to the control system through the CAN bus, and the control system adjusts in real time according to the feedback information so as to ensure that the chassis and the mechanical arm CAN accurately move according to the preset path.
In one embodiment, step S06, the second sensor detects the environmental data and the robot state in real time, updates the second planning path through a d×lite algorithm based on the real-time environmental data and the additional constraint condition, and generates a real-time moving path of the chassis, which specifically includes:
In this embodiment, the robot is in a factory or outdoor environment without a map of the surrounding environment, after the robot is started, the robot starts from an initial position, navigates and moves to a target position, drives two arms to grasp an object, moves to form a point-to-point predicted track, and continuously updates gesture data of the robot in the process of executing a second planning path by the robot, the first sensor continuously detects environmental changes, such as movement of an obstacle or a newly-appearing obstacle, determines an additional point based on obstacle information and an obstacle avoidance range of an extended obstacle, and re-plans the second planning path according to the additional point and the target task point.
The updating of the second planning path depends on a D Lite algorithm, the path planning efficiency is improved by incrementally updating the path information, and the path planning can be quickly updated according to real-time road condition information, so that the robot can quickly adjust the path when moving in the face of an obstacle or changing the environment, and the robot can safely reach a target task point. Compared with the A algorithm, the A algorithm needs to re-plan the path from the beginning in the dynamic environment, and the D Lite algorithm can quickly adjust the path through incremental updating, so that the path planning efficiency in the dynamic environment is higher.
The D Lite algorithm recalculates a more optimal path of travel based on environmental changes and newly emerging obstacles, and the chassis control system immediately receives this new path information and responds quickly. The control system firstly evaluates the current motion state of the chassis, and then performs necessary adjustment according to the requirement of a new path, wherein the adjustment comprises changing the driving direction, accelerating or decelerating and the like so as to ensure that the chassis can stably and rapidly advance along the new path.
In one embodiment, the step S07 reaches the target task position, the chassis adjusts based on the object state information, and the chassis is matched with the mechanical arm to grasp the object from the initial position, which specifically includes:
When the robot moves to the vicinity of the target task position, the mechanical arm can conduct accurate pose adjustment according to the position and the pose of the chassis and the relative position of the target task position relative to the chassis so as to ensure that the tail end of the mechanical arm can accurately reach the initial position of the target object, then according to the recognized state information of the target object, the system can adjust an actuator at the tail end of the mechanical arm to adapt to the object, adjusting execution parameters including the size and the shape of the object, adjusting parameters such as the opening size and the clamping force of the actuator, and the like, and based on the adjusted execution parameters, the mechanical arm can conduct grabbing action to move the object from the initial position to the target position.
Further, after the object is moved to the target position, the system uses equipment such as a camera to shoot a picture or video of the object at the target position, compares the picture or video with a preset target position, ensures that the posture of the object at the target position accords with the expected posture, and has no problems such as tilting, inversion and the like, so as to ensure that the task is completed successfully.
Example 2
The robot assembly described in the embodiments of the present invention includes a processor, such as a CPU, and a communication bus, user interface, network interface, and memory. Wherein a communication bus connects these components and enables communication connection of the large components. The user interface is of a rich design, covering not only the display and input unit (e.g. keyboard), but also optionally standard wired and wireless interfaces. The network interface is also flexible and can optionally integrate a standard wired interface or a wireless interface (e.g., wi-Fi).
The memory is configured in the main body of the industrial robot, is used for carrying a program capable of realizing a specific function when being executed, and is responsible for storing parameters required by the operation of the industrial robot, and the types of the memory are various, and can be a high-speed RAM memory for pursuing performance, a stable nonvolatile memory (such as a magnetic disk memory) or even a storage unit which is independent of a processor.
It is noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises an element.
The foregoing is merely illustrative of the present invention and not restrictive, and other modifications and equivalents thereof may occur to those skilled in the art without departing from the spirit and scope of the present invention.