CN111844040A - Motion planning method for electric wheelchair with mechanical arm - Google Patents
Motion planning method for electric wheelchair with mechanical arm Download PDFInfo
- Publication number
- CN111844040A CN111844040A CN202010717874.4A CN202010717874A CN111844040A CN 111844040 A CN111844040 A CN 111844040A CN 202010717874 A CN202010717874 A CN 202010717874A CN 111844040 A CN111844040 A CN 111844040A
- Authority
- CN
- China
- Prior art keywords
- electric wheelchair
- mechanical arm
- joint
- quadratic
- planning
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The invention provides a motion planning method of an electric wheelchair with a mechanical arm, which comprises the following steps: 1) obtaining a mathematical model of the electric wheelchair with the mechanical arm according to the structure of the electric wheelchair with the mechanical arm; 2) performing redundancy analysis on the motion plan of the electric wheelchair with the mechanical arm according to the mathematical model in the step 1), and establishing a quadratic optimization scheme; 3) converting the quadratic optimization scheme in the step 2) into quadratic programming; 4) solving the quadratic programming of the step 3) by using a numerical algorithm (namely a quadratic programming solver); 5) and controlling the electric wheelchair with the mechanical arm according to the solving result obtained in the step 4). The invention can simultaneously carry out coordinated motion control on the electric wheelchair and the mechanical arm, so that the electric wheelchair with the mechanical arm can efficiently complete given tasks.
Description
Technical Field
The invention relates to the field of motion planning and control of an electric wheelchair with a mechanical arm, in particular to an inverse kinematics solving method of the electric wheelchair with the mechanical arm.
Background
The electric wheelchair with the mechanical arm consists of the electric wheelchair and the mechanical arm arranged on the electric wheelchair, and the mechanical arm can be arranged at the tail end of the armrest of the electric wheelchair to ensure the safety of a user and enough activity space. The disabled patient can complete actions which cannot be completed due to limitation of physical functions through the electric wheelchair with the mechanical arm, so that the burden of the disabled patient can be relieved to a certain extent, and the life quality of the disabled patient is improved. In recent years, an electric wheelchair with a mechanical arm has entered into the life of people as an important service robot, and the auxiliary mechanical arm can well help the old and the disabled to perform certain daily life activities, such as door opening, water pouring and the like, so that a convenient and comfortable life is provided for the old and the disabled. However, the combination of an electric wheelchair and robotic arms presents greater difficulty in planning the movement of an electric wheelchair with robotic arms. Therefore, the invention provides a motion planning method of the electric wheelchair with the mechanical arm, which realizes coordinated motion control of the electric wheelchair and the mechanical arm at the same time, so that the electric wheelchair with the mechanical arm can efficiently complete given tasks.
Disclosure of Invention
The invention aims to provide a motion planning method of an electric wheelchair with a mechanical arm, which can solve the problem of coordinated motion control of the electric wheelchair and the mechanical arm and enable the electric wheelchair with the mechanical arm to efficiently complete a given task.
In order to solve the technical problems, the invention is realized by the following technical scheme:
1. a motion planning method of an electric wheelchair with a mechanical arm comprises the following steps:
1) obtaining a mathematical model of the electric wheelchair with the mechanical arm according to the structure of the electric wheelchair with the mechanical arm;
2) performing redundancy analysis on the motion plan of the electric wheelchair with the mechanical arm according to the mathematical model in the step 1), and establishing a quadratic optimization scheme;
3) converting the quadratic optimization scheme in the step 2) into quadratic programming;
4) solving the quadratic programming of the step 3) by using a numerical algorithm (namely a quadratic programming solver);
5) and controlling the electric wheelchair with the mechanical arm according to the solving result obtained in the step 4).
In step 1), the mathematical model of the electric wheelchair with the mechanical arm is expressed as Where p represents the position of the end effector of the robotic arm, f (-) represents a non-linear mapping function,shows the joint angle of the electric wheelchair and the mechanical arm,indicating the velocity of the end effector of the robotic arm,a joint jacobian matrix representing the powered wheelchair and robotic arms,representing the combined joint speed of the electric wheelchair and the robotic arm, f (-) andis determined by the mechanical structure of the electric wheelchair with a mechanical arm.
In step 2), the quadratic optimization scheme is represented as: the designed minimized performance index is a quadratic function of the combined velocity vector of the electric wheelchair and the mechanical arm, is restricted by a combined Jacobian equation, a combined joint angle limit and a combined joint angular velocity limit of the electric wheelchair and the mechanical arm, and is the minimized performance indexIs constrained toWhereinFor the performance index of the sport to be optimized,for joint Jacobian equations, superscript T represents the transpose operation of the matrixBy doing so, M denotes a non-zero coefficient matrix, q denotes a non-zero coefficient vector, and M and q are determined by the objective to be optimized,indicating the desired velocity of the end effector of the robotic arm,represents the joint angle limit of the electric wheelchair and the mechanical arm,representing the upper and lower limits of the joint angle,represents the joint angular speed limit of the electric wheelchair and the mechanical arm,representing the upper and lower limits of the joint angular velocity.
In step 3), the quadratic optimization scheme is converted into a quadratic programming, i.e. minimizationIs constrained toWhereinα±The ith elements of the upper and lower bounds of the joint constraint of the electric wheelchair and the mechanical arm are respectively defined asi represents a joint number, a normal number kαFor adjusting and ensuring a sufficiently large feasible range for joint velocity.
And 4) solving the quadratic programming through a quadratic programming solver to obtain an optimal solution of the motion programming of the electric wheelchair with the mechanical arm.
In the step 5), the solution result of the quadratic programming solver is converted into a control signal required by motor driving, so that each motor is driven to enable the electric wheelchair with the mechanical arm to move.
Compared with the prior art, the invention has the following advantages:
in the past, the motion planning control of the electric wheelchair is only a path planning process of the electric wheelchair, the motion planning of the electric wheelchair with the mechanical arm is not considered, and the coordinated motion planning of the electric wheelchair and the mechanical arm cannot be simultaneously performed. According to the invention, unified kinematics modeling and analysis are carried out on the electric wheelchair and the mechanical arm, and a joint kinematics secondary planning scheme of the electric wheelchair with the mechanical arm is established, so that the electric wheelchair and the mechanical arm are coordinately controlled at the same time, and a given task is completed.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a three-dimensional model view of an electric wheelchair with robotic arms embodying the present invention;
FIG. 3 is a schematic top view of an electric wheelchair embodying the present invention;
FIG. 4 is a diagram of the movement process of an electric wheelchair with a mechanical arm for implementing the invention;
fig. 5 is a diagram showing the position error of an end effector during the movement of the electric wheelchair with a mechanical arm.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
The motion planning method of the electric wheelchair with the mechanical arm shown in fig. 1 comprises the following steps:
1) obtaining a mathematical model of the electric wheelchair with the mechanical arm according to the structure of the electric wheelchair with the mechanical arm;
2) performing redundancy analysis on the motion plan of the electric wheelchair with the mechanical arm according to the mathematical model of the electric wheelchair with the mechanical arm in the step 1), and establishing a quadratic optimization scheme;
3) converting the quadratic optimization scheme in the step 2) into quadratic programming;
4) solving the quadratic programming of the step 3) by using a numerical algorithm (namely a quadratic programming solver);
5) and controlling the electric wheelchair with the mechanical arm according to the solving result obtained in the step 4).
Fig. 2 shows a three-dimensional model of an electric wheelchair with a robot arm according to the present invention. The electric wheelchair with the mechanical arm consists of a two-wheel-driven electric wheelchair and the mechanical arm, and the mechanical arm is fixed at the tail end of the armrest of the electric wheelchair. The electric wheelchair includes a right driving wheel 1 and a left driving wheel 2. The mechanical arm consists of six connecting rods and is connected through a joint 3, a joint 4, a joint 5, a joint 6, a joint 7 and a joint 8.
Fig. 3 shows a schematic top view of an electric wheelchair for implementing the present invention. In this particular example, the electric wheelchair has a top view of a 2b square with the drive wheel at the midpoint of the side, the support wheel at the rear of the wheelchair base, and the point of attachment P for the electric wheelchair to the robotic armdAt the top left front of the square. Through the structural analysis of the electric wheelchair, the kinematic description equation of the electric wheelchair is obtained as
WhereinIndicating the course angular velocity of the electric wheelchair rotation,represents the moving speed of the connecting point of the electric wheelchair and the mechanical arm, r represents the radius of the driving wheel, theta represents the course angle of the rotation of the electric wheelchair,andwhich respectively indicate the rotational speed of the left driving wheel 2 and the right driving wheel 1. Further, a mathematical model of the electric wheelchair with the mechanical arm is obtained through analysisWhere p represents the position of the end effector of the robotic arm, f (-) represents a non-linear mapping function,shows the joint angle of the electric wheelchair and the mechanical arm,indicating the velocity of the end effector of the robotic arm,represents the joint speed of the electric wheelchair and the mechanical arm,is a combined Jacobian matrix of the electric wheelchair and the mechanical arm, and the matrixIs a jacobian matrix of mechanical armsVector quantityThe matrix I is an identity matrix.
The quadratic form optimization scheme designed by the invention is
whereinFor the performance index of the sport to be optimized,for the combined Jacobian equation of the electric wheelchair and the mechanical arm, the superscript T represents the transposition operation of a matrix, M represents a non-zero coefficient matrix, q represents a non-zero coefficient vector, and M and q are determined by the target to be optimized,indicating the desired velocity of the end effector of the robotic arm,represents the joint angle limit of the electric wheelchair and the mechanical arm,representing the upper and lower limits of the joint angle,represents the joint angular speed limit of the electric wheelchair and the mechanical arm,representing the upper and lower limits of the joint angular velocity. The quadratic optimization schemes (1) - (4) of the electric wheelchair with mechanical arm with physical constraint can be described as the following quadratic programming schemes:
whereinα±The ith elements of the upper and lower bounds of the joint constraint of the electric wheelchair and the mechanical arm are respectively defined as i represents a joint number, a normal number kαTo adjust and ensure a sufficiently large feasible domain for joint constraint. An example of a possible quadratic programming solver is given below (the invention is not limited to this solver example), and the above problem can be equated again to a piecewise linear projection equation system by using the gradient descent method and the velocity compensation method:
wherein gamma is more than 0, the feedback coefficient of the position error of the end effector of the mechanical arm is represented, p represents the actual track of the end effector of the mechanical arm, w represents an auxiliary variable of a projection equation set,represents the velocity compensation, v > 0 represents the proportionality coefficient for controlling the convergence velocity,is a projection function expressed as
After the solution of the quadratic programming is obtained through the quadratic programming solver, the result is converted into a control signal required by motor driving, and each joint motor of the electric wheelchair with the mechanical arm is driven to move through a controller of the electric wheelchair with the mechanical arm, so that each joint motor is driven to enable the electric wheelchair and the mechanical arm to move in a coordinated manner.
The workflow of the present invention will now be described with reference to a specific example operation.
MATLAB software is used for carrying out motion trail tracking experiment simulation on the electric wheelchair with the mechanical arm. The specific modeling parameters are as follows: and r is 0.2463M, b is 0.4912M, and the performance index to be optimized is set as the minimum speed norm, namely, the matrix M is set as a unit matrix, and the vector q is a zero vector. The specific control parameters are set as follows: the upper and lower limits of the combined angle of the electric wheelchair with the mechanical arm areRadian, upper and lower limits of combined angular velocity of electric wheelchair with mechanical armRadian/second, computer simulation time is set to 10 seconds, execution task is a circular track tracking task, and initial angle of the electric wheelchair with the mechanical arm is set toRadian, γ ═ 105,v=10-3,kα20. Solving is carried out through a quadratic programming solver, and a solving result is transmitted to the controller, so that the coordinated movement of the electric wheelchair and the mechanical arm is controlled simultaneously.
FIG. 4 is a diagram showing the movement of an electric wheelchair with a robot arm to which the present invention is applied, in which a connection point P is recorded in an x-y planedIs movedThe broken line represents the motion state of the mechanical arm at different moments. It can be seen from figure 4 that the given circular trajectory tracking task is performed in coordination by the electric wheelchair and the robotic arm.
Fig. 5 is a diagram showing the position error of an end effector during the movement of the electric wheelchair with a mechanical arm. It can be seen from fig. 5 that the position error of the end effector during the entire task execution is less than 10-5Rice, embodying the precise nature of the invention.
Claims (6)
1. A motion planning method of an electric wheelchair with a mechanical arm is characterized by comprising the following steps:
1) obtaining a mathematical model of the electric wheelchair with the mechanical arm according to the structure of the electric wheelchair with the mechanical arm;
2) performing redundancy analysis on the motion plan of the electric wheelchair with the mechanical arm according to the mathematical model in the step 1), and establishing a quadratic optimization scheme;
3) converting the quadratic optimization scheme in the step 2) into quadratic programming;
4) solving the quadratic programming of the step 3) by using a numerical algorithm (namely a quadratic programming solver);
5) controlling the electric wheelchair with the mechanical arm according to the solving result obtained in the step 4).
2. The method of claim 1, wherein the mathematical model of the powered wheelchair is represented as p ═ f (θ),wherein p represents the position of the end effector of the mechanical arm, f (-) represents a nonlinear mapping function, theta represents the joint angle of the electric wheelchair and the mechanical arm,indicating the velocity of the end effector of the robotic arm,a joint jacobian matrix representing the powered wheelchair and robotic arms,representing the joint speed of the electric wheelchair and the mechanical arm.
3. The method for planning the movement of an electric wheelchair with a mechanical arm as claimed in claim 1, wherein the quadratic optimization scheme of step 2) can be expressed as: the designed minimized performance index is a quadratic function of the combined velocity vector of the electric wheelchair and the mechanical arm, is restricted by a combined Jacobian equation, a combined joint angle limit and a combined joint angular velocity limit of the electric wheelchair and the mechanical arm, and is the minimized performance indexIs constrained to WhereinFor the performance index of the sport to be optimized,for joint Jacobian equations, the superscript T represents the transpose operation of the matrix, M represents the non-zero coefficient matrix, q represents the non-zero coefficient vector, and M and q are determined by the objective to be optimized,representing a desired velocity, θ, of the end effector of the robotic arm-≤θ≤θ+Electric wheelchair with indicationAnd joint angle limit, theta, of the arm±Representing the upper and lower limits of the joint angle,represents the joint angular speed limit of the electric wheelchair and the mechanical arm,representing the upper and lower limits of the joint angular velocity.
4. The method for planning the movement of an electric wheelchair with a mechanical arm as claimed in claim 1, wherein the quadratic optimization scheme of step 2) can be converted into quadratic planning, and the performance index is designed to be minimizedIs constrained toWhereinα±Upper and lower bounds, alpha, for joint constraint of the electric wheelchair and the robotic arm±Are respectively defined as i represents a joint number, a normal number kαFor adjusting and ensuring a sufficiently large feasible range for joint velocity.
5. The method for planning the movement of the electric wheelchair with a mechanical arm according to claim 1, wherein the method for planning the movement is solved by the quadratic programming solver in the step 4), so as to obtain an optimal solution for the movement planning of the electric wheelchair with a mechanical arm.
6. The method for planning the movement of an electric wheelchair with a mechanical arm according to any one of claims 1 to 5, wherein the solution result of the quadratic programming solver in the step 4) can be converted into a control signal required by motor driving, so that each motor is driven to make the electric wheelchair and the mechanical arm move in coordination at the same time.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010717874.4A CN111844040A (en) | 2020-07-23 | 2020-07-23 | Motion planning method for electric wheelchair with mechanical arm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010717874.4A CN111844040A (en) | 2020-07-23 | 2020-07-23 | Motion planning method for electric wheelchair with mechanical arm |
Publications (1)
Publication Number | Publication Date |
---|---|
CN111844040A true CN111844040A (en) | 2020-10-30 |
Family
ID=72950822
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010717874.4A Withdrawn CN111844040A (en) | 2020-07-23 | 2020-07-23 | Motion planning method for electric wheelchair with mechanical arm |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111844040A (en) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103144111A (en) * | 2013-02-26 | 2013-06-12 | 中山大学 | QP unified and coordinated motion describing and programming method for movable manipulator |
CN105563490A (en) * | 2016-03-03 | 2016-05-11 | 吉首大学 | Fault tolerant motion planning method for obstacle avoidance of mobile manipulator |
CN108326844A (en) * | 2017-01-20 | 2018-07-27 | 香港理工大学深圳研究院 | The motion planning method and device of the operable degree optimization of redundancy mechanical arm |
CN108908347A (en) * | 2018-09-07 | 2018-11-30 | 浙江科技学院 | One kind is towards redundancy mobile mechanical arm error-tolerance type repetitive motion planning method |
CN108972548A (en) * | 2018-06-29 | 2018-12-11 | 华南理工大学 | A kind of mobile platform-mechanical arm system modeling method |
-
2020
- 2020-07-23 CN CN202010717874.4A patent/CN111844040A/en not_active Withdrawn
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103144111A (en) * | 2013-02-26 | 2013-06-12 | 中山大学 | QP unified and coordinated motion describing and programming method for movable manipulator |
CN105563490A (en) * | 2016-03-03 | 2016-05-11 | 吉首大学 | Fault tolerant motion planning method for obstacle avoidance of mobile manipulator |
CN108326844A (en) * | 2017-01-20 | 2018-07-27 | 香港理工大学深圳研究院 | The motion planning method and device of the operable degree optimization of redundancy mechanical arm |
CN108972548A (en) * | 2018-06-29 | 2018-12-11 | 华南理工大学 | A kind of mobile platform-mechanical arm system modeling method |
CN108908347A (en) * | 2018-09-07 | 2018-11-30 | 浙江科技学院 | One kind is towards redundancy mobile mechanical arm error-tolerance type repetitive motion planning method |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN101804627B (en) | Redundant manipulator motion planning method | |
CN106426164B (en) | A kind of multi objective coordinated movement of various economic factors planing method of redundancy double mechanical arms | |
CN108381553B (en) | Relative navigation close-range tracking method and system for space non-cooperative target capture | |
CN109848983A (en) | A kind of method of highly conforming properties people guided robot work compound | |
US8322468B2 (en) | Robot apparatus and method of controlling the same, and computer program | |
CN104760041B (en) | A kind of Obstacle avoidance motion planning method based on impact degree | |
CN102514008B (en) | Method for optimizing performance indexes of different layers of redundancy mechanical arm simultaneously | |
JPH09109072A (en) | Control method for redundant manipulator | |
CN108508906B (en) | Biped robot bilateral touch teleoperation system and control method in outdoor environment | |
CN110561425B (en) | Rope-driven flexible robot force and position hybrid control method and system | |
CN106003034A (en) | Master-slave robot control system and control method | |
CN113601512A (en) | Universal avoidance method and system for singular points of mechanical arm | |
CN108422421A (en) | The muscle of skeletal muscle formula robot controls and assembly method | |
CN109129487B (en) | Redundant manipulator repetitive motion planning method based on Taylor type discrete periodic rhythm neural network under periodic noise | |
CN113568422B (en) | Four-foot robot control method based on model predictive control optimization reinforcement learning | |
CN111844040A (en) | Motion planning method for electric wheelchair with mechanical arm | |
CN109875659B (en) | Flexible needle plane puncture control device and method based on brain emotion learning intelligent control algorithm | |
CN110434854B (en) | Redundant manipulator visual servo control method and device based on data driving | |
CN115755592B (en) | Multi-mode control method for adjusting motion state of three-degree-of-freedom exoskeleton and exoskeleton | |
CN113867157B (en) | Optimal trajectory planning method and device for control compensation and storage device | |
Xie et al. | A Biobjective Scheme for Kinematic Control of Mobile Robotic Arms With Manipulability Optimization | |
JP2629291B2 (en) | Manipulator learning control method | |
CN114102636B (en) | Welding seam polishing control system of teleoperation robot and design method and application thereof | |
Ouyang et al. | Motion control of a snake robot via cerebellum-inspired learning control | |
Nait-Chabane et al. | Mobile arm for disabled people assistance Manipulability measure for redundancy solve |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
WW01 | Invention patent application withdrawn after publication | ||
WW01 | Invention patent application withdrawn after publication |
Application publication date: 20201030 |