Accurate collision-free automatic docking control method

Abstract

The invention relates to an accurate collision-free automatic docking control method which is characterized by comprising the following steps: S1, remote guidance: triggering an automatic docking signal, establishing a three-dimensional semantic map, completing autonomous positioning, planning a docking path, performing real-time obstacle avoidance navigation, and executing safe and stable movement to reach a pre-docking position; s2, posture adjustment: when reaching the pre-docking position, determining the target posture of the target docking object and the posture of the object to be docked, and adjusting the posture of the object to be docked to the pre-docking posture; and S3, short-range docking: based on motion compensation, detecting a motion error, generating a motion correction instruction according to error signal feedback, and realizing collision-free accurate docking. The movement function and the body position changing function of the intelligent wheelchair are achieved through the intelligent wheelchair, smooth and stable switching can be achieved in the changing process of the intelligent wheelchair, and meanwhile the intelligent wheelchair is an indoor power assisting system, has the obstacle avoiding walking function and can be in collision-free automatic butt joint with a bed body and a closestool.

Description

Technical Field

[0001] The present invention relates to the field of intelligent control technology, and in particular to a precise collision-free automatic docking control method. Background Art

[0002] In order to meet the needs of patients and the elderly to change their body positions such as lying down and standing, and to reduce the labor intensity of nursing staff, an intelligent assistive system that meets daily needs and is easy to operate and control has a more practical significance. The existing assistive systems have the following problems: poor flexibility and operability when in a wheelchair; when in a lying position, its width and height cannot meet the nursing requirements and lack comfort; the existing assistive systems have a complex conversion process between sitting, lying and standing positions.

[0003] In the prior art, a separated walking-aid nursing bed comprises a fixed bed surface part, a separated bed surface part, a bed frame, a back-lifting mechanism and a leg-bending mechanism: the fixed bed surface part is divided into a U-shape, and the separated bed surface part can be just embedded in the U-shaped groove of the fixed bed surface part, so that the fixed bed surface part and the separated bed surface part form a rectangular integral bed surface; the bed frame comprises an integral bracket with casters and a movable bracket connected thereto by a rotating connecting shaft: the back-lifting mechanism is composed of a four-bar mechanism and a movable actuator; the leg-bending mechanism comprises left and right supports symmetrically fixedly connected to the integral bracket, on which left and right motors are respectively installed, and the output shafts of the left and right motors are respectively connected to the left and right gear pairs, and the left and right gear pairs are respectively connected to one end of the left and right cranks, and the other ends of the left and right cranks are respectively fixedly connected to the fixed bed surface part. This technology has greatly enhanced the flexibility of the nursing bed and added a body position change function, but it is only a nursing bed, and its auxiliary function for the passengers is insufficient. It cannot be changed into a wheelchair to help the passengers to carry out outdoor activities. It cannot realize the docking and positioning function, and the control lacks a certain degree of intelligence.

[0004] In summary, there is an urgent need to provide a control method that can achieve accurate and collision-free automatic docking of an intelligent wheelchair with a bed and a toilet. Summary of the invention

[0005] The purpose of the present invention is to provide a control method that can realize accurate and non-collision automatic docking of an intelligent wheelchair with a bed and a toilet.

[0006] The above object is achieved through the following technical solution: a precise collision-free automatic docking control method, comprising the following steps:

[0007] S1, remote guidance: automatic docking signal triggering, building a three-dimensional semantic map, completing autonomous positioning and planning the docking path, real-time obstacle avoidance navigation, and executing safe and smooth movement to reach the pre-docking position;

[0008] S2, posture adjustment: Arrive at the pre-docking position, determine the target posture of the target docking object and the own posture of the object to be docked, and adjust its own posture to the pre-docking posture;

[0009] S3, short-range docking: Based on motion compensation, motion errors are detected, and motion correction instructions are generated according to error signal feedback to achieve collision-free and precise docking.

[0010] The intelligent wheelchair used in the multifunctional bed-chair integrated device of the present invention is separated from the bed body and moves to automatically dock with the toilet and the bed body. The multifunctional bed-chair integrated device consists of two parts: an intelligent wheelchair and a bed body. The intelligent wheelchair is used to realize its movement function and body position change function. At the same time, the docking of the bed body and the wheelchair widens the activity space of the user in a lying position. The intelligent wheelchair is an indoor auxiliary system, which can enable the intelligent wheelchair body to have the function of avoiding obstacles and walking, and automatically driving into the bed body or toilet for docking.

[0011] The control method of the present invention mainly includes a long-range guidance stage, a posture adjustment stage and a short-range docking stage. In the first stage, a three-dimensional semantic map is established through multi-sensor fusion, autonomous positioning is completed based on the map information, the optimal docking path is planned, the intelligent wheelchair navigates in real time to avoid obstacles, and performs safe and smooth movement to reach the pre-docking position. In the second stage, the intelligent wheelchair identifies the docking target and posture based on the three-dimensional semantic map, and through the closed-loop feedback of its own inertial sensor and encoder information, the omnidirectional chassis moves, and the intelligent wheelchair is adjusted to the docking posture; in the third stage, the motion compensation method is studied, the motion error of the intelligent wheelchair is detected online, and the motion correction instruction is generated according to the error signal feedback to achieve collision-free and precise docking of the chair-bed / barrel.

