CN103144111B - The mobile mechanical arm sports immunology of the unified coordination of a kind of quadratic programming and planing method - Google Patents

Abstract

The invention provides a kind of QP(quadratic programming) the unified mobile mechanical arm sports immunology coordinated and planing method, comprise the steps: 1) to mobile mechanical arm, set up the unified redundancy coordinated of QP and resolve scheme, the minimum performance index of described parsing conceptual design is the quadratic function of the associating velocity vector of mobile platform and mechanical arm, is tied in the Unify legislation equation of mobile platform and mechanical arm, associating speed limit and associating angle limit; 2) QP solver is used to solve unified for the QP of the step 1) redundancy coordinated scheme of resolving; 3) by step 2) solving result pass to slave computer controller, drive mobile platform and mechanical arm to coordinate given end task simultaneously.Feature of the present invention is, by the Unify legislation of mobile platform and mechanical arm and planning, controls mobile platform and the mechanical arm coordinated movement of various economic factors simultaneously of mobile mechanical arm, completes given end task.

Description

The mobile mechanical arm sports immunology of the unified coordination of a kind of quadratic programming and planing method

Technical field

The present invention relates to motion planning and the control field of mobile mechanical arm, more specifically, relate to mobile mechanical arm sports immunology and the planing method of the unified coordination of a kind of quadratic programming.

Background technology

Mobile mechanical arm is made up of a mobile platform and a fixing mechanical arm on a mobile platform.Compared to the mechanical arm of fixed platform, mobile mechanical arm had both had the mobility on a large scale of mobile platform, and what have again a mechanical arm can operational capacity, therefore receives and pays close attention to widely and study.But the combination of mobile platform and fixing machine mechanical arm creates new problem: the how simultaneously motion of cooperation control mobile platform and mechanical arm? with regard to common planning scheme, or be the motion first controlling mobile platform, the motion of rear controller mechanical arm; Be the motion of the motion of first controller mechanical arm, rear control mobile platform, thus cooperation control cannot be carried out to mobile platform and mechanical arm simultaneously.And mobile mechanical arm sports immunology and the planing method of coordination is unified based on quadratic programming, exactly cooperation control motion is carried out to mobile platform and mechanical arm simultaneously, thus make mobile mechanical arm more effectively complete given end task.

Summary of the invention

Technical problem to be solved by this invention is to provide mobile mechanical arm sports immunology and the planing method that a kind of amount of calculation is little, for mobile mechanical arm, mobile platform and mechanical arm are carried out simultaneously to the unified coordination of quadratic programming of cooperation control motion.

For solving the problems of the technologies described above, the present invention is achieved by the following technical programs:

The mobile mechanical arm sports immunology of the unified coordination of quadratic programming and a planing method, comprise the steps:

S1. to mobile mechanical arm, set up the unified redundancy coordinated of quadratic programming and resolve scheme, the minimum performance index of described parsing conceptual design is the quadratic function of the associating velocity vector of mobile platform and mechanical arm, is tied in the Unify legislation equation of mobile platform and mechanical arm, associating speed limit and associating angle limit;

S2. Quadratic Programming Solution device is used to solve unified for the quadratic programming of the step S1 redundancy coordinated scheme of resolving;

S3. the solving result of step S2 is passed to slave computer controller, drive mobile platform and mechanical arm to coordinate given end task simultaneously.

Preferably, the unified redundancy parsing conceptual design coordinated of quadratic programming is: minimize be tied in $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w,$ q ^-≤q≤qw、 ${\stackrel{\·}{q}}^{-}\≤\stackrel{\·}{q}\≤{\stackrel{\·}{q}}^{+},$ Wherein ${\stackrel{\·}{q}}^{T}W\stackrel{\·}{q}/2+p^T\stackrel{\·}{q}$ For the exercise performance index that need optimize, represent the associating velocity vector of mobile platform and mechanical arm, subscript ^tthe transposition of representing matrix and vector, W is nonzero coefficient matrix, and p is coefficient vector, and W and p is determined by the target for optimizing, equality constraint the Unify legislation equation of corresponding mobile platform and mechanical arm, K represents the connection matrix of robot arm end effector velocity vector and mobile mechanical arm associating velocity vector, represent the velocity vector of mobile mechanical arm end effector, q represents the associating angle vector of mobile platform and mechanical arm, q ^-≤ q≤q ⁺with represent associating angular range and associating velocity interval respectively, q ^±represent associating angle high-low limit, represent associating speed high-low limit.

