Three-dimensional scanning posture sensing system and method applied to intelligent wheelchair

Abstract

The three-dimensional scanning posture sensing system and method applied to smart wheelchairs belong to the field of smart wheelchair control technology. In order to achieve precise control of smart wheelchairs in dynamic scenes, the present invention collects three-dimensional pedestrian posture photos during the automatic driving of the wheelchair, and constructs a three-dimensional scanning posture sensing model data set for the smart wheelchair; extracts the key points of the target pedestrian from the collected three-dimensional pedestrian posture photos, and then maps them from the image coordinates to the camera coordinates; uses Kalman filtering to predict and correct the three-dimensional coordinates of the target pedestrian in the camera coordinate system; performs three-dimensional scanning posture sensing on the target pedestrian based on the assumption of the continuity of pedestrian spatial movement, and obtains the three-dimensional scanning posture sensing result of the target pedestrian; constructs a prediction model for mobile interaction of the smart wheelchair, inputs the three-dimensional scanning posture sensing result based on the target pedestrian into the constructed prediction model for mobile interaction of the smart wheelchair, and outputs the predicted interaction action between the wheelchair and the target pedestrian.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent wheelchair control, and in particular relates to a three-dimensional scanning posture sensing system and method applied to an intelligent wheelchair. Background Art

[0002] Wheelchairs are important auxiliary tools to help the elderly and disabled people recover. With the rapid development of sensor technology, artificial intelligence technology, and industrial manufacturing technology, smart wheelchairs have gradually entered the field of vision of ordinary families. Compared with traditional wheelchairs, smart wheelchairs have the characteristics of diversified functions, time-saving and labor-saving, easy to learn and operate, and personalization. In addition to basic motion control, they also have added intelligent functions such as positioning navigation, voice interaction, obstacle avoidance, multi-sensor measurement, and multi-structure transformation. Smart wheelchairs can be divided into outdoor smart wheelchairs and indoor smart wheelchairs according to the different daily life occasions of the elderly and disabled. The indoor environment is a static environment, and the technology of outdoor smart wheelchairs in relevant static environments is already very mature, but the outdoor environment is a dynamic environment, a complex and changeable environment, especially in dynamic scenes. Smart wheelchairs react slowly and fail to avoid, which makes the control of outdoor smart wheelchairs more difficult. Summary of the invention

[0003] Problem to be solved by the present invention In order to achieve precise control of an intelligent wheelchair in a dynamic scene, a three-dimensional scanning posture sensing system and method for an intelligent wheelchair are proposed.

[0004] To achieve the above object, the present invention is implemented through the following technical solutions:

[0005] A three-dimensional scanning posture sensing method for an intelligent wheelchair comprises the following steps:

[0006] S1. The binocular camera collects 3D pedestrian posture photos in real time during the automatic driving of the wheelchair, and constructs a 3D scanning posture sensing model dataset for the smart wheelchair;

[0007] S2. extracting the key points of the target pedestrian from the three-dimensional pedestrian posture photo collected in step S1, and then mapping them from the image coordinates to the camera coordinates;

[0008] S3. Use Kalman filtering to predict and correct the three-dimensional coordinates of the target pedestrian in the camera coordinate system obtained in step S2;

[0009] S4. Performing three-dimensional scanning posture sensing on the target pedestrian based on the assumption of continuity of pedestrian spatial movement, and obtaining a three-dimensional scanning posture sensing result of the target pedestrian;

[0010] S5. Construct a prediction model for mobile interaction of the smart wheelchair, input the three-dimensional scanning posture sensing result of the target pedestrian obtained in step S4 into the constructed prediction model for mobile interaction of the smart wheelchair, and output the predicted interaction action between the wheelchair and the target pedestrian.

[0011] Furthermore, the specific implementation method of step S2 is to estimate the human body posture of the three-dimensional pedestrian posture photos collected in step S1, obtain the pixel coordinates of the key points of the target pedestrian, use the key point pixel coordinates to index in the point cloud image, map the image coordinates to the camera coordinates, and obtain the three-dimensional camera coordinates of the key points of the target pedestrian.