[0012] A further technical solution is that in step S1, a three-dimensional semantic map of each area of ​​the environment is constructed by machine vision scanning, the constructed three-dimensional semantic map is loaded, the collected image recognition scene is used to obtain its own position in the three-dimensional semantic map, the navigation system is used to receive the task target point to be reached, the path is planned and the motion control quantity is generated in the constructed three-dimensional semantic map, and real-time dynamic obstacle avoidance is performed based on the artificial potential field method.

[0013] It should be noted that the task target point here is the pre-docking position, such as the set docking position of the bed and the toilet.

[0014] A further technical solution is that in step S1, the optimal docking path planning is realized based on the improved A-Star algorithm, and the coordinates of the two nodes are set to be p and i (x i ,y i ) and p j (x j ,y j ), set the path line equation to y = kp (xx i )+y i , and determine whether the distance between the route and the obstacle is safe, where the slope of the equation of the path line

[0015] In the process of intelligent wheelchair path planning, on the one hand, it is necessary to determine the initial position of the intelligent wheelchair and combine it with the target position for global optimal path planning. On the other hand, it is also necessary to track the intelligent wheelchair in real time to complete local path planning and obstacle avoidance. The accuracy requirement of the remote guidance stage is not high. It only needs to reach the pre-docking position to ensure the optimal path planning and safe and autonomous obstacle avoidance to a greater extent. The optimal path planning based on the improved A-Star algorithm and the real-time dynamic obstacle avoidance based on the artificial potential field method are proposed.

[0016] The A-Star algorithm is the most classic and commonly used path planning method, which can be regarded as an extension of the Dijkstra algorithm and has been widely used in the manufacturing industry and the field of smart wheelchairs. The present invention proposes an optimal docking path planning based on an improved A-Star algorithm, optimizes its evaluation function, improves the algorithm's path search efficiency, and adjusts the generated path to avoid oblique penetration of obstacle vertices.

[0017] A further technical solution is that, in step S1, the step of performing real-time dynamic obstacle avoidance based on the artificial potential field method comprises:

[0018] An artificial potential field is established, and the mission target point generates a predetermined attraction to attract the object to be docked to move toward the mission target point. The obstacles generate a repulsive force to prevent the wheelchair from colliding with the obstacles. Under the action of the repulsive forces of multiple obstacles and the predetermined attraction generated by the mission target point, the object to be docked moves along the direction of the resultant force.

[0019] When the smart wheelchair performs local planning, it needs to rely on multiple sensors (camera, lidar, IMU, encoder) to integrate the surrounding environment information. According to the changes in obstacle information detected in real time, the artificial potential field method is used for planning to avoid dynamic obstacles in time to reach the target point. When the smart wheelchair moves from the starting point to the end point, it is affected by the simulated attraction or repulsion, which affects the choice of path. In the artificial potential field, the end point generates an attraction, which attracts the smart wheelchair to move toward the end point, and the obstacles that hinder the passage generate repulsion to prevent the wheelchair from colliding with obstacles. Under the combined effect of multiple repulsion and gravity, the mobile smart wheelchair will move in the direction of the resultant force.

[0020] A further technical solution is that in the step S1, during the process of establishing the artificial potential field, by improving the repulsive field function, when the object to be docked searches for a path near the task target point, the attraction of the task target point to the object to be docked is greater than the repulsion of the task target point to the object to be docked, thereby ensuring that the mobile intelligent wheelchair moves toward the task target point.

[0021] In this way, an improved random walk method is designed to prevent the smart wheelchair from falling into the local optimal solution of the map. To avoid the object to be docked from being subjected to excessive repulsion near the mission target point, only the repulsion of the obstacle on it can prevent it from colliding with the obstacle. When the smart wheelchair is searching for a path, the gravitational force of the target point on it ensures that the smart wheelchair moves towards the destination, while the repulsion of the obstacle on it prevents the smart wheelchair from colliding with the obstacle.

[0022] A further technical solution is that the object to be docked is provided with an inertial sensor, a laser radar, an omnidirectional wheel and an encoder. In step S2, after reaching the pre-docking position, the target posture is determined by a three-dimensional semantic map, the own posture is determined by an inertial sensor, the orientation and measurement angle of the laser radar, and the measured distance between the object to be docked and the target docking object are obtained, the docking angle and direction are adjusted by a 360° rotation of the omnidirectional wheel, and the closed-loop feedback of the inertial sensor and encoder detection data is performed, and the omnidirectional chassis of the smart wheelchair moves according to the inertial sensor and encoder detection data to adjust the object to be docked to the docking posture.

[0023] It should be noted that the omnidirectional chassis is used for the bed-chair conversion of the smart wheelchair. When the smart wheelchair reaches the pre-docking position, the target posture is determined based on the three-dimensional semantic map, and its own posture is determined through the inertial sensor. At this time, the smart wheelchair adjusts its position and posture to the pre-docking posture. Multi-sensor fusion improves the accuracy and robustness of posture estimation.