Preferably, described equality constraint for the unified movement descriptive equation of mobile platform and mechanical arm, according to given mobile mechanical arm, by its structural analysis, try to achieve concrete connection matrix K.

Preferably, the unified redundancy coordinated of described quadratic programming scheme of resolving is converted into a standard quadratic programming: minimize x ^twx/2+p ^tx, is tied in Kx=d, ζ ^-≤ x≤ζ ⁺, wherein positive constant k is used for regulating and ensureing the feasible zone that associating speed is enough large.Solve described standard quadratic programming by Quadratic Programming Solution device, thus obtain the unified mobile mechanical arm sports immunology of coordination of quadratic programming and the optimal solution of planing method.

Compared with prior art, tool of the present invention has the following advantages:

In the past, control, or be the motion first controlling mobile platform to the motion planning of mobile mechanical arm, or be the motion of first controller mechanical arm, can not carry out cooperation control motion to mobile platform and mechanical arm, control procedure is comparatively loaded down with trivial details simultaneously.The present invention is by carrying out Unify legislation to mobile platform and Mechanical transmission test equation, the redundancy setting up the unified mobile mechanical arm coordinated of quadratic programming resolves scheme, thus cooperation control motion is carried out to mobile platform and mechanical arm simultaneously, complete given end task, avoid and control additional workload that mobile platform or manipulator motion bring and complicated processes respectively.

Accompanying drawing explanation

Fig. 1 is flow chart of the present invention.

Fig. 2 is specific embodiment of the invention mobile mechanical arm three-dimensional model diagram.

Fig. 3 is specific embodiment of the invention mobile platform structural representation.

Detailed description of the invention

Below in conjunction with accompanying drawing, the present invention will be further described, but embodiments of the present invention are not limited to this.

The unified redundancy coordinated of quadratic programming that the mobile mechanical arm sports immunology of the unified coordination of the quadratic programming shown in Fig. 1 and planing method first mainly set up mobile mechanical arm resolves scheme; Then Quadratic Programming Solution device is used to solve quadratic programming scheme; Slave computer controller controls the motion of mobile mechanical arm according to solving result.

Specific embodiment one

Fig. 2 illustrates and realizes the mobile mechanical arm that mobile mechanical arm of the present invention is three-dimensional two wheel drive.Mechanical arm is made up of six connecting rods, is connected with joint 10 by joint 5, joint 6, joint 7, joint 8, joint 9.Mobile platform comprises two driving wheels: left driving wheel 11 and right driving wheel 12.In patent example of the present invention, this joint of mechanical arm 5-10 initial angle is set to [0, π/3, π/6, π, pi/2,0] ^tradian, left driving wheel 11 and right driving wheel 12 initial angle are set to [0,0] ^tradian, i.e. q (0)=[0,0,0, π/3, π/6, π, pi/2,0] ^t, its associating angle high-low limit is set to q ^±=[± ∞, ± ∞, ± 0.5236, ± 2.3562, ± 6.2832, ± 6.2832, ± 6.2832, ± 6.2832] ^tradian, its associating speed high-low limit is set to ${\stackrel{\·}{q}}^{\±}={[\±30.0,\±30.0,\±3.0,\±3.0,\±3.0,\±3.0,\±3.0,\±3.0]}^T$ Radian per second.The Forward kinematics equation of this space six degree of freedom redundancy mechanical arm is

$f\left(\mathrm{\θ}\right)=\left[\begin{array}{c}(l_6+l_5)(c_5{s}_{32}{c}_1-s_5{c}_4{c}_{32}{c}_1+s_5{s}_4{s}_1)+(l_4+l_3)s_{32}{c}_1+l_2{s}_2{c}_1\\ (l_6+l_5)(c_5{s}_{32}{s}_1-s_5{c}_4{c}_{32}{s}_1-s_5{s}_4{c}_1)+(l_4+l_3)s_{32}{s}_1+l_2{s}_2{s}_1\\ (l_6+l_5)(s_5{c}_4{s}_{32}+c_5{c}_{32})+(l_4+l_3)c_{32}+l_2{c}_2+l_1+h\end{array}\right],$