[0012] Furthermore, the specific implementation method of step S3 includes the following steps:

[0013] S3.1. Set the prediction equation of the target pedestrian in the three-dimensional coordinates of the camera coordinate system as follows:

[0014] Y t =AY t-1 +Bu t-1

[0015] Among them, Y' t is the optimal prediction value at time t, Y t-1 is the optimal prediction value at time t-1, A is the state transfer matrix, B is the matrix that converts the control vector into the state, u t-1 is the control vector at time t-1;

[0016] S3.2. Error P of the optimal prediction value based on time t-1 t-1 Calculate the error P of the optimal prediction value at time t t , the expression is:

[0017] P t =AP t-1 A T +Q

[0018] Where Q is the process excitation noise covariance;

[0019] S3.3. Error P of the optimal prediction value based on time t t The error R of the measured value at time t is used to calculate the Kalman gain K at time t. t , the expression is:

[0020] K t =P t H T (HP t H T +R) -1

[0021] Where H is the observation transformation matrix;

[0022] S3.4. Calculate the final output result Y of Kalman filter t ', the expression is:

[0023] Y t '=Y t +K t (Z t -HY t )

[0024] Among them, Z t is the input of Kalman filter;

[0025] Calculate the error P of the final output result of the Kalman filter t ', the expression is:

[0026] P t '=(IK t H)P t

[0027] Where I is the identity matrix.

[0028] Furthermore, in step S4, the 3D camera coordinates of the i-th target pedestrian at time t-1 are set to Set the 3D camera coordinates of the i-th target pedestrian at time t to Then the expression of the pedestrian spatial movement continuity assumption is:

[0029]

[0030] Among them, argmin is the function of the parameter value that achieves the minimum value in its definition domain, and U is the threshold of the pedestrian spatial movement continuity assumption.

[0031] Furthermore, the specific implementation method of step S5 for constructing a prediction model for mobile interaction of the smart wheelchair includes the following steps:

[0032] S5.1. Using the A-star algorithm as the path planning method for mobile interaction of the smart wheelchair, a cost function is established to realize the path planning for mobile interaction of the smart wheelchair, and the expression is:

[0033] f(n)=g(n)+h(n)

[0034] Where f(n) is the estimated cost from the initial path via path node n to the target state, g(n) is the actual cost from the initial path to path node n in the path space, and h(n) is the estimated cost of the best path from path node n to the target path;

[0035] S5.2. Based on the path nodes obtained in step S5.1 and the three-dimensional scanning posture sensing results of the target pedestrian obtained in step S4, the DWA algorithm is used to perform path avoidance for mobile interaction of the smart wheelchair.

[0036] A three-dimensional scanning posture sensing system applied to an intelligent wheelchair comprises a processor, a memory and a computer program stored in the memory and executable on the processor. When the computer program is executed, the steps of a three-dimensional scanning posture sensing method applied to an intelligent wheelchair as described in the present invention are implemented.

[0037] Beneficial effects of the present invention:

[0038] The three-dimensional scanning posture sensing method applied to an intelligent wheelchair described in the present invention collects three-dimensional pedestrian posture photos in the process of automatic driving of the wheelchair in real time based on a binocular camera, then extracts the key points of the target pedestrian, and then maps the image coordinates to the camera coordinates, and uses Kalman filtering to predict and correct the obtained three-dimensional coordinates of the target pedestrian in the camera coordinate system, and then constructs a pedestrian spatial movement continuity hypothesis to perform three-dimensional scanning posture sensing on the target pedestrian. This method can realize real-time and precise control of the intelligent wheelchair in dynamic scenes, and realize accurate positioning of the three-dimensional scanning posture in outdoor dynamic scenes.

[0039] The three-dimensional scanning posture sensing method applied to a smart wheelchair described in the present invention inputs the three-dimensional scanning posture in a real-time positioned outdoor dynamic scene into a prediction model for mobile interaction of the smart wheelchair, so as to realize the safe and stable movement of the smart wheelchair towards the target pedestrian in indoor and outdoor environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 This is a flow chart of a three-dimensional scanning posture sensing method applied to an intelligent wheelchair according to the present invention. DETAILED DESCRIPTION