[0024] A further technical solution is that the object to be docked is a smart wheelchair, the target docking object is a bed, and step S3 includes the following steps:

[0025] S3.1, determine three marking positions of the target docking object, wherein the first marking position is located below the bed surface on the front side of the bed body, the second marking position is located below the bed surface on the rear side of the bed body, and the third marking position is located below the bed surface on the side opposite to the opening in the bed body, and the first marking position, the second marking position, and the third marking position are respectively parallel to the side of the bed body, and the laser radar is installed on the seat frame of the wheelchair and is located on one side of the seat frame,

[0026] S3.2, establish a coordinate system, set the position of the laser radar as the origin, the direction in which the laser radar points directly behind the wheelchair is the X-axis direction, and the direction in which the laser radar points to the bed and is perpendicular to the X-axis is the Y-coordinate direction. The laser radar is used to obtain the coordinate points of each point at the first mark position in the coordinate system, the coordinate points of each point at the second mark position in the coordinate system, and the coordinate points of each point at the third mark position in the coordinate system. The acquired coordinate information is sent to the controller, and the controller performs motion compensation processing and plans the short-range docking path.

[0027] This makes it easier for the control system to calibrate and precisely dock. After the smart wheelchair completes precise posture adjustment, the laser radar and three marker positions are used to achieve the docking of the smart wheelchair with the bed. At the same time, the Kalman filter is planned to achieve motion compensation.

[0028] A further technical solution is that the step S3.2 also includes the following steps: fitting the straight line equations of the first mark position, the second mark position and the third mark position in the coordinate system, respectively, which are y1=a1+b1x1, y2=a2+b2x2 and y3=a3+b3x3, wherein (x1, y1) are the coordinate points of each point of the first mark position relative to the coordinate system, (x2, y2) are the coordinate points of each point of the second mark position relative to the coordinate system, (x3, y3) are the coordinate points of each point of the third mark position relative to the coordinate system, and a i is the intercept of the straight line on the Y axis, b i is the slope of the line; i is 1, 2 or 3;

[0029] The inclination angles θ of the three straight lines corresponding to the straight line equations of the first mark position, the second mark position and the third mark position in the coordinate system after fitting are calculated respectively. i and the distance d from the origin to the three straight lines i , the calculation formula is as follows:

[0030]

[0031] The calculated coordinates of the docking target point P in the coordinate system (x p ,y p ), and the deflection angle α of the second mark position relative to the omnidirectional chassis of the intelligent wheelchair. When the second mark position is parallel to the X-axis, α=0. The calculation formula is as follows:

[0032]

[0033] Wherein, d1 is the distance from the origin of the laser radar to the first marked position, d2 is the distance from the origin of the laser radar to the second marked position, d3 is the distance from the origin of the laser radar to the third marked position, θ1, θ2, θ3 are the inclination angles of the straight line equations of the first marked position, the second marked position and the third marked position in the coordinate system respectively;

[0034] When the wheelchair is located inside the bed and forms an integrated structure with the bed, the coordinates of the docking target point P are (x′ P ,y′ P ), the deflection angle of the docking side armrest relative to the omnidirectional chassis of the smart wheelchair is α′, and the angular velocity v of the driving motor of the smart wheelchair is calculated r , linear velocity v l , the calculation formula is as follows:

[0035]

[0036] A further technical solution is that the object to be docked is a smart wheelchair, and the target docking object is a toilet. In step S3.1, three marking positions of the target docking object are determined, wherein the first marking position is located below the toilet tank, the second marking position is located below one side of the toilet, and the third marking position is located below the other side of the toilet. The first marking position, the second marking position, and the third marking position are respectively parallel to the lower side of the toilet. In step S3.2, the distances of the three straight lines corresponding to the linear equations of the first marking position, the second marking position, and the third marking position in the coordinate system obtained after fitting are respectively calculated to be opposite to the origin; for the docking process of the smart wheelchair and the toilet, the laser radar will lose some line segments that have entered the wheelchair during the scanning process. It is proposed to adopt a method based on the line segment compensation of the bed / bucket template, and complete the line segments that have not been scanned according to the scanned model and the template matching existing in the system library. When the smart wheelchair and the toilet are docked, the toilet is embedded in the smart wheelchair, which is opposite to the docking of the smart wheelchair and the bed, so the calculated distance structure needs to take the opposite value.

[0037] There may be a certain angle difference between the wheelchair and the docking target, and the compensation system will calculate the current position of the two in real time and continuously adjust the posture so that the wheelchair and the target remain relatively horizontal and fixed until they are completely inserted into the bed.

[0038] A further technical solution is that in step S3, the postures of the object to be docked and the target object are calculated in real time during the docking process, and a Kalman filter is used to achieve motion compensation and continuously adjust the postures so that the object to be docked and the target object remain relatively horizontal and fixed until the docking is completed. The Kalman filter describes the state evolution of the system based on a state space model. The model includes two equations: the state prediction equation: describes how the system state evolves over time, expressed as:

[0039] x(k)=A*x(k-1)+B*u(K-1)+w(k-1)

[0040] Observation equation: describes how to obtain observations from the system state, usually expressed as:

[0041] z(k)=H*x(k)+v(k)

[0042] The Kalman filter aims to estimate the system state x(k), that is, the state at time k, using the observed value z(k) and the known past state estimate x(k-1) to calculate the state estimate x(k) at time k. By continuously predicting and updating the state estimate, the optimal estimate of the system state is finally obtained.

[0043] Compared with the prior art, the smart wheelchair used in the multifunctional bed-chair integrated device of the present invention is separated from the bed and moves, and then automatically docks with the toilet and the bed. The smart wheelchair realizes its movement function and body position change function, and the change process of the smart wheelchair can achieve smooth and stable switching. At the same time, the smart wheelchair is an indoor assistive system with obstacle avoidance walking function, and can realize automatic docking of the smart wheelchair with the bed and toilet without collision. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0045] Figure 1 A schematic flow chart of a precise collision-free automatic docking control method according to an embodiment of the present invention;

[0046] Figure 2 A schematic diagram of a laser radar posture recognition process involved in one embodiment of the present invention;

[0047] Figure 3 A schematic diagram of a laser radar scanning process during the docking process of an intelligent wheelchair and a bed body involved in an embodiment of the present invention;

[0048] Figure 4 This is a schematic diagram of a laser radar scanning process during the docking process of a smart wheelchair and a toilet involved in one embodiment of the present invention.

[0049] In the figure:

[0050] 1 Smart wheelchair 2 Bed 3 Toilet 4 LiDAR

[0051] 5First mark position 6Second mark position 7Third mark position DETAILED DESCRIPTION

[0052] The present invention is described in detail below in conjunction with the accompanying drawings. The description in this part is only exemplary and explanatory and should not have any limiting effect on the scope of protection of the present invention. In addition, those skilled in the art can make corresponding combinations of the features in the embodiments and in different embodiments in this document based on the description of this document.

[0053] The embodiments of the present invention are as follows, referring to Figure 1 , a precise collision-free automatic docking control method, comprising the following steps:

[0054] S1, remote guidance: automatic docking signal triggering, building a three-dimensional semantic map, completing autonomous positioning and planning the docking path, real-time obstacle avoidance navigation, and executing safe and smooth movement to reach the pre-docking position;

[0055] S2, posture adjustment: Arrive at the pre-docking position, determine the target posture of the target docking object and the own posture of the object to be docked, and adjust its own posture to the pre-docking posture;

[0056] S3, short-range docking: Based on motion compensation, motion errors are detected, and motion correction instructions are generated according to error signal feedback to achieve collision-free and precise docking.

[0057] The intelligent wheelchair 1 used in the multifunctional bed-chair integrated device of the present invention is separated from the bed body 2 and moves, and then automatically docks with the toilet 3 and the bed body 2. The multifunctional bed-chair integrated device consists of two parts: the intelligent wheelchair 1 and the bed body 2. The intelligent wheelchair 1 is used to realize its movement function and body position change function. At the same time, the docking of the bed body 2 and the wheelchair widens the activity space of the user in a lying position. The intelligent wheelchair 1 is an indoor auxiliary system, which can enable the intelligent wheelchair 1 body to have the function of avoiding obstacles and walking, and automatically driving into the bed body 2 or the toilet 3 for docking.

[0058] The control method of the present invention mainly includes a long-range guidance stage, a posture adjustment stage and a short-range docking stage. In the first stage, a three-dimensional semantic map is established through multi-sensor fusion, autonomous positioning is completed based on the map information, the optimal docking path is planned, the intelligent wheelchair 1 navigates in real time to avoid obstacles, and performs safe and smooth movement to reach the pre-docking position. In the second stage, the intelligent wheelchair 1 identifies the docking target and posture based on the three-dimensional semantic map, and the omnidirectional chassis moves through the closed-loop feedback of its own inertial sensor and encoder information, and the intelligent wheelchair 1 is adjusted to the docking posture; in the third stage, the motion compensation method is studied, the motion error of the intelligent wheelchair 1 is detected online, and the motion correction instruction is generated according to the error signal feedback to achieve collision-free and precise docking of the chair-bed / barrel.

[0059] On the basis of the above-mentioned embodiment, in another embodiment of the present invention, in the step S1, a three-dimensional semantic map of each area of ​​the environment is constructed by machine vision scanning, the constructed three-dimensional semantic map is loaded, the collected image recognition scene is used to obtain its own position in the three-dimensional semantic map, the navigation system is used to receive the task target point to be reached, the path planning is performed in the constructed three-dimensional semantic map and the motion control quantity is generated, and real-time dynamic obstacle avoidance is performed based on the artificial potential field method.

[0060] It should be noted that the task target point here is the pre-docking position, such as the set docking position of the bed 2 and the toilet 3.

[0061] Based on the above embodiment, in another embodiment of the present invention, in step S1, the optimal docking path planning is realized based on the improved A-Star algorithm, and the coordinates of the two nodes are set to be p and p respectively. i (x i ,y i ) and p j (xj ,y j ), set the path line equation to y = k p (xx i )+y i , and determine whether the distance between the route and the obstacle is safe, where the slope of the equation of the path line