Wherein l _irepresent the length of i-th connecting rod, c _i=cos (θ _i), s _i=sin (θ _i), θ _irepresent the angle in i-th joint, i=1,2 ... 6, s ₃₂=sin (θ ₃+ θ ₂), c ₃₂=cos (θ ₃+ θ ₂), h represents the distance bottom first joint to mobile platform.In this space six degree of freedom redundancy mechanical arm, l=[0.065,0.555,0.19,0.13,0.082,0.018] ^trice, h=0.698 rice.

Fig. 3 is for realizing mobile platform structural representation of the present invention.In this particular example, by the structural analysis to mobile platform, the mobile platform kinematical equation of trying to achieve two wheel drive is with wherein ( with represent the rotary speed of mobile platform left and right wheels respectively, its initial velocity is set to zero), represent the speed (initial velocity is set to zero) of mobile platform course angle, represent mobile platform and mechanical arm tie point speed ( with represent its abscissa and the speed of ordinate respectively), $A=\frac{r}{2b}[-\mathrm{1,1}],B=\frac{r}{2}\left[\begin{array}{cc}\cos \mathrm{\φ},& -\sin \mathrm{\φ}\\ \sin \mathrm{\φ},& \cos \mathrm{\φ}\end{array}\right]\left[\begin{array}{cc}1,& 1\\ -d/b,& d/d\end{array}\right]$ (r represents the radius of driving wheel, and it is 0.1025 meter, and d represents the distance of the mid point of two driving wheels to mechanical arm and mobile platform tie point, it is 0.1 meter, b represents the distance driving and take turns to its two mid point, and it is 0.32 meter, and φ represents the course angle of mobile platform).According to the kinematics descriptive equation of mobile platform and mechanical arm, set up mobile platform and the kinematics descriptive equation unified on velocity layer of mechanical arm is $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w,$ Wherein $K=\left[\begin{array}{cc}B,& 0\\ 0,& 0\end{array}\right]+J(\mathrm{\θ},\mathrm{\φ})\left[\begin{array}{cc}A,& 0\\ 0,& I\end{array}\right]J(\mathrm{\θ},\mathrm{\φ})=\frac{\∂g(\mathrm{\θ},\mathrm{\φ})}{\∂w},$ $g(\mathrm{\θ},\mathrm{\φ})=\left[\begin{array}{ccc}\cos \mathrm{\φ},& -\sin \mathrm{\φ},& 0\\ \sin \mathrm{\φ},& \cos \mathrm{\φ},& 0\\ 0,& 0,& 1\end{array}\right]f\left(\mathrm{\θ}\right),w=\left[\begin{array}{c}\mathrm{\φ}\\ \mathrm{\θ}\end{array}\right],$ I is unit matrix.

Here is that the present invention unifies mobile mechanical arm sports immunology and the planing method of coordination about quadratic programming:

Minimize ${\stackrel{\·}{q}}^{T}W\stackrel{\·}{q}/2+p^T\stackrel{\·}{q}$

Constraints $K\stackrel{\·}{q}={\stackrel{\·}{r}}_w---\left(2\right)$

q ^-≤q≤q ⁺(3)

${\stackrel{\·}{q}}^{-}\≤\stackrel{\·}{q}\≤{\stackrel{\·}{q}}^{+}---\left(4\right)$

Wherein for the exercise performance index for optimizing, represent associating velocity vector, W is nonzero coefficient matrix, and p is coefficient vector, and W and p is determined by the target for optimizing, equality constraint the Unify legislation equation of corresponding mobile platform and mechanical arm, K represents the end effector velocity vector of mechanical arm and the connection matrix of mobile mechanical arm associating speed variables, represent mobile mechanical arm end effector velocity vector, q represents the associating angle vector of mobile mechanical arm, q ^-≤ q≤q ⁺with represent associating angular range and associating velocity interval respectively, q ^±represent associating angle bound, represent associating speed bound.

Consider that above-mentioned optimization problem solves on velocity layer, therefore need the joint angle degree constrain (3) of mobile mechanical arm, associating constraint of velocity (4) to merge, thus obtain based on associating speed both-end inequality constraints:

${\mathrm{\ζ}}^{-}\≤\stackrel{\·}{q}\≤{\mathrm{\ζ}}^{+}---\left(5\right)$

Wherein ζ ^-and ζ ⁺i-th element be defined as respectively ${\mathrm{\ζ}}_i^{-}=\max \{k(q_i^{-}-q_i),{\stackrel{\·}{q}}_i^{-}\}$ With ${\mathrm{\ζ}}_i^{+}=\min \{k(q_i^{+}-q_i),{\stackrel{\·}{q}}_i^{+}\}.$ Positive constant k (as being taken as 4) is used for regulating the feasible zone of associating speed.The associating speed of mobile mechanical arm is represented with x mobile mechanical arm redundancy with physical constraint is resolved scheme (1)-(4) and just be can be described as following standard quadratic programming scheme:

Minimize x ^twx/2+p ^tx (6)

Constraints Kx=d (7)

ζ ^-≤x≤ζ ⁺(8)

Wherein with in the present embodiment, W is unit matrix, and p is the combination of null vector and q-q (0), and exercise performance index like this can play the dual purpose of minimum wheel speed and minimum joint motions.Above-mentioned standard quadratic programming Quadratic Programming Solution device solves, and then the mobile mechanical arm obtaining the unified coordination of quadratic programming describes the optimal solution with motion planning method.

Above-described embodiments of the present invention, do not form limiting the scope of the present invention.Any amendment done within spiritual principles of the present invention, equivalent replacement and improvement etc., all should be included within claims of the present invention.

Claims (3)

1. quadratic programming unifies mobile mechanical arm sports immunology and a planing method for coordination, it is characterized in that, comprises the steps:

S1. to mobile mechanical arm, set up the unified redundancy coordinated of quadratic programming and resolve scheme, the minimum performance index of described parsing scheme is the quadratic function of the associating velocity vector of mobile platform and mechanical arm, is tied in the Unify legislation equation of mobile platform and mechanical arm, associating speed limit and associating angle limit;

S2. Quadratic Programming Solution device is used to solve unified for the quadratic programming of the step S1 mobile mechanical arm redundancy coordinated scheme of resolving;

S3. the solving result of step S2 is passed to slave computer controller, drive mobile platform and mechanical arm to coordinate given end task simultaneously;

The unified redundancy coordinated of the quadratic programming scheme of resolving of described step S1 is specially: minimize be tied in q ^-≤ q≤q ⁺, wherein for the exercise performance index for optimizing, represent the associating velocity vector of mobile platform and mechanical arm, subscript ^tthe transposition of representing matrix and vector, W is nonzero coefficient matrix, and p is coefficient vector, and W and p is determined by the target for optimizing, equality constraint the Unify legislation equation of corresponding mobile platform and mechanical arm, K represents the connection matrix of robot arm end effector velocity vector and mobile mechanical arm associating velocity vector, represent the velocity vector of mobile mechanical arm end effector, q represents the associating angle vector of mobile platform and mechanical arm, q ^-≤ q≤q ⁺with represent associating angular range and associating velocity interval respectively, q ^±represent associating angle high-low limit, represent associating speed high-low limit.

2. quadratic programming according to claim 1 unifies mobile mechanical arm sports immunology and the planing method of coordination, it is characterized in that: described equality constraint for the unified movement descriptive equation of mobile platform and mechanical arm, according to given mobile mechanical arm, by its structural analysis, try to achieve concrete connection matrix K.

3. the unified mobile mechanical arm sports immunology coordinated of quadratic programming according to claim 1 and planing method, is characterized in that: the unified redundancy coordinated of the quadratic programming scheme of resolving of described step S1 is converted into a standard quadratic programming: minimize x ^twx/2+p ^tx, is tied in Kx=d, wherein with represent associating speed respectively two end points of both-end inequality constraints, with i-th element be defined as respectively positive constant k is used for regulating and ensureing the feasible zone that associating speed is enough large; Solve described standard quadratic programming by Quadratic Programming Solution device, thus obtain the unified mobile mechanical arm sports immunology of coordination of quadratic programming and the optimal solution of planing method.