[0041] In order to make the purpose, technical solution and advantages of the present invention more clear, the present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention, that is, the specific embodiments described are only part of the embodiments of the present invention, rather than all of the specific embodiments. The components of the specific embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations, and the present invention can also have other embodiments.

[0042] Therefore, the following detailed description of the specific embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents the selected specific embodiments of the present invention. Based on the specific embodiments of the present invention, all other specific embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present invention.

[0043] In order to further understand the content, features and effects of the present invention, the following specific implementation methods are given as examples, and the attached Figure 1 The detailed instructions are as follows: Specific implementation method one:

[0045] A three-dimensional scanning posture sensing method for an intelligent wheelchair comprises the following steps:

[0046] S1. The binocular camera collects 3D pedestrian posture photos in real time during the automatic driving of the wheelchair, and constructs a 3D scanning posture sensing model dataset for the smart wheelchair;

[0047] S2. extracting the key points of the target pedestrian from the three-dimensional pedestrian posture photo collected in step S1, and then mapping them from the image coordinates to the camera coordinates;

[0048] Furthermore, the specific implementation method of step S2 is to estimate the human body posture of the three-dimensional pedestrian posture photo collected in step S1, obtain the pixel coordinates of the key points of the target pedestrian, use the pixel coordinates of the key points to index in the point cloud image, map the image coordinates to the camera coordinates, and obtain the three-dimensional camera coordinates of the key points of the target pedestrian;

[0049] S2.1. Using the left-eye image frame as the main clue in the time dimension to estimate the 3D pedestrian posture, search for the key points of the target pedestrian in the right-eye image space dimension frame corresponding to the left-eye image frame. If the key points of the target pedestrian in the right-eye image frame are found, the 3D pedestrian posture of the current frame is estimated by constructing a least squares problem. If the key points of the target pedestrian in the right-eye image frame cannot be found, proceed to the next step.

[0050] Set the 3D point clouds corresponding to the two consecutive frames of the left image frame with the corresponding key points of the target pedestrian in the time dimension as follows: the first frame point cloud is c = {c1, c2, ... c n}, the second frame point cloud is c′={c1′,c′2,...c′ n};

[0051] Set the first frame to remove the centroid point cloud The second frame de-centred point cloud is Among them, the average value of the first frame point cloud is The average value of the second frame point cloud is By constructing a least squares problem to solve (R, t), where R is the rotation matrix and t is the translation vector, the calculation expression is:

[0052]

[0053] S2.2 When the key point of the target pedestrian in the right eye image frame cannot be searched in the right eye image space dimension frame, triangulation processing is performed through multiple frame perspectives, and the three-dimensional pedestrian posture of the current frame is solved by using the PnP method;

[0054] set up and are the reference points and control points of the key points of the target pedestrian in the camera coordinate system, then the reference points and control points of the key points of the target pedestrian in the camera coordinate system are associated by weighted sum, and the calculation expression is:

[0055]

[0056] Among them, ε ij is the correlation coefficient between the reference point of the key point of the i-th target pedestrian and the j-th control point, m is the total number of control points of the target pedestrian;

[0057] Reference points of the key points of the target pedestrian in the camera coordinate system The corresponding pixel The projection equation is described as follows:

[0058]

[0059] Among them, w i is the scale factor, K is the camera internal parameter, f u is the focal coordinate after scaling on the u axis, f v is the focal coordinate after scaling on the v axis; then the least squares and SVD methods are used to solve the equation to obtain the coordinate values ​​of the control points of the key points of the target pedestrian in the camera coordinate system, combined with the known coordinate values ​​of the control points of the key points of the target pedestrian in the world coordinate system, and then the ICP algorithm is used to find the transformation relationship (R, t) between the control points of the key points of the target pedestrian between the two coordinate systems.