[0062] In the process of path planning of the smart wheelchair 1, on the one hand, it is necessary to determine the initial position of the smart wheelchair 1 and combine it with the target position for global optimal path planning. On the other hand, it is also necessary to track the smart wheelchair 1 in real time to complete local path planning and obstacle avoidance. The accuracy requirement of the remote guidance stage is not high. It only needs to reach the pre-docking position to ensure the optimal path planning and safe and autonomous obstacle avoidance to a greater extent. The optimal path planning based on the improved A-Star algorithm and the real-time dynamic obstacle avoidance based on the artificial potential field method are proposed.

[0063] The A-Star algorithm is the most classic and commonly used path planning method, which can be regarded as an extension of the Dijkstra algorithm and has been widely used in the fields of manufacturing and intelligent wheelchairs. The present invention proposes an optimal docking path planning based on an improved A-Star algorithm, optimizes its evaluation function, improves the path search efficiency of the algorithm, and adjusts the generated path to avoid oblique penetration of obstacle vertices.

[0064] Based on the above embodiment, in another embodiment of the present invention, in step S1, the step of performing real-time dynamic obstacle avoidance based on the artificial potential field method includes:

[0065] An artificial potential field is established, and the mission target point generates a predetermined attraction to attract the object to be docked to move toward the mission target point. The obstacles generate a repulsive force to prevent the wheelchair from colliding with the obstacles. Under the action of the repulsive forces of multiple obstacles and the predetermined attraction generated by the mission target point, the object to be docked moves along the direction of the resultant force.

[0066] When the smart wheelchair 1 performs local planning, it needs to rely on multiple sensors (camera, lidar 4, IMU, encoder) to fuse the surrounding environment information. According to the changes in the obstacle information detected in real time, the artificial potential field method is used for planning to avoid dynamic obstacles in time to reach the target point. When the smart wheelchair 1 moves from the starting point to the end point, it is affected by the simulated attraction or repulsion, which affects the choice of path. In the artificial potential field, the end point generates an attraction, which attracts the smart wheelchair 1 to move toward the end point, and the obstacles that hinder the passage generate repulsion to prevent the wheelchair from colliding with obstacles. Under the combined effect of multiple repulsion and gravity, the mobile smart wheelchair 1 will move along the direction of the resultant force.

[0067] On the basis of the above-mentioned embodiment, in another embodiment of the present invention, in the step S1, in the process of establishing the artificial potential field, by improving the repulsive field function, when the object to be docked searches for a path near the task target point, the attraction of the task target point to the object to be docked is greater than the repulsion of the task target point to the object to be docked, so as to ensure that the mobile intelligent wheelchair 1 moves in the direction of the task target point. In this way, an improved random walk method is designed to prevent the intelligent wheelchair 1 from falling into the local optimal solution of the map. To prevent the object to be docked from being subjected to excessive repulsion near the task target point, only the repulsive force of the obstacle on it can be used to prevent it from colliding with the obstacle. When the intelligent wheelchair 1 is searching for a path, the gravitational force of the target point on it ensures that the intelligent wheelchair 1 moves in the direction of the end point, while the repulsive force of the obstacle on it prevents the intelligent wheelchair 1 from colliding with the obstacle.

[0068] On the basis of the above embodiments, in another embodiment of the present invention, the object to be docked is provided with an inertial sensor, a laser radar 4, an omnidirectional wheel and an encoder. In the step S2, after reaching the pre-docking position, the target posture is determined by a three-dimensional semantic map, the own posture is determined by an inertial sensor, the orientation and measurement angle of the laser radar 4, and the measured distance between the object to be docked and the target docking object are obtained, the docking angle and direction are adjusted by a 360° rotation of the omnidirectional wheel, and the closed-loop feedback of the inertial sensor and the encoder detection data is performed, and the omnidirectional chassis of the smart wheelchair 1 moves according to the inertial sensor and the encoder detection data to adjust the object to be docked to the docking posture.

[0069] It should be noted that the omnidirectional chassis is used for the bed-chair conversion of the smart wheelchair 1. When the smart wheelchair 1 reaches the pre-docking position, the target posture is determined based on the three-dimensional semantic map, and the own posture is determined by the inertial sensor. At this time, the smart wheelchair 1 adjusts its position and posture to the pre-docking posture. Multi-sensor fusion improves the accuracy and robustness of posture estimation.

[0070] like Figure 2 After the remote guidance stage, the smart wheelchair 1 is located at point A, and the direction of the radar is the direction of the smart wheelchair 1. At this time, the position of the smart wheelchair 1 is A(x t ,y t ,w t ), the radar's measurement angle is θ, the distance to the bed / bucket is d, and the radar's measurement point is B(x, y, β). According to the geometric relationship, we can get:

[0071]

[0072] The laser radar 4 data is input, and the omnidirectional wheels and encoders adjust the wheelchair to the docking position and enter the short-range docking stage.

[0073] Based on the above embodiment, in another embodiment of the present invention, Figure 3, the object to be docked is the smart wheelchair 1, the target docking object is the bed 2, and step S3 includes the following steps:

[0074] S3.1, determine the three marking positions of the target docking object, wherein the first marking position 5 is located below the front side of the bed surface of the bed body 2, the second marking position 6 is located below the rear side of the bed surface of the bed body 2, and the third marking position 7 is located below the bed surface on the side opposite to the opening in the bed body 2. The first marking position 5, the second marking position 6, and the third marking position 7 are respectively parallel to the side of the bed body 2. The laser radar 4 is installed on the seat frame of the wheelchair and is located on one side of the seat frame.

[0075] S3.2, establish a coordinate system, set the position of the laser radar 4 as the origin, the direction of the laser radar 4 pointing to the rear of the wheelchair is the X-axis direction, and the direction pointing to the bed 2 and perpendicular to the X-axis is the Y-coordinate direction. The laser radar 4 obtains the coordinate points of each point of the first mark position 5 in the coordinate system, the coordinate points of each point of the second mark position 6 in the coordinate system, and the coordinate points of each point of the third mark position 7 in the coordinate system, and sends the acquired coordinate information to the controller, which performs motion compensation processing and plans the short-range docking path.