[0060] S3. Use Kalman filtering to predict and correct the three-dimensional coordinates of the target pedestrian in the camera coordinate system obtained in step S2;

[0061] Furthermore, the specific implementation method of step S3 includes the following steps:

[0062] S3.1. Set the prediction equation of the target pedestrian in the three-dimensional coordinates of the camera coordinate system as follows:

[0063] Yt =AY t-1 +Bu t-1

[0064] Among them, Y' t is the optimal prediction value at time t, Y t-1 is the optimal prediction value at time t-1, A is the state transfer matrix, B is the matrix that converts the control vector into the state, u t-1 is the control vector at time t-1;

[0065] S3.2. Error P of the optimal prediction value based on time t-1 t-1 Calculate the error P of the optimal prediction value at time t t , the expression is:

[0066] P t =AP t-1 A T +Q

[0067] Where Q is the process excitation noise covariance;

[0068] S3.3. Error P of the optimal prediction value based on time t t The error R of the measured value at time t is used to calculate the Kalman gain K at time t. t , the expression is:

[0069] K t =P t H T (HP t H T +R) -1

[0070] Where H is the observation transformation matrix;

[0071] S3.4. Calculate the final output result Y of Kalman filter t ', the expression is:

[0072] Y t '=Y t +K t (Z t -HY t )

[0073] Among them, Z t is the input of Kalman filter;

[0074] Calculate the error P of the final output result of the Kalman filter t ', the expression is:

[0075] P t '=(IK t H)P t

[0076] Where I is the identity matrix.

[0077] S4. Performing three-dimensional scanning posture sensing on the target pedestrian based on the assumption of continuity of pedestrian spatial movement, and obtaining a three-dimensional scanning posture sensing result of the target pedestrian;

[0078] Furthermore, in step S4, the 3D camera coordinates of the i-th target pedestrian at time t-1 are set to Set the 3D camera coordinates of the i-th target pedestrian at time t to Then the expression of the pedestrian spatial movement continuity assumption is:

[0079]

[0080] Among them, argmin is the function of the parameter value that achieves the minimum value in its definition domain, and U is the threshold of the pedestrian spatial movement continuity assumption.

[0081] S5. Construct a prediction model for mobile interaction of the smart wheelchair, input the three-dimensional scanning posture sensing result of the target pedestrian obtained in step S4 into the constructed prediction model for mobile interaction of the smart wheelchair, and output the predicted interaction action between the wheelchair and the target pedestrian;

[0082] Furthermore, the specific implementation method of step S5 for constructing a prediction model for mobile interaction of the smart wheelchair includes the following steps:

[0083] S5.1. Using the A-star algorithm as the path planning method for mobile interaction of the smart wheelchair, a cost function is established to realize the path planning for mobile interaction of the smart wheelchair, and the expression is:

[0084] f(n)=g(n)+h(n)

[0085] Where f(n) is the estimated cost from the initial path via path node n to the target state, g(n) is the actual cost from the initial path to path node n in the path space, and h(n) is the estimated cost of the best path from path node n to the target path;

[0086] S5.2. Based on the path nodes obtained in step S5.1 and the three-dimensional scanning posture sensing results of the target pedestrian obtained in step S4, the DWA algorithm is used to perform path avoidance for mobile interaction of the smart wheelchair.

[0087] Furthermore, the steps of the DWA algorithm include the following:

[0088] S5.2.1. Speed ​​sampling: Considering the hardware constraints of the smart wheelchair, such as the maximum speed, minimum speed, maximum linear acceleration and maximum angular acceleration, the speed and angular velocity are sampled;

[0089] S5.2.2. Trajectory prediction: simulate the movement trajectory of the smart wheelchair within a certain period of time according to the sampling speed;

[0090] S5.2.3. Trajectory evaluation: the simulated trajectory is evaluated by an evaluation function, which usually takes into account the heading angle, the distance to the obstacle, and the speed of the smart wheelchair;

[0091] S5.2.4. Select the optimal trajectory. Select the trajectory with the highest evaluation as the movement instruction of the smart wheelchair. Specific implementation method 2:

[0093] A three-dimensional scanning posture sensing system applied to an intelligent wheelchair comprises a processor, a memory and a computer program stored in the memory and executable on the processor. When the computer program is executed, the steps of a three-dimensional scanning posture sensing method applied to an intelligent wheelchair as described in the first specific implementation manner are implemented.

[0094] It should be noted that relational terms such as "first" and "second" are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0095] Although the present application has been described above with reference to specific embodiments, various modifications may be made thereto and parts thereof may be replaced with equivalents without departing from the scope of the present application. In particular, as long as there is no structural conflict, the various features in the specific embodiments disclosed in the present application may be used in combination with each other in any manner, and the fact that these combinations are not exhaustively described in this specification is only for the sake of omitting space and saving resources. Therefore, the present application is not limited to the specific embodiments disclosed herein, but includes all technical solutions falling within the scope of the claims.

Claims

1. A three-dimensional scanning posture sensing method for an intelligent wheelchair, characterized in that: The steps include: S1. The binocular camera collects 3D pedestrian posture photos in real time during the automatic driving of the wheelchair, and constructs a 3D scanning posture sensing model dataset for the smart wheelchair; S2. extracting the key points of the target pedestrian from the three-dimensional pedestrian posture photo collected in step S1, and then mapping them from the image coordinates to the camera coordinates; S2.

1. Using the left-eye image frame as the main clue in the time dimension to estimate the 3D pedestrian posture, search for the key points of the target pedestrian in the right-eye image space dimension frame corresponding to the left-eye image frame. If the key points of the target pedestrian in the right-eye image frame are found, the 3D pedestrian posture of the current frame is estimated by constructing a least squares problem. If the key points of the target pedestrian in the right-eye image frame cannot be found, proceed to the next step. Set the 3D point clouds corresponding to the two consecutive frames of the left image frame with the corresponding key points of the target pedestrian in the time dimension as follows: the first frame point cloud is c = {c1, c2, ... c n }, the second frame point cloud is c′={c1′,c′2,...c′ n }; Set the first frame to remove the centroid point cloud The second frame de-centred point cloud is Among them, the average value of the first frame point cloud is The average value of the second frame point cloud is By constructing a least squares problem to solve (R, t), where R is the rotation matrix and t is the translation vector, the calculation expression is: S2.2 When the key point of the target pedestrian in the right eye image frame cannot be searched in the right eye image space dimension frame, triangulation processing is performed through multiple frame perspectives, and the three-dimensional pedestrian posture of the current frame is solved by using the PnP method; set up and are the reference points and control points of the key points of the target pedestrian in the camera coordinate system, then the reference points and control points of the key points of the target pedestrian in the camera coordinate system are associated by weighted sum, and the calculation expression is: Among them, ε ij is the correlation coefficient between the reference point of the key point of the i-th target pedestrian and the j-th control point, m is the total number of control points of the target pedestrian; Reference points of the key points of the target pedestrian in the camera coordinate system The corresponding pixel The projection equation is described as follows: Among them, w i is the scale factor, K is the camera internal parameter, f u is the focal coordinate after scaling on the u axis, f v is the focal coordinate after scaling on the v axis; then, the least squares and SVD methods are used to solve the equation to obtain the coordinate values ​​of the control points of the key points of the target pedestrian in the camera coordinate system, combined with the known coordinate values ​​of the control points of the key points of the target pedestrian in the world coordinate system, and then the ICP algorithm is used to find the transformation relationship (R, t) between the control points of the key points of the target pedestrian in the two coordinate systems; S3. Use Kalman filtering to predict and correct the three-dimensional coordinates of the target pedestrian in the camera coordinate system obtained in step S2; S4. Performing three-dimensional scanning posture sensing on the target pedestrian based on the assumption of continuity of pedestrian spatial movement, and obtaining a three-dimensional scanning posture sensing result of the target pedestrian; In step S4, the 3D camera coordinates of the i-th target pedestrian at time t-1 are set to Set the 3D camera coordinates of the i-th target pedestrian at time t to Then the expression of the pedestrian spatial movement continuity assumption is: Among them, argmin is the function of the parameter value that achieves the minimum value in its definition domain, and U is the threshold of the pedestrian spatial movement continuity assumption; S5. Construct a prediction model for mobile interaction of the smart wheelchair, input the three-dimensional scanning posture sensing result of the target pedestrian obtained in step S4 into the constructed prediction model for mobile interaction of the smart wheelchair, and output the predicted interaction action between the wheelchair and the target pedestrian; The specific implementation method of step S5 for constructing a prediction model for mobile interaction of the smart wheelchair includes the following steps: S5.