[0076] In this way, the control system can be easily calibrated and accurately docked. After the intelligent wheelchair 1 completes the precise posture adjustment, the laser radar 4 and the three marking positions (such as Figure 3 , which can be three bed boards, namely board 1, board 2 and board 3, are used to realize the docking between the intelligent wheelchair 1 and the bed body 2. At the same time, a Kalman filter is proposed to realize motion compensation.

[0077] like Figure 3 Plate 1 is located under the front side of the bed surface of the bed body 2, plate 2 is located under the rear side of the bed surface of the bed body 2, and plate 3 is located under the side of the bed surface opposite to the opening in the bed body 2. Plate 1, plate 2, and plate 3 are respectively parallel to the sides of the bed body 2; the laser radar 4 is installed on the seat frame of the wheelchair and is located on one side of the seat frame.

[0078] The position of the laser radar 4 is set as the origin O, the direction pointing to the rear of the wheelchair is the X-axis direction, and the direction pointing to the bed 2 and perpendicular to the X-axis is the Y-coordinate direction. The laser radar 4 scans the three plates 1, 2, and 3 in real time to obtain the coordinate points (x1, y1) of each point on plate 1 relative to the coordinate system XY, a series of coordinate points (x2, y2) of each point on the marking plate 2 relative to the coordinate system XY, and a series of coordinate points (x3, y3) of each point on the marking plate 3 relative to the coordinate system XY. These are sent to the controller, which performs Kalman filtering and algorithm processing to automatically plan the docking path, which is convenient for the control system to calibrate and accurately dock.

[0079] Based on the above embodiment, in another embodiment of the present invention, the step S3.2 further includes the following steps:

[0080] The linear equations of the first mark position 5, the second mark position 6 and the third mark position 7 in the coordinate system are fitted respectively, which are y1=a1+b1x1, y2=a2+b2x2 and y3=a3+b3x3, where (x1, y1) are the coordinates of each point of the first mark position 5 relative to the coordinate system, (x2, y2) are the coordinates of each point of the second mark position 6 relative to the coordinate system, (x3, y3) are the coordinates of each point of the third mark position 7 relative to the coordinate system, and a i is the intercept of the straight line on the Y axis, b i is the slope of the line; i is 1, 2 or 3; a i and b i Use the following formula to calculate:

[0081]

[0082] Then, the inclination angles θ of the three straight lines corresponding to the straight line equations of the first mark position 5, the second mark position 6 and the third mark position 7 in the coordinate system after fitting are calculated respectively: i and the distance d from the origin to the three straight lines i , the calculation formula is as follows:

[0083]

[0084] The calculated coordinates of the docking target point P in the coordinate system (x p ,y p ), and the deflection angle α of the second marking position 6 relative to the omnidirectional chassis of the intelligent wheelchair 1. When the second marking position 6 is parallel to the X-axis, α=0; the calculation formula is as follows:

[0085]

[0086] Wherein, d1 is the distance from the coordinate origin of the laser radar 4 to the first marking position 5, d2 is the distance from the coordinate origin of the laser radar 4 to the second marking position 6, d3 is the distance from the coordinate origin of the laser radar 4 to the third marking position 7, θ1, θ2, θ3 are the inclination angles of the straight line equations of the first marking position 5, the second marking position 6 and the third marking position 7 in the coordinate system respectively;

[0087] When the wheelchair is located inside the bed 2 and forms an integrated structure with the bed 2, the coordinates of the docking target point P are (x′ P ,y′ P ), the deflection angle of the docking side armrest relative to the omnidirectional chassis of the intelligent wheelchair 1 is α′, and the angular velocity v of the driving motor of the intelligent wheelchair 1 is calculated. r, linear velocity v l , the calculation formula is as follows:

[0088]

[0089] Based on the above embodiment, in another embodiment of the present invention, Figure 4 , the object to be docked is a wheelchair, and the target docking object is a toilet 3. In the step S3.1, three marking positions of the target docking object are determined, wherein the first marking position 5 is located below the water tank of the toilet 3, the second marking position 6 is located below one side of the toilet 3, and the third marking position 7 is located below the other side of the toilet 3. The first marking position 5, the second marking position 6, and the third marking position 7 are respectively parallel to the lower side of the toilet 3. In the step S3.2, the distances of the three straight lines corresponding to the fitted straight line equations of the first marking position 5, the second marking position 6, and the third marking position 7 in the coordinate system from the origin are taken as opposite values; for the docking process of the intelligent wheelchair 1-toilet 3, the laser radar 4 will lose some line segments that have entered the wheelchair during the scanning process. It is proposed to adopt a method based on bed / bucket template line segment compensation, and complete the unscanned line segments according to the scanned model and the existing template matching in the system library. When the smart wheelchair 1 and toilet 3 are docked, the toilet 3 is embedded in the smart wheelchair 1, which is opposite to the docking of the smart wheelchair 1 and bed 2, so the calculated distance structure needs to take the opposite value.

[0090] There may be a certain angle difference between the wheelchair part and the docking target part, and the compensation system will calculate the current posture of the two in real time, and continuously adjust the posture so that the wheelchair and the target remain relatively horizontal and fixed until they are completely inserted into the bed 2.

[0091] On the basis of the above embodiment, in another embodiment of the present invention, in step S3, the postures of the object to be docked and the target object to be docked are calculated in real time during the docking process, and a Kalman filter is used to implement motion compensation and continuously adjust the postures so that the object to be docked and the target object to be docked remain relatively horizontal and fixed until the docking is completed. The Kalman filter describes the state evolution of the system based on a state space model, which includes two equations: the state prediction equation: describes how the system state evolves over time, expressed as:

[0092] x(k)=A*x(k-1)+B*u(k-1)+w(k-1)

[0093] Observation equation: describes how to obtain observations from the system state, usually expressed as:

[0094] z(k)=H*x(k)+v(k)

[0095] The Kalman filter aims to estimate the system state x(k), that is, the state at time k, using the observed value z(k) and the known past state estimate x(k-1) to calculate the state estimate x(k) at time k. By continuously predicting and updating the state estimate, the optimal estimate of the system state is finally obtained.

[0096] Compared with the prior art, the smart wheelchair 1 used in the multifunctional bed-chair integrated device of the present invention can be separated from the bed body 2 and then automatically docked with the toilet 3 and the bed body 2. The smart wheelchair 1 is used to realize its movement function and body position change function. The transformation process of the smart wheelchair 1 can achieve smooth and stable switching. At the same time, the smart wheelchair 1 is an indoor assistive system with an obstacle avoidance walking function, and can realize automatic docking of the smart wheelchair 1 with the bed body 2 and the toilet 3 without collision.

[0097] For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the scope of protection of the present invention.

Claims

1. A precise collision-free automatic docking control method, characterized in that: The steps include: S1, remote guidance: automatic docking signal triggering, building a three-dimensional semantic map, completing autonomous positioning and planning the docking path, real-time obstacle avoidance navigation, and executing safe and smooth movement to reach the pre-docking position; S2, posture adjustment: Arrive at the pre-docking position, determine the target posture of the target docking object and the own posture of the object to be docked, and adjust its own posture to the pre-docking posture; S3, short-range docking: Based on motion compensation, motion errors are detected, and motion correction instructions are generated according to error signal feedback to achieve collision-free and precise docking.

2. The precise non-collision automatic docking control method according to claim 1 is characterized in that: In step S1, a three-dimensional semantic map of each area of ​​the environment is constructed by machine vision scanning, the constructed three-dimensional semantic map is loaded, the collected image recognition scene is used to obtain the position of the vehicle in the three-dimensional semantic map, the navigation system is used to receive the task target point to be reached, the path is planned in the constructed three-dimensional semantic map and the motion control amount is generated, and real-time dynamic obstacle avoidance is performed based on the artificial potential field method.

3. The precise collision-free automatic docking control method according to claim 2 is characterized in that: In step S1, the optimal docking path planning is realized based on the improved A-Star algorithm. In the improved A-Star algorithm, the coordinates of the two nodes are set to be p and p respectively. i (x i ,y i ) and p j (x j ,y j ), set the path line equation to y = k p (xx i )+y i , and determine whether the distance between the route and the obstacle is safe, where the slope of the equation of the path line 4. The precise collision-free automatic docking control method according to claim 3 is characterized in that: In step S1, the step of performing real-time dynamic obstacle avoidance based on the artificial potential field method includes: An artificial potential field is established, and the mission target point generates a predetermined attraction to attract the object to be docked to move toward the mission target point. The obstacles generate a repulsive force to prevent the wheelchair from colliding with the obstacles. Under the action of the repulsive forces of multiple obstacles and the predetermined attraction generated by the mission target point, the object to be docked moves along the direction of the resultant force.

5. The precise collision-free automatic docking control method according to claim 4 is characterized in that: In the step S1, during the process of establishing the artificial potential field, by improving the repulsive field function, when the object to be docked searches for a path near the task target point, the attraction of the task target point to the object to be docked is greater than the repulsion of the task target point to the object to be docked, thereby ensuring that the mobile intelligent wheelchair moves toward the task target point.

6. The precise collision-free automatic docking control method according to any one of claims 1 to 5, characterized in that: The object to be docked is provided with an inertial sensor, a laser radar, an omnidirectional wheel and an encoder. In the step S2, after reaching the pre-docking position, the target posture is determined by a three-dimensional semantic map, the own posture is determined by an inertial sensor, the orientation and measurement angle of the laser radar, and the measured distance between the object to be docked and the target docking object are obtained, the docking angle and direction are adjusted by a 360° rotation of the omnidirectional wheel, and the closed-loop feedback of the inertial sensor and the encoder detection data is performed, and the omnidirectional chassis of the smart wheelchair moves according to the inertial sensor and the encoder detection data, so as to adjust the object to be docked to the docking posture.

7. The precise collision-free automatic docking control method according to claim 6, characterized in that: The object to be docked is a smart wheelchair, and the target docking object is a bed. Step S3 includes the following steps: S3.1, determine three marking positions of the target docking object, wherein the first marking position is located below the bed surface on the front side of the bed body, the second marking position is located below the bed surface on the rear side of the bed body, and the third marking position is located below the bed surface on the side opposite to the opening in the bed body, and the first marking position, the second marking position, and the third marking position are respectively parallel to the side of the bed body, and the laser radar is installed on the seat frame of the wheelchair and is located on one side of the seat frame, S3.2, establish a coordinate system, set the position of the laser radar as the origin, the direction in which the laser radar points directly behind the wheelchair is the X-axis direction, and the direction in which the laser radar points to the bed and is perpendicular to the X-axis is the Y-coordinate direction. The laser radar is used to obtain the coordinate points of each point at the first mark position in the coordinate system, the coordinate points of each point at the second mark position in the coordinate system, and the coordinate points of each point at the third mark position in the coordinate system. The acquired coordinate information is sent to the controller, and the controller performs motion compensation processing and plans the short-range docking path.

8. The precise collision-free automatic docking control method according to claim 7, characterized in that: The step S3.2 also includes the following steps: The linear equations of the first, second and third marking positions in the coordinate system are fitted, which are y1=a1+b1x1, y2=a2+b2x2 and y3=a3+b3x3, respectively, where (x1, y1) is the coordinate point of each point of the first marking position relative to the coordinate system, (x2, y2) is the coordinate point of each point of the second marking position relative to the coordinate system, (x3, y3) is the coordinate point of each point of the third marking position relative to the coordinate system, and a i is the intercept of the straight line on the Y axis, b i is the slope of the line; i is 1, 2 or 3; The inclination angles θ of the three straight lines corresponding to the straight line equations of the first mark position, the second mark position and the third mark position in the coordinate system after fitting are calculated respectively. i and the distance d from the origin to the three straight lines i , the calculation formula is as follows: The calculated coordinates of the docking target point P in the coordinate system (x p ,y p ), and the deflection angle α of the second mark position relative to the omnidirectional chassis of the intelligent wheelchair. When the second mark position is parallel to the X-axis, α=0. The calculation formula is as follows: Wherein, d1 is the distance from the origin of the laser radar to the first marked position, d2 is the distance from the origin of the laser radar to the second marked position, d3 is the distance from the origin of the laser radar to the third marked position, θ1, θ2, θ3 are the inclination angles of the straight line equations of the first marked position, the second marked position and the third marked position in the coordinate system respectively; When the wheelchair is located inside the bed and forms an integrated structure with the bed, the coordinates of the docking target point P are (x′ P ,y′ P ), the deflection angle of the docking side armrest relative to the omnidirectional chassis of the smart wheelchair is α′, and the angular velocity v of the driving motor of the smart wheelchair is calculated r , linear velocity v l , the calculation formula is as follows:

9. The precise collision-free automatic docking control method according to claim 8, characterized in that: The object to be docked is a smart wheelchair, and the target docking object is a toilet. In the step S3.1, three marking positions of the target docking object are determined, wherein the first marking position is located below the toilet tank, the second marking position is located below one side of the toilet, and the third marking position is located below the other side of the toilet. The first marking position, the second marking position, and the third marking position are respectively parallel to the lower side of the toilet. In the step S3.2, the distances of the three straight lines corresponding to the fitted straight line equations of the first marking position, the second marking position, and the third marking position in the coordinate system from the origin are calculated to be opposite values; in the step 3.2, according to the model scanned by the laser radar and the template matching already in the system library, some line segments that were lost by the laser radar during the scanning process but have entered the smart wheelchair are completed.

10. The precise collision-free automatic docking control method according to claim 9, characterized in that: In step S3, the positions of the object to be docked and the target object are calculated in real time during the docking process, and a Kalman filter is used to achieve motion compensation and continuously adjust the postures so that the object to be docked and the target object remain relatively horizontal and fixed until the docking is completed.

Images