1. Using the A-star algorithm as the path planning method for mobile interaction of the smart wheelchair, a cost function is established to realize the path planning for mobile interaction of the smart wheelchair, and the expression is: f(n)=g(n)+h(n) Where f(n) is the estimated cost from the initial path via path node n to the target state, g(n) is the actual cost from the initial path to path node n in the path space, and h(n) is the estimated cost of the best path from path node n to the target path; S5.

2. Based on the path nodes obtained in step S5.1 and the three-dimensional scanning posture sensing results of the target pedestrian obtained in step S4, the DWA algorithm is used to avoid the path for mobile interaction of the smart wheelchair; The steps of the DWA algorithm include the following: S5.2.

1. Speed ​​sampling: Considering the hardware constraints of the smart wheelchair, such as the maximum speed, minimum speed, maximum linear acceleration and maximum angular acceleration, the speed and angular velocity are sampled; S5.2.

2. Trajectory prediction: simulate the movement trajectory of the smart wheelchair within a certain period of time according to the sampling speed; S5.2.

3. Trajectory evaluation: the simulated trajectory is evaluated by an evaluation function, which usually takes into account the heading angle, the distance to the obstacle, and the speed of the smart wheelchair; S5.2.

4. Select the optimal trajectory and select the trajectory with the highest evaluation as the movement instruction of the smart wheelchair.

2. A three-dimensional scanning posture sensing method for an intelligent wheelchair according to claim 1, characterized in that: The specific implementation method of step S2 is to estimate the human body posture of the three-dimensional pedestrian posture photos collected in step S1, obtain the pixel coordinates of the key points of the target pedestrian, use the key point pixel coordinates to index in the point cloud image, map the image coordinates to the camera coordinates, and obtain the three-dimensional camera coordinates of the key points of the target pedestrian.

3. A three-dimensional scanning posture sensing method for an intelligent wheelchair according to claim 2, characterized in that: The specific implementation method of step S3 includes the following steps: S3.

1. Set the prediction equation of the target pedestrian in the three-dimensional coordinates of the camera coordinate system as follows: Y t =YES t-1 +Bu t-1 Among them, Y' t is the optimal prediction value at time t, Y t-1 is the optimal prediction value at time t-1, A is the state transfer matrix, B is the matrix that converts the control vector into the state, u t-1 is the control vector at time t-1; S3.

2. Error P of the optimal prediction value based on time t-1 t-1 Calculate the error P of the optimal prediction value at time t t , the expression is: P t =AP t-1 From T +Q Where Q is the process excitation noise covariance; S3.

3. Error P of the optimal prediction value based on time t t The error R of the measured value at time t is used to calculate the Kalman gain K at time t. t , the expression is: K t =P t H T (HP t H T +R) -1 Where H is the observation transformation matrix; S3.

4. Calculate the final output result Y of Kalman filter t ', the expression is: Y t '=Y t +K t (Z t -HY t ) Among them, Z t is the input of Kalman filter; Calculate the error P of the final output result of the Kalman filter t ', the expression is: P t ’=(I-K t H)P t Where I is the identity matrix.

4. A three-dimensional scanning posture sensing system applied to an intelligent wheelchair, characterized in that: The invention comprises a processor, a memory and a computer program stored in the memory and executable on the processor, wherein when the computer program is executed, the steps of a three-dimensional scanning posture sensing method applied to an intelligent wheelchair as claimed in any one of claims 1 to 3 are implemented.

